From 62750bb9733f86a1a4d7220ee4765a61184adb4d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Tue, 26 Nov 2024 13:56:18 +0100
Subject: [PATCH] [RIGv2] Introduce initial Orion servos and new conrig buttons
---
src/RIGv2/Actuators/Actuators.cpp | 89 ++++++++++++++-----
src/RIGv2/Actuators/Actuators.h | 2 +-
src/RIGv2/CanHandler/CanHandler.h | 2 +-
src/RIGv2/Configs/ActuatorsConfig.h | 2 +-
src/RIGv2/Configs/RadioConfig.h | 2 +-
src/RIGv2/Radio/Radio.cpp | 110 +++++++++++++++++-------
src/RIGv2/Radio/Radio.h | 2 +-
src/RIGv2/StateMachines/TARS1/TARS1.cpp | 10 +--
8 files changed, 156 insertions(+), 63 deletions(-)
diff --git a/src/RIGv2/Actuators/Actuators.cpp b/src/RIGv2/Actuators/Actuators.cpp
index beedaff56..cb671723e 100644
--- a/src/RIGv2/Actuators/Actuators.cpp
+++ b/src/RIGv2/Actuators/Actuators.cpp
@@ -132,7 +132,8 @@ Actuators::Actuators()
Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
Config::Servos::FREQUENCY);
// This servo is currently unusable, due to it sharing the same timer as
- // miosix, TIM5 infos[4].servo = std::make_unique<Servo>(
+ // miosix, TIM5
+ // infos[4].servo = std::make_unique<Servo>(
// MIOSIX_SERVOS_5_TIM, TimerUtils::Channel::MIOSIX_SERVOS_5_CHANNEL,
// Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
// Config::Servos::FREQUENCY);
@@ -142,10 +143,11 @@ Actuators::Actuators()
Config::Servos::FREQUENCY);
infos[6].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_7_TIM, TimerUtils::Channel::MIOSIX_SERVOS_7_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::SERVO2_MIN_PULSE, Config::Servos::SERVO2_MAX_PULSE,
Config::Servos::FREQUENCY);
// This servo is currently unusable, due to it sharing the same timer as
- // servo 1 infos[7].servo = std::make_unique<Servo>(
+ // servo 1
+ // infos[7].servo = std::make_unique<Servo>(
// MIOSIX_SERVOS_8_TIM, TimerUtils::Channel::MIOSIX_SERVOS_8_CHANNEL,
// Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
// Config::Servos::FREQUENCY);
@@ -159,7 +161,7 @@ Actuators::Actuators()
Config::Servos::FREQUENCY);
ServoInfo* info;
- info = getServo(ServosList::FILLING_VALVE);
+ info = getServo(ServosList::N2O_FILLING_VALVE);
info->defaultMaxAperture = Config::Servos::DEFAULT_FILLING_MAX_APERTURE;
info->defaultOpeningTime = Config::Servos::DEFAULT_FILLING_OPENING_TIME;
info->limit = Config::Servos::FILLING_LIMIT;
@@ -170,7 +172,7 @@ Actuators::Actuators()
info->maxApertureKey = CONFIG_ID_FILLING_MAX_APERTURE;
info->unsafeSetServoPosition(0.0f);
- info = getServo(ServosList::RELEASE_VALVE);
+ info = getServo(ServosList::N2O_RELEASE_VALVE);
info->defaultMaxAperture = Config::Servos::DEFAULT_RELEASE_MAX_APERTURE;
info->defaultOpeningTime = Config::Servos::DEFAULT_RELEASE_OPENING_TIME;
info->limit = Config::Servos::RELEASE_LIMIT;
@@ -181,7 +183,40 @@ Actuators::Actuators()
info->maxApertureKey = CONFIG_ID_RELEASE_MAX_APERTURE;
info->unsafeSetServoPosition(0.0f);
- info = getServo(ServosList::DISCONNECT_SERVO);
+ info = getServo(ServosList::N2O_VENTING_VALVE);
+ info->defaultMaxAperture = Config::Servos::DEFAULT_VENTING_MAX_APERTURE;
+ info->defaultOpeningTime = Config::Servos::DEFAULT_VENTING_OPENING_TIME;
+ info->limit = Config::Servos::VENTING_LIMIT;
+ info->flipped = Config::Servos::VENTING_FLIPPED;
+ info->openingEvent = Common::Events::MOTOR_OPEN_VENTING_VALVE;
+ info->closingEvent = Common::Events::MOTOR_CLOSE_VENTING_VALVE;
+ info->openingTimeKey = CONFIG_ID_VENTING_OPENING_TIME;
+ info->maxApertureKey = CONFIG_ID_VENTING_MAX_APERTURE;
+ info->unsafeSetServoPosition(0.0f);
+
+ info = getServo(ServosList::N2_FILLING_VALVE);
+ info->defaultMaxAperture = Config::Servos::DEFAULT_FILLING_MAX_APERTURE;
+ info->defaultOpeningTime = Config::Servos::DEFAULT_FILLING_OPENING_TIME;
+ info->limit = Config::Servos::FILLING_LIMIT;
+ info->flipped = Config::Servos::FILLING_FLIPPED;
+ info->openingEvent = Common::Events::MOTOR_OPEN_FILLING_VALVE;
+ info->closingEvent = Common::Events::MOTOR_CLOSE_FILLING_VALVE;
+ info->openingTimeKey = CONFIG_ID_FILLING_OPENING_TIME;
+ info->maxApertureKey = CONFIG_ID_FILLING_MAX_APERTURE;
+ info->unsafeSetServoPosition(0.0f);
+
+ info = getServo(ServosList::N2_RELEASE_VALVE);
+ info->defaultMaxAperture = Config::Servos::DEFAULT_RELEASE_MAX_APERTURE;
+ info->defaultOpeningTime = Config::Servos::DEFAULT_RELEASE_OPENING_TIME;
+ info->limit = Config::Servos::RELEASE_LIMIT;
+ info->flipped = Config::Servos::RELEASE_FLIPPED;
+ info->openingEvent = Common::Events::MOTOR_OPEN_RELEASE_VALVE;
+ info->closingEvent = Common::Events::MOTOR_CLOSE_RELEASE_VALVE;
+ info->openingTimeKey = CONFIG_ID_RELEASE_OPENING_TIME;
+ info->maxApertureKey = CONFIG_ID_RELEASE_MAX_APERTURE;
+ info->unsafeSetServoPosition(0.0f);
+
+ info = getServo(ServosList::N2_DETACH_SERVO);
info->defaultMaxAperture = Config::Servos::DEFAULT_DISCONNECT_MAX_APERTURE;
info->defaultOpeningTime = Config::Servos::DEFAULT_DISCONNECT_OPENING_TIME;
info->limit = Config::Servos::DISCONNECT_LIMIT;
@@ -202,15 +237,15 @@ Actuators::Actuators()
info->maxApertureKey = CONFIG_ID_MAIN_MAX_APERTURE;
info->unsafeSetServoPosition(0.0f);
- info = getServo(ServosList::VENTING_VALVE);
- info->defaultMaxAperture = Config::Servos::DEFAULT_VENTING_MAX_APERTURE;
- info->defaultOpeningTime = Config::Servos::DEFAULT_VENTING_OPENING_TIME;
- info->limit = Config::Servos::VENTING_LIMIT;
- info->flipped = Config::Servos::VENTING_FLIPPED;
- info->openingEvent = Common::Events::MOTOR_OPEN_VENTING_VALVE;
- info->closingEvent = Common::Events::MOTOR_CLOSE_VENTING_VALVE;
- info->openingTimeKey = CONFIG_ID_VENTING_OPENING_TIME;
- info->maxApertureKey = CONFIG_ID_VENTING_MAX_APERTURE;
+ info = getServo(ServosList::NITROGEN_VALVE);
+ info->defaultMaxAperture = Config::Servos::DEFAULT_MAIN_MAX_APERTURE;
+ info->defaultOpeningTime = Config::Servos::DEFAULT_MAIN_OPENING_TIME;
+ info->limit = Config::Servos::MAIN_LIMIT;
+ info->flipped = Config::Servos::MAIN_FLIPPED;
+ info->openingEvent = 0;
+ info->closingEvent = 0;
+ info->openingTimeKey = CONFIG_ID_MAIN_OPENING_TIME;
+ info->maxApertureKey = CONFIG_ID_MAIN_MAX_APERTURE;
info->unsafeSetServoPosition(0.0f);
}
@@ -328,7 +363,8 @@ void Actuators::closeAllServos()
infos[idx].closeServo();
getModule<CanHandler>()->sendServoCloseCommand(ServosList::MAIN_VALVE);
- getModule<CanHandler>()->sendServoCloseCommand(ServosList::VENTING_VALVE);
+ getModule<CanHandler>()->sendServoCloseCommand(
+ ServosList::N2O_VENTING_VALVE);
}
bool Actuators::setMaxAperture(ServosList servo, float aperture)
@@ -366,7 +402,7 @@ bool Actuators::isCanServoOpen(ServosList servo)
Lock<FastMutex> lock(infosMutex);
if (servo == ServosList::MAIN_VALVE)
return canMainOpen;
- else if (servo == ServosList::VENTING_VALVE)
+ else if (servo == ServosList::N2O_VENTING_VALVE)
return canVentingOpen;
else
return false;
@@ -423,7 +459,7 @@ void Actuators::setCanServoOpen(ServosList servo, bool open)
Lock<FastMutex> lock(infosMutex);
if (servo == ServosList::MAIN_VALVE)
canMainOpen = open;
- else if (servo == ServosList::VENTING_VALVE)
+ else if (servo == ServosList::N2O_VENTING_VALVE)
canVentingOpen = open;
}
@@ -436,18 +472,25 @@ void Actuators::inject(DependencyInjector& injector)
Actuators::ServoInfo* Actuators::getServo(ServosList servo)
{
+ // info[4] and info[7] are currently unavailable
switch (servo)
{
- case FILLING_VALVE:
+ case N2O_FILLING_VALVE:
return &infos[0];
- case RELEASE_VALVE:
+ case N2O_RELEASE_VALVE:
return &infos[1];
- case DISCONNECT_SERVO:
+ case N2O_VENTING_VALVE:
return &infos[2];
- case MAIN_VALVE:
+ case N2_FILLING_VALVE:
return &infos[3];
- case VENTING_VALVE:
+ case N2_RELEASE_VALVE:
+ return &infos[5];
+ case N2_DETACH_SERVO:
return &infos[6];
+ case MAIN_VALVE:
+ return &infos[8];
+ case NITROGEN_VALVE:
+ return &infos[9];
default:
// Oh FUCK
diff --git a/src/RIGv2/Actuators/Actuators.h b/src/RIGv2/Actuators/Actuators.h
index 58342d074..41cf009ff 100644
--- a/src/RIGv2/Actuators/Actuators.h
+++ b/src/RIGv2/Actuators/Actuators.h
@@ -26,7 +26,7 @@
#include <RIGv2/CanHandler/CanHandler.h>
#include <RIGv2/Registry/Registry.h>
#include <actuators/Servo/Servo.h>
-#include <common/MavlinkLyra.h>
+#include <common/MavlinkOrion.h>
#include <miosix.h>
#include <scheduler/TaskScheduler.h>
diff --git a/src/RIGv2/CanHandler/CanHandler.h b/src/RIGv2/CanHandler/CanHandler.h
index 282d8eef9..0b8a305e5 100644
--- a/src/RIGv2/CanHandler/CanHandler.h
+++ b/src/RIGv2/CanHandler/CanHandler.h
@@ -26,7 +26,7 @@
#include <RIGv2/Configs/CanHandlerConfig.h>
#include <RIGv2/Sensors/Sensors.h>
#include <common/CanConfig.h>
-#include <common/MavlinkLyra.h>
+#include <common/MavlinkOrion.h>
#include <drivers/canbus/CanProtocol/CanProtocol.h>
#include <utils/DependencyManager/DependencyManager.h>
diff --git a/src/RIGv2/Configs/ActuatorsConfig.h b/src/RIGv2/Configs/ActuatorsConfig.h
index 7d7fee299..bfa3ddf43 100644
--- a/src/RIGv2/Configs/ActuatorsConfig.h
+++ b/src/RIGv2/Configs/ActuatorsConfig.h
@@ -72,7 +72,7 @@ constexpr bool FILLING_FLIPPED = true;
constexpr bool VENTING_FLIPPED = true;
constexpr bool MAIN_FLIPPED = true;
constexpr bool RELEASE_FLIPPED = true;
-constexpr bool DISCONNECT_FLIPPED = true;
+constexpr bool DISCONNECT_FLIPPED = false;
} // namespace Servos
diff --git a/src/RIGv2/Configs/RadioConfig.h b/src/RIGv2/Configs/RadioConfig.h
index 98830ac09..af1b8643e 100644
--- a/src/RIGv2/Configs/RadioConfig.h
+++ b/src/RIGv2/Configs/RadioConfig.h
@@ -22,7 +22,7 @@
#pragma once
-#include <common/MavlinkLyra.h>
+#include <common/MavlinkOrion.h>
namespace RIGv2
{
diff --git a/src/RIGv2/Radio/Radio.cpp b/src/RIGv2/Radio/Radio.cpp
index 5e7bdc47a..f657b77f3 100644
--- a/src/RIGv2/Radio/Radio.cpp
+++ b/src/RIGv2/Radio/Radio.cpp
@@ -515,10 +515,18 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.timestamp = TimestampTimer::getTimestamp();
// Sensors
- tm.loadcell_rocket = sensors->getTankWeightLastSample().load;
- tm.loadcell_vessel = sensors->getVesselWeightLastSample().load;
- tm.filling_pressure = sensors->getFillingPressLastSample().pressure;
- tm.vessel_pressure = sensors->getVesselPressLastSample().pressure;
+ tm.rocket_mass = sensors->getTankWeightLastSample().load;
+ tm.n2o_vessel_mass = sensors->getVesselWeightLastSample().load;
+
+ tm.n2o_filling_pressure =
+ sensors->getFillingPressLastSample().pressure;
+ tm.n2o_vessel_pressure =
+ sensors->getVesselPressLastSample().pressure;
+
+ tm.n2_filling_pressure = -1.f; // TODO
+ tm.n2_vessel_1_pressure = -1.f; // TODO
+ tm.n2_vessel_2_pressure = -1.f; // TODO
+
tm.battery_voltage = sensors->getBatteryVoltageLastSample().voltage;
tm.current_consumption =
sensors->getServoCurrentLastSample().current;
@@ -540,15 +548,28 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
sdLogger.log(cpuStats);
// Valve states
- tm.filling_valve_state =
- actuators->isServoOpen(ServosList::FILLING_VALVE) ? 1 : 0;
- tm.venting_valve_state =
- actuators->isServoOpen(ServosList::VENTING_VALVE) ? 1 : 0;
- tm.release_valve_state =
- actuators->isServoOpen(ServosList::RELEASE_VALVE) ? 1 : 0;
+ tm.n2o_filling_valve_state =
+ actuators->isServoOpen(ServosList::N2O_FILLING_VALVE);
+ tm.n2o_release_valve_state =
+ actuators->isServoOpen(ServosList::N2O_RELEASE_VALVE);
+ tm.n2o_detach_state = 0; // TODO
+ tm.n2o_venting_valve_state =
+ actuators->isServoOpen(ServosList::N2O_VENTING_VALVE);
+
+ tm.n2_filling_valve_state =
+ actuators->isServoOpen(ServosList::N2_FILLING_VALVE);
+ tm.n2_release_valve_state =
+ actuators->isServoOpen(ServosList::N2_RELEASE_VALVE);
+ tm.n2_detach_state =
+ actuators->isServoOpen(ServosList::N2_DETACH_SERVO);
+ tm.n2_quenching_valve_state = 0; // TODO
+ tm.n2_3way_valve_state = 0; // TODO
+
tm.main_valve_state =
- actuators->isServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
- tm.nitrogen_valve_state = actuators->isNitrogenOpen() ? 1 : 0;
+ actuators->isServoOpen(ServosList::MAIN_VALVE);
+ tm.nitrogen_valve_state =
+ actuators->isServoOpen(ServosList::NITROGEN_VALVE);
+ tm.ignition_state = actuators->isNitrogenOpen();
// Internal states
tm.gmm_state = getModule<GroundModeManager>()->getState();
@@ -600,7 +621,8 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.main_valve_state =
actuators->isCanServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
tm.venting_valve_state =
- actuators->isCanServoOpen(ServosList::VENTING_VALVE) ? 1 : 0;
+ actuators->isCanServoOpen(ServosList::N2O_VENTING_VALVE) ? 1
+ : 0;
// Can data
CanHandler::CanStatus canStatus =
@@ -857,53 +879,81 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
lastManualActuation = currentTime;
}
- if (oldConrigState.filling_valve_btn == 0 &&
- state.filling_valve_btn == 1)
+ if (oldConrigState.n2o_filling_btn == 0 && state.n2o_filling_btn == 1)
{
- // The filling switch was pressed
+ // The N2O filling switch was pressed
EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
- getModule<Actuators>()->toggleServo(ServosList::FILLING_VALVE);
+ getModule<Actuators>()->toggleServo(ServosList::N2O_FILLING_VALVE);
lastManualActuation = currentTime;
}
- if (oldConrigState.quick_connector_btn == 0 &&
- state.quick_connector_btn == 1)
+ if (oldConrigState.n2o_release_btn == 0 && state.n2o_release_btn == 1)
{
- // The quick conector switch was pressed
+ // The N2O release switch was pressed
EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
- getModule<Actuators>()->toggleServo(ServosList::DISCONNECT_SERVO);
+ getModule<Actuators>()->toggleServo(ServosList::N2O_RELEASE_VALVE);
lastManualActuation = currentTime;
}
- if (oldConrigState.release_pressure_btn == 0 &&
- state.release_pressure_btn == 1)
+ // TODO: n2o_detach_btn
+
+ if (oldConrigState.n2o_venting_btn == 0 && state.n2o_venting_btn == 1)
{
- // The release switch was pressed
+ // The N2O venting switch was pressed
EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
- getModule<Actuators>()->toggleServo(ServosList::RELEASE_VALVE);
+ getModule<Actuators>()->toggleServo(ServosList::N2O_VENTING_VALVE);
lastManualActuation = currentTime;
}
- if (oldConrigState.venting_valve_btn == 0 &&
- state.venting_valve_btn == 1)
+ if (oldConrigState.n2_filling_btn == 0 && state.n2_filling_btn == 1)
{
- // The venting switch was pressed
+ // The N2 filling switch was pressed
EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
- getModule<Actuators>()->toggleServo(ServosList::VENTING_VALVE);
+ getModule<Actuators>()->toggleServo(ServosList::N2_FILLING_VALVE);
lastManualActuation = currentTime;
}
- if (oldConrigState.start_tars_btn == 0 && state.start_tars_btn == 1)
+ if (oldConrigState.n2_release_btn == 0 && state.n2_release_btn == 1)
+ {
+ // The N2 release switch was pressed
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
+ getModule<Actuators>()->toggleServo(ServosList::N2_RELEASE_VALVE);
+
+ lastManualActuation = currentTime;
+ }
+
+ if (oldConrigState.n2_detach_btn == 0 && state.n2_detach_btn == 1)
+ {
+ // The N2 detach switch was pressed
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
+ getModule<Actuators>()->toggleServo(ServosList::N2_DETACH_SERVO);
+
+ lastManualActuation = currentTime;
+ }
+
+ // TODO: n2_quenching_btn
+ // TODO: n2_3way_btn
+
+ if (oldConrigState.tars_btn == 0 && state.tars_btn == 1)
{
// The TARS switch was pressed
EventBroker::getInstance().post(MOTOR_START_TARS, TOPIC_TARS);
lastManualActuation = currentTime;
}
+
+ if (oldConrigState.nitrogen_btn == 0 && state.nitrogen_btn == 1)
+ {
+ // The nitrogen switch was pressed
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
+ getModule<Actuators>()->toggleServo(ServosList::NITROGEN_VALVE);
+
+ lastManualActuation = currentTime;
+ }
}
// Special case for disarming, that can be done bypassing the timeout
diff --git a/src/RIGv2/Radio/Radio.h b/src/RIGv2/Radio/Radio.h
index 26a32da17..582ee6d97 100644
--- a/src/RIGv2/Radio/Radio.h
+++ b/src/RIGv2/Radio/Radio.h
@@ -31,7 +31,7 @@
#include <RIGv2/Sensors/Sensors.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
#include <RIGv2/StateMachines/TARS1/TARS1.h>
-#include <common/MavlinkLyra.h>
+#include <common/MavlinkOrion.h>
#include <interfaces-impl/hwmapping.h>
#include <radio/MavlinkDriver/MavlinkDriver.h>
#include <radio/SX1278/SX1278Lora.h>
diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
index 5b0f879d6..50a1f90d7 100644
--- a/src/RIGv2/StateMachines/TARS1/TARS1.cpp
+++ b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
@@ -104,13 +104,13 @@ void TARS1::state_refueling(const Event& event)
logAction(TarsActionType::WASHING);
// Start washing
- actuators->openServoWithTime(ServosList::VENTING_VALVE,
+ actuators->openServoWithTime(ServosList::N2O_VENTING_VALVE,
Config::TARS1::WASHING_OPENING_TIME);
// Wait a bit so that the servo don't actuate at the same time
Thread::sleep(Config::TARS1::WASHING_TIME_DELAY);
- actuators->openServoWithTime(ServosList::FILLING_VALVE,
+ actuators->openServoWithTime(ServosList::N2O_FILLING_VALVE,
Config::TARS1::WASHING_OPENING_TIME);
// After double the time we opened the valve, move to the next phase
@@ -127,7 +127,7 @@ void TARS1::state_refueling(const Event& event)
logAction(TarsActionType::OPEN_FILLING);
// Open the filling for a long time
- actuators->openServoWithTime(ServosList::FILLING_VALVE,
+ actuators->openServoWithTime(ServosList::N2O_FILLING_VALVE,
Config::TARS1::FILLING_OPENING_TIME);
nextDelayedEventId = EventBroker::getInstance().postDelayed(
@@ -176,11 +176,11 @@ void TARS1::state_refueling(const Event& event)
logAction(TarsActionType::OPEN_VENTING);
// Open the venting and check for pressure stabilization
- actuators->openServo(ServosList::VENTING_VALVE);
+ actuators->openServo(ServosList::N2O_VENTING_VALVE);
// Calculate next check time based on the time the valve stays open
unsigned int nextCheckTime =
- actuators->getServoOpeningTime(ServosList::VENTING_VALVE) +
+ actuators->getServoOpeningTime(ServosList::N2O_VENTING_VALVE) +
Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME * 5;
nextDelayedEventId = EventBroker::getInstance().postDelayed(
--
GitLab