diff --git a/CMakeLists.txt b/CMakeLists.txt
index d44bbc9def4d34462530edbaf8486b6c115a3ff3..a1510b8c65a58309f0cf4c4f80d968cb325dea86 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -72,6 +72,10 @@ add_executable(rig-v2-entry src/RIGv2/rig-v2-entry.cpp ${RIG_V2_COMPUTER})
target_include_directories(rig-v2-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(rig-v2-entry stm32f767zi_rig_v2)
+add_executable(rig-v2-adc-test src/RIGv2/rig-v2-adc-test.cpp ${RIG_V2_COMPUTER})
+target_include_directories(rig-v2-adc-test PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(rig-v2-adc-test stm32f767zi_rig_v2)
+
add_executable(con_rig-entry src/ConRIG/con_rig-entry.cpp ${CON_RIG_COMPUTER})
target_include_directories(con_rig-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(con_rig-entry stm32f429zi_con_rig)
diff --git a/scripts/logdecoder/General/logdecoder.cpp b/scripts/logdecoder/General/logdecoder.cpp
index c2639f17f9dc77a5cce605effb5cb22d7846d97d..d6226ce7dfcf4e7269812bc242826f169749e16f 100644
--- a/scripts/logdecoder/General/logdecoder.cpp
+++ b/scripts/logdecoder/General/logdecoder.cpp
@@ -109,14 +109,15 @@ void registerTypes(Deserializer& ds)
ds.registerType<RIGv2::ADC2Data>();
ds.registerType<RIGv2::TC1Data>();
ds.registerType<RIGv2::OxVesselWeightData>();
+ ds.registerType<RIGv2::RocketWeightData>();
ds.registerType<RIGv2::OxTankWeightData>();
ds.registerType<RIGv2::OxVesselPressureData>();
ds.registerType<RIGv2::OxFillingPressureData>();
ds.registerType<RIGv2::N2Vessel1PressureData>();
ds.registerType<RIGv2::N2Vessel2PressureData>();
ds.registerType<RIGv2::N2FillingPressureData>();
- ds.registerType<RIGv2::OxTankTopPressureData>();
- ds.registerType<RIGv2::OxTankBottomPressureData>();
+ ds.registerType<RIGv2::OxTankPressureData>();
+ ds.registerType<RIGv2::N2TankPressureData>();
ds.registerType<RIGv2::ActuatorsData>();
ds.registerType<RIGv2::GroundModeManagerData>();
ds.registerType<RIGv2::TarsActionData>();
diff --git a/scripts/logdecoder/RIGv2/logdecoder.cpp b/scripts/logdecoder/RIGv2/logdecoder.cpp
index 666ae2f71b2185ac68989df5efbef11f8dccdc98..6062e9ff7a2a7f5c962c439b4cd167fc406ea0e3 100644
--- a/scripts/logdecoder/RIGv2/logdecoder.cpp
+++ b/scripts/logdecoder/RIGv2/logdecoder.cpp
@@ -57,14 +57,15 @@ void registerTypes(Deserializer& ds)
ds.registerType<ADC2Data>();
ds.registerType<TC1Data>();
ds.registerType<OxVesselWeightData>();
+ ds.registerType<RocketWeightData>();
ds.registerType<OxTankWeightData>();
ds.registerType<OxVesselPressureData>();
ds.registerType<OxFillingPressureData>();
ds.registerType<N2Vessel1PressureData>();
ds.registerType<N2Vessel2PressureData>();
ds.registerType<N2FillingPressureData>();
- ds.registerType<OxTankTopPressureData>();
- ds.registerType<OxTankBottomPressureData>();
+ ds.registerType<OxTankPressureData>();
+ ds.registerType<N2TankPressureData>();
ds.registerType<ActuatorsData>();
ds.registerType<GroundModeManagerData>();
ds.registerType<TarsActionData>();
diff --git a/src/ConRIG/Buttons/Buttons.cpp b/src/ConRIG/Buttons/Buttons.cpp
index 8390a9494aaeeccff1c67dcba0a204da15672c8c..c4b5a98d179e46698949a65174c12c55c5e4c504 100644
--- a/src/ConRIG/Buttons/Buttons.cpp
+++ b/src/ConRIG/Buttons/Buttons.cpp
@@ -32,11 +32,10 @@ using namespace miosix;
using namespace Boardcore;
using namespace ConRIG;
-Buttons::Buttons() : state() {}
-
bool Buttons::start()
{
- TaskScheduler& scheduler = getModule<BoardScheduler>()->getRadioScheduler();
+ TaskScheduler& scheduler =
+ getModule<BoardScheduler>()->getButtonsScheduler();
return scheduler.addTask([this]() { periodicStatusCheck(); },
Config::Buttons::BUTTON_SAMPLE_PERIOD) != 0;
@@ -44,131 +43,43 @@ bool Buttons::start()
mavlink_conrig_state_tc_t Buttons::getState() { return state; }
-void Buttons::resetState()
-{
- // Preserve the arm switch state
- auto armSwitch = state.arm_switch;
- state = {};
- state.arm_switch = armSwitch;
-}
-
void Buttons::periodicStatusCheck()
{
+#define CHECK_BUTTON(cond, btn) \
+ if (cond) \
+ { \
+ if (guard.btn > Config::Buttons::GUARD_THRESHOLD) \
+ { \
+ guard.btn = 0; \
+ state.btn = true; \
+ LOG_DEBUG(logger, #btn " button pressed"); \
+ } \
+ else \
+ { \
+ guard.btn++; \
+ } \
+ } \
+ else \
+ { \
+ guard.btn = 0; \
+ state.btn = false; \
+ }
+
state.arm_switch = btns::arm::value();
- if (!btns::ignition::value() && state.arm_switch)
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ignition_btn = true;
- LOG_DEBUG(logger, "Ignition button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2o_filling::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_filling_btn = true;
- LOG_DEBUG(logger, "ox filling button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2o_release::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_release_btn = true;
- LOG_DEBUG(logger, "ox release button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_release::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.n2_release_btn = true;
- LOG_DEBUG(logger, "n2 release button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2o_venting::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_venting_btn = true;
- LOG_DEBUG(logger, "ox venting button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_detach::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.n2_detach_btn = true;
- LOG_DEBUG(logger, "n2 detach button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_filling::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.n2_filling_btn = true;
- LOG_DEBUG(logger, "n2 filling button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::nitrogen::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.nitrogen_btn = true;
- LOG_DEBUG(logger, "nitrogen button pressed");
- }
- else
- {
- guard++;
- }
- }
- else
- {
- // Reset all the states and guard
- guard = 0;
- resetState();
- }
+ CHECK_BUTTON(!btns::ignition::value() && state.arm_switch, ignition_btn);
+ CHECK_BUTTON(btns::n2o_filling::value(), ox_filling_btn);
+ CHECK_BUTTON(btns::n2o_release::value(), ox_release_btn);
+ CHECK_BUTTON(btns::n2_release::value(), n2_release_btn);
+ CHECK_BUTTON(btns::n2o_venting::value(), ox_venting_btn);
+ CHECK_BUTTON(btns::n2_detach::value(), n2_detach_btn);
+ CHECK_BUTTON(btns::n2_filling::value(), n2_filling_btn);
+ CHECK_BUTTON(btns::nitrogen::value(), nitrogen_btn);
+
+#undef CHECK_BUTTON
// Set the internal button state in Radio module
- getModule<Radio>()->setButtonsState(state);
+ getModule<Radio>()->updateButtonState(state);
}
void Buttons::enableIgnition() { ui::armedLed::high(); }
diff --git a/src/ConRIG/Buttons/Buttons.h b/src/ConRIG/Buttons/Buttons.h
index ecd160f2fd7d43d43d0c08e8de4aba4aee57a0bf..2d6fff4ae199923019fe7a44d52ae447647899e2 100644
--- a/src/ConRIG/Buttons/Buttons.h
+++ b/src/ConRIG/Buttons/Buttons.h
@@ -36,8 +36,6 @@ class Radio;
class Buttons : public Boardcore::InjectableWithDeps<BoardScheduler, Radio>
{
public:
- Buttons();
-
[[nodiscard]] bool start();
mavlink_conrig_state_tc_t getState();
@@ -46,14 +44,11 @@ public:
void disableIgnition();
private:
- void resetState();
-
void periodicStatusCheck();
- mavlink_conrig_state_tc_t state;
-
+ mavlink_conrig_state_tc_t state{};
// Counter guard to avoid spurious triggers
- uint8_t guard = 0;
+ mavlink_conrig_state_tc_t guard{};
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("buttons");
};
diff --git a/src/ConRIG/Configs/RadioConfig.h b/src/ConRIG/Configs/RadioConfig.h
index 735ae3f51db71ad76c0ebe6a8ce151bdc35a39b9..af7f66cdea1a9a808c22f1cdf18404ea3e97bd82 100644
--- a/src/ConRIG/Configs/RadioConfig.h
+++ b/src/ConRIG/Configs/RadioConfig.h
@@ -50,6 +50,12 @@ constexpr uint8_t MAV_COMPONENT_ID = 96;
// Periodic telemetries frequency
constexpr Hertz PING_GSE_PERIOD = 2_hz;
+// Audio feedback message threshold
+constexpr auto AUDIO_FEEDBACK_THRESHOLD = 10;
+// Value to reset the message counter to, to avoid long pauses without audio
+// feedback after startup or disarming
+constexpr auto AUDIO_FEEDBACK_RESET_VALUE = AUDIO_FEEDBACK_THRESHOLD * 4 / 5;
+
} // namespace Radio
} // namespace Config
} // namespace ConRIG
diff --git a/src/ConRIG/Radio/Radio.cpp b/src/ConRIG/Radio/Radio.cpp
index af3e1cf783a54f6658200b941995d4e095973c00..f9e218372658f5aea704a04a780a7963e658f7a7 100644
--- a/src/ConRIG/Radio/Radio.cpp
+++ b/src/ConRIG/Radio/Radio.cpp
@@ -63,10 +63,18 @@ void Radio::handleMessage(const mavlink_message_t& msg)
case MAVLINK_MSG_ID_GSE_TM:
{
int armingState = mavlink_msg_gse_tm_get_arming_state(&msg);
- messagesReceived += 1;
+ bool wasArmed = isArmed;
+ bool isNowArmed = armingState == 1;
+ isArmed = isNowArmed;
+
+ // Reset the message counter to a value higher than zero to avoid
+ // long pauses without audio feedback after disarming
+ if (wasArmed && !isNowArmed)
+ messagesReceived = Config::Radio::AUDIO_FEEDBACK_RESET_VALUE;
+ else
+ messagesReceived += 1;
- isArmed = armingState == 1;
- if (armingState == 1)
+ if (isNowArmed)
getModule<Buttons>()->enableIgnition();
else
getModule<Buttons>()->disableIgnition();
@@ -80,9 +88,8 @@ void Radio::handleMessage(const mavlink_message_t& msg)
// we assume this ack is about the last sent message
if (id == MAVLINK_MSG_ID_CONRIG_STATE_TC)
{
- Lock<FastMutex> lock{buttonsMutex};
// Reset the internal button state
- buttonState = {};
+ resetButtonState();
}
break;
@@ -145,7 +152,8 @@ void Radio::loopBuzzer()
Thread::sleep(100);
// Doesn't matter the precision, the important thing is
// the beep, this is because an atomic is used
- if ((!isArmed && messagesReceived > 2) ||
+ if ((!isArmed &&
+ messagesReceived > Config::Radio::AUDIO_FEEDBACK_THRESHOLD) ||
(isArmed && messagesReceived > 0))
{
messagesReceived = 0;
@@ -156,11 +164,11 @@ void Radio::loopBuzzer()
}
}
-void Radio::setButtonsState(mavlink_conrig_state_tc_t state)
+void Radio::updateButtonState(const mavlink_conrig_state_tc_t& state)
{
Lock<FastMutex> lock{buttonsMutex};
- // The OR operator is introduced to make sure that the receiver
- // understood the command
+ // Merge the new state with the old one, an extra-dumb way to ensure
+ // that we don't lose any button pressess
buttonState.ox_filling_btn |= state.ox_filling_btn;
buttonState.ox_release_btn |= state.ox_release_btn;
buttonState.n2_filling_btn |= state.n2_filling_btn;
@@ -170,11 +178,24 @@ void Radio::setButtonsState(mavlink_conrig_state_tc_t state)
buttonState.nitrogen_btn |= state.nitrogen_btn;
buttonState.ox_detach_btn |= state.ox_detach_btn;
buttonState.n2_quenching_btn |= state.n2_quenching_btn;
- buttonState.n2_3way_btn |= state.n2_3way_btn;
buttonState.tars3_btn |= state.tars3_btn;
buttonState.tars3m_btn |= state.tars3m_btn;
buttonState.ignition_btn |= state.ignition_btn;
- buttonState.arm_switch |= state.arm_switch;
+
+ // Don't merge lever states
+ buttonState.n2_3way_btn = state.n2_3way_btn;
+ buttonState.arm_switch = state.arm_switch;
+}
+
+void Radio::resetButtonState()
+{
+ Lock<FastMutex> lock{buttonsMutex};
+ // Save and restore lever states
+ auto n2_3way_btn = buttonState.n2_3way_btn;
+ auto arm_switch = buttonState.arm_switch;
+ buttonState = {};
+ buttonState.n2_3way_btn = n2_3way_btn;
+ buttonState.arm_switch = arm_switch;
}
bool Radio::start()
@@ -229,5 +250,3 @@ bool Radio::start()
}
MavlinkStatus Radio::getMavlinkStatus() { return mavDriver->getStatus(); }
-
-Radio::Radio() : buttonState() {}
diff --git a/src/ConRIG/Radio/Radio.h b/src/ConRIG/Radio/Radio.h
index f9d5edb293ce01516b11dfd26ea984318950b110..29197d28011941501edad0de994f1d06b94e7954 100644
--- a/src/ConRIG/Radio/Radio.h
+++ b/src/ConRIG/Radio/Radio.h
@@ -49,13 +49,11 @@ class Radio : public Boardcore::InjectableWithDeps<Buses, BoardScheduler,
Buttons, Serial>
{
public:
- Radio();
-
[[nodiscard]] bool start();
Boardcore::MavlinkStatus getMavlinkStatus();
- void setButtonsState(mavlink_conrig_state_tc_t state);
+ void updateButtonState(const mavlink_conrig_state_tc_t& state);
bool enqueueMessage(const mavlink_message_t& msg);
@@ -64,6 +62,8 @@ private:
void loopBuzzer();
void handleMessage(const mavlink_message_t& msg);
+ void resetButtonState();
+
std::unique_ptr<Boardcore::SX1278Lora> radio;
std::unique_ptr<MavDriver> mavDriver;
@@ -72,14 +72,15 @@ private:
messageQueue;
miosix::FastMutex queueMutex;
- miosix::FastMutex buttonsMutex;
// Button internal state
- mavlink_conrig_state_tc_t buttonState;
+ miosix::FastMutex buttonsMutex;
+ mavlink_conrig_state_tc_t buttonState{};
std::thread buzzerLooper;
- std::atomic<uint8_t> messagesReceived{0};
+ std::atomic<uint8_t> messagesReceived{
+ Config::Radio::AUDIO_FEEDBACK_RESET_VALUE};
std::atomic<bool> isArmed{false};
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("radio");
diff --git a/src/ConRIGv2/Buttons/Buttons.cpp b/src/ConRIGv2/Buttons/Buttons.cpp
index e7056ab79167ecda0e60e0254917e566461ca41e..f30478643745bf43486aa25a24b2980da0fd1913 100644
--- a/src/ConRIGv2/Buttons/Buttons.cpp
+++ b/src/ConRIGv2/Buttons/Buttons.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2025 Skyward Experimental Rocketry
- * Author: Ettore Pane
+ * Authors: Ettore Pane, Niccolò Betto
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -32,15 +32,10 @@ using namespace miosix;
using namespace Boardcore;
using namespace ConRIGv2;
-Buttons::Buttons()
-{
- resetState();
- state.arm_switch = false;
-}
-
bool Buttons::start()
{
- TaskScheduler& scheduler = getModule<BoardScheduler>()->getRadioScheduler();
+ TaskScheduler& scheduler =
+ getModule<BoardScheduler>()->getButtonsScheduler();
return scheduler.addTask([this]() { periodicStatusCheck(); },
Config::Buttons::BUTTON_SAMPLE_PERIOD) != 0;
@@ -48,196 +43,48 @@ bool Buttons::start()
mavlink_conrig_state_tc_t Buttons::getState() { return state; }
-void Buttons::resetState()
-{
- // Preserve the arm switch state
- auto armSwitch = state.arm_switch;
- state = {};
- state.arm_switch = armSwitch;
-}
-
void Buttons::periodicStatusCheck()
{
- state.arm_switch = btns::arm::value();
-
- if (!btns::ignition::value() && state.arm_switch)
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ignition_btn = true;
- LOG_DEBUG(logger, "Ignition button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::ox_filling::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_filling_btn = true;
- LOG_DEBUG(logger, "Ox filling button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::ox_release::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_release_btn = true;
- LOG_DEBUG(logger, "Ox release button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::ox_detach::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_detach_btn = true;
- LOG_DEBUG(logger, "Ox detach button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_3way::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_detach_btn = true;
- LOG_DEBUG(logger, "n2 3way button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_filling::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.n2_filling_btn = true;
- LOG_DEBUG(logger, "N2 filling button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_release::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.n2_release_btn = true;
- LOG_DEBUG(logger, "N2 release button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_detach::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.n2_detach_btn = true;
- LOG_DEBUG(logger, "N2 detach button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::nitrogen::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.nitrogen_btn = true;
- LOG_DEBUG(logger, "Nitrogen button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::ox_venting::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.ox_venting_btn = true;
- LOG_DEBUG(logger, "Ox venting button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::n2_quenching::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.n2_quenching_btn = true;
- LOG_DEBUG(logger, "N2 quenching button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::tars3::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.tars3_btn = true;
- LOG_DEBUG(logger, "Tars3 button pressed");
- }
- else
- {
- guard++;
- }
- }
- else if (btns::tars3m::value())
- {
- if (guard > Config::Buttons::GUARD_THRESHOLD)
- {
- guard = 0;
- state.tars3m_btn = true;
- LOG_DEBUG(logger, "Tars3m button pressed");
- }
- else
- {
- guard++;
- }
- }
- else
- {
- // Reset all the states and guard
- guard = 0;
- resetState();
+#define CHECK_BUTTON(cond, btn) \
+ if (cond) \
+ { \
+ if (guard.btn > Config::Buttons::GUARD_THRESHOLD) \
+ { \
+ guard.btn = 0; \
+ state.btn = true; \
+ LOG_DEBUG(logger, #btn " button pressed"); \
+ } \
+ else \
+ { \
+ guard.btn++; \
+ } \
+ } \
+ else \
+ { \
+ guard.btn = 0; \
+ state.btn = false; \
}
+ state.arm_switch = btns::arm::value();
+ state.n2_3way_btn = btns::n2_3way::value();
+
+ CHECK_BUTTON(!btns::ignition::value() && state.arm_switch, ignition_btn);
+ CHECK_BUTTON(btns::ox_filling::value(), ox_filling_btn);
+ CHECK_BUTTON(btns::ox_release::value(), ox_release_btn);
+ CHECK_BUTTON(btns::ox_detach::value(), ox_detach_btn);
+ CHECK_BUTTON(btns::ox_venting::value(), ox_venting_btn);
+ CHECK_BUTTON(btns::n2_filling::value(), n2_filling_btn);
+ CHECK_BUTTON(btns::n2_release::value(), n2_release_btn);
+ CHECK_BUTTON(btns::n2_detach::value(), n2_detach_btn);
+ CHECK_BUTTON(btns::n2_quenching::value(), n2_quenching_btn);
+ CHECK_BUTTON(btns::nitrogen::value(), nitrogen_btn);
+ CHECK_BUTTON(btns::tars3::value(), tars3_btn);
+ CHECK_BUTTON(btns::tars3m::value(), tars3m_btn);
+
+#undef CHECK_BUTTON
+
// Set the internal button state in Radio module
- getModule<Radio>()->setButtonsState(state);
+ getModule<Radio>()->updateButtonState(state);
}
void Buttons::enableIgnition() { ui::armedLed::high(); }
diff --git a/src/ConRIGv2/Buttons/Buttons.h b/src/ConRIGv2/Buttons/Buttons.h
index 5d2c26d0e205475718628903b21934478dfb8c6e..2c4d5440825d41614c65c8dc960fa64bd26fe4fc 100644
--- a/src/ConRIGv2/Buttons/Buttons.h
+++ b/src/ConRIGv2/Buttons/Buttons.h
@@ -36,8 +36,6 @@ class Radio;
class Buttons : public Boardcore::InjectableWithDeps<BoardScheduler, Radio>
{
public:
- Buttons();
-
[[nodiscard]] bool start();
mavlink_conrig_state_tc_t getState();
@@ -46,14 +44,11 @@ public:
void disableIgnition();
private:
- void resetState();
-
void periodicStatusCheck();
- mavlink_conrig_state_tc_t state;
-
+ mavlink_conrig_state_tc_t state{};
// Counter guard to avoid spurious triggers
- uint8_t guard = 0;
+ mavlink_conrig_state_tc_t guard{};
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("buttons");
};
diff --git a/src/ConRIGv2/Configs/RadioConfig.h b/src/ConRIGv2/Configs/RadioConfig.h
index 8f2695e6784a4d18966dbb0ad481efeb2eebe4bb..b6065aa5e0652ef0d3aeaa43bc9316b47d37160a 100644
--- a/src/ConRIGv2/Configs/RadioConfig.h
+++ b/src/ConRIGv2/Configs/RadioConfig.h
@@ -50,6 +50,12 @@ constexpr uint8_t MAV_COMPONENT_ID = 96;
// Periodic telemetries frequency
constexpr Hertz PING_GSE_PERIOD = 2_hz;
+// Audio feedback message threshold
+constexpr auto AUDIO_FEEDBACK_THRESHOLD = 10;
+// Value to reset the message counter to, to avoid long pauses without audio
+// feedback after startup or disarming
+constexpr auto AUDIO_FEEDBACK_RESET_VALUE = AUDIO_FEEDBACK_THRESHOLD * 4 / 5;
+
} // namespace Radio
} // namespace Config
} // namespace ConRIGv2
diff --git a/src/ConRIGv2/Radio/Radio.cpp b/src/ConRIGv2/Radio/Radio.cpp
index 8e7ebb8436e7f309b9cc74e4bb1e0665e72c250a..3091b2a7c18990bc20ff87c01ee2c452cc6f6381 100644
--- a/src/ConRIGv2/Radio/Radio.cpp
+++ b/src/ConRIGv2/Radio/Radio.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2025 Skyward Experimental Rocketry
- * Author: Ettore Pane
+ * Authors: Ettore Pane, Niccolò Betto
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -63,10 +63,18 @@ void Radio::handleMessage(const mavlink_message_t& msg)
case MAVLINK_MSG_ID_GSE_TM:
{
int armingState = mavlink_msg_gse_tm_get_arming_state(&msg);
- messagesReceived += 1;
+ bool wasArmed = isArmed;
+ bool isNowArmed = armingState == 1;
+ isArmed = isNowArmed;
+
+ // Reset the message counter to a value higher than zero to avoid
+ // long pauses without audio feedback after disarming
+ if (wasArmed && !isNowArmed)
+ messagesReceived = Config::Radio::AUDIO_FEEDBACK_RESET_VALUE;
+ else
+ messagesReceived += 1;
- isArmed = armingState == 1;
- if (armingState == 1)
+ if (isNowArmed)
getModule<Buttons>()->enableIgnition();
else
getModule<Buttons>()->disableIgnition();
@@ -80,9 +88,8 @@ void Radio::handleMessage(const mavlink_message_t& msg)
// we assume this ack is about the last sent message
if (id == MAVLINK_MSG_ID_CONRIG_STATE_TC)
{
- Lock<FastMutex> lock{buttonsMutex};
// Reset the internal button state
- buttonState = {};
+ resetButtonState();
}
break;
@@ -150,9 +157,8 @@ void Radio::buzzerOff()
void Radio::buzzerTask()
{
- constexpr int beepPeriod = 5; // seconds
-
- if ((!isArmed && messagesReceived > beepPeriod * 2) ||
+ if ((!isArmed &&
+ messagesReceived > Config::Radio::AUDIO_FEEDBACK_THRESHOLD) ||
(isArmed && messagesReceived > 0))
{
messagesReceived = 0;
@@ -164,11 +170,11 @@ void Radio::buzzerTask()
}
}
-void Radio::setButtonsState(const mavlink_conrig_state_tc_t& state)
+void Radio::updateButtonState(const mavlink_conrig_state_tc_t& state)
{
Lock<FastMutex> lock{buttonsMutex};
- // The OR operator is introduced to make sure that the receiver
- // understood the command
+ // Merge the new state with the old one, an extra-dumb way to ensure
+ // that we don't lose any button pressess
buttonState.ox_filling_btn |= state.ox_filling_btn;
buttonState.ox_release_btn |= state.ox_release_btn;
buttonState.n2_filling_btn |= state.n2_filling_btn;
@@ -178,11 +184,24 @@ void Radio::setButtonsState(const mavlink_conrig_state_tc_t& state)
buttonState.nitrogen_btn |= state.nitrogen_btn;
buttonState.ox_detach_btn |= state.ox_detach_btn;
buttonState.n2_quenching_btn |= state.n2_quenching_btn;
- buttonState.n2_3way_btn |= state.n2_3way_btn;
buttonState.tars3_btn |= state.tars3_btn;
buttonState.tars3m_btn |= state.tars3m_btn;
buttonState.ignition_btn |= state.ignition_btn;
- buttonState.arm_switch |= state.arm_switch;
+
+ // Don't merge lever states
+ buttonState.n2_3way_btn = state.n2_3way_btn;
+ buttonState.arm_switch = state.arm_switch;
+}
+
+void Radio::resetButtonState()
+{
+ Lock<FastMutex> lock{buttonsMutex};
+ // Save and restore lever states
+ auto n2_3way_btn = buttonState.n2_3way_btn;
+ auto arm_switch = buttonState.arm_switch;
+ buttonState = {};
+ buttonState.n2_3way_btn = n2_3way_btn;
+ buttonState.arm_switch = arm_switch;
}
bool Radio::start()
@@ -239,7 +258,7 @@ bool Radio::start()
MavlinkStatus Radio::getMavlinkStatus() { return mavDriver->getStatus(); }
-Radio::Radio() : buzzer(MIOSIX_BUZZER_TIM, 523), buttonState()
+Radio::Radio() : buzzer(MIOSIX_BUZZER_TIM, 523)
{
buzzer.setDutyCycle(TimerUtils::Channel::MIOSIX_BUZZER_CHANNEL, 0.5);
}
diff --git a/src/ConRIGv2/Radio/Radio.h b/src/ConRIGv2/Radio/Radio.h
index 399eb42fba46ada9bbf87528b16a6d8b04091493..71120d3fbe62145106615d3ea97724a1744d7129 100644
--- a/src/ConRIGv2/Radio/Radio.h
+++ b/src/ConRIGv2/Radio/Radio.h
@@ -56,7 +56,7 @@ public:
Boardcore::MavlinkStatus getMavlinkStatus();
- void setButtonsState(const mavlink_conrig_state_tc_t& state);
+ void updateButtonState(const mavlink_conrig_state_tc_t& state);
bool enqueueMessage(const mavlink_message_t& msg);
@@ -65,6 +65,8 @@ private:
void buzzerTask();
void handleMessage(const mavlink_message_t& msg);
+ void resetButtonState();
+
std::unique_ptr<Boardcore::SX1278Lora> radio;
std::unique_ptr<MavDriver> mavDriver;
@@ -81,12 +83,13 @@ private:
messageQueue;
miosix::FastMutex queueMutex;
- miosix::FastMutex buttonsMutex;
// Button internal state
- mavlink_conrig_state_tc_t buttonState;
+ miosix::FastMutex buttonsMutex;
+ mavlink_conrig_state_tc_t buttonState{};
- std::atomic<uint8_t> messagesReceived{0};
+ std::atomic<uint8_t> messagesReceived{
+ Config::Radio::AUDIO_FEEDBACK_RESET_VALUE};
std::atomic<bool> isArmed{false};
Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("radio");
diff --git a/src/RIGv2/Actuators/Actuators.cpp b/src/RIGv2/Actuators/Actuators.cpp
index 6f2f570b529ad4d884e56f5557e12fd87a59caff..6138b6901c1dfade4006863700a83eabee0cc7da 100644
--- a/src/RIGv2/Actuators/Actuators.cpp
+++ b/src/RIGv2/Actuators/Actuators.cpp
@@ -116,14 +116,11 @@ bool Actuators::ServoInfo::setOpeningTime(uint32_t time)
}
Actuators::Actuators()
- : infos{
- MAKE_SERVO(OX_FIL), MAKE_SERVO(OX_REL),
- MAKE_DETACH_SERVO(OX_DET),
- MAKE_SERVO(N2_FIL), MAKE_SERVO(N2_REL),
- MAKE_DETACH_SERVO(N2_DET), MAKE_SERVO(NITR),
- MAKE_SERVO(OX_VEN), MAKE_SERVO(N2_QUE),
- MAKE_SERVO(MAIN),
- }, n2_3wayValveInfo(MAKE_SIMPLE_SERVO(N2_3W))
+ : infos{MAKE_SERVO(OX_FIL), MAKE_SERVO(OX_REL), MAKE_DETACH_SERVO(OX_DET),
+ MAKE_SERVO(N2_FIL), MAKE_SERVO(N2_REL), MAKE_DETACH_SERVO(N2_DET),
+ MAKE_SERVO(NITR), MAKE_SERVO(OX_VEN), MAKE_SERVO(N2_QUE),
+ MAKE_SERVO(MAIN)},
+ n2_3wayValveInfo(MAKE_SIMPLE_SERVO(N2_3W))
{
for (auto& servo : infos)
servo.unsafeSetServoPosition(0.0f);
@@ -142,6 +139,8 @@ bool Actuators::start()
for (ServoInfo& info : infos)
info.servo->enable();
+ n2_3wayValveInfo.servo->enable();
+
uint8_t result =
scheduler.addTask([this]() { updatePositionsTask(); },
Config::Servos::SERVO_TIMINGS_CHECK_PERIOD);
@@ -164,6 +163,19 @@ void Actuators::igniterOff() { relays::ignition::high(); }
bool Actuators::wiggleServo(ServosList servo)
{
+ // Special handling for the 3-way valve
+ if (servo == N2_3WAY_VALVE)
+ {
+ // Toggle the valve to the current opposite state
+ auto state = get3wayValveState();
+
+ set3wayValveState(!state);
+ Thread::sleep(1000);
+ set3wayValveState(state);
+
+ return true;
+ }
+
// Wiggle means open the servo for 1s
return openServoWithTime(servo, 1000);
}
@@ -288,16 +300,9 @@ bool Actuators::isCanServoOpen(ServosList servo)
return false;
}
-void Actuators::set3wayValveState(bool state)
-{
- auto position = state ? 1.0f : 0.0f;
- n2_3wayValveInfo.unsafeSetServoPosition(position);
-}
+void Actuators::set3wayValveState(bool state) { n2_3wayValveState = state; }
-bool Actuators::get3wayValveState()
-{
- return n2_3wayValveInfo.getServoPosition() == 1.0f;
-}
+bool Actuators::get3wayValveState() { return n2_3wayValveState; }
void Actuators::openChamberWithTime(uint32_t time)
{
@@ -371,19 +376,19 @@ Actuators::ServoInfo* Actuators::getServo(ServosList servo)
case OX_DETACH_SERVO: // OX_DET
return &infos[2];
case N2_FILLING_VALVE: // N2_FIL
- return &infos[4];
+ return &infos[3];
case N2_RELEASE_VALVE: // N2_REL
- return &infos[5];
+ return &infos[4];
case N2_DETACH_SERVO: // N2_DET
- return &infos[6];
+ return &infos[5];
case NITROGEN_VALVE: // NITR
- return &infos[7];
+ return &infos[6];
case OX_VENTING_VALVE: // OX_VEN
- return &infos[8];
+ return &infos[7];
case N2_QUENCHING_VALVE: // N2_QUE
- return &infos[9];
+ return &infos[8];
case MAIN_VALVE: // MAIN
- return &infos[10];
+ return &infos[9];
default:
// Oh FUCK
@@ -472,4 +477,8 @@ void Actuators::updatePositionsTask()
unsafeCloseChamber();
}
+
+ // Handle the 3-way valve
+ auto position = n2_3wayValveState ? 1.0f : 0.0f;
+ n2_3wayValveInfo.unsafeSetServoPosition(position);
}
diff --git a/src/RIGv2/Actuators/Actuators.h b/src/RIGv2/Actuators/Actuators.h
index 0289b1a7812906c4e7925c55a602085917b7266c..b6cb422d7fd882adbb001e0faf14e596dfd95c62 100644
--- a/src/RIGv2/Actuators/Actuators.h
+++ b/src/RIGv2/Actuators/Actuators.h
@@ -132,8 +132,9 @@ private:
std::atomic<bool> started{false};
miosix::FastMutex infosMutex;
- ServoInfo infos[10];
+ std::array<ServoInfo, 10> infos;
ServoInfo n2_3wayValveInfo;
+ std::atomic<bool> n2_3wayValveState{false};
long long chamberCloseTs =
0; ///< Timestamp to close the chamber (0 if closed)
diff --git a/src/RIGv2/Configs/RadioConfig.h b/src/RIGv2/Configs/RadioConfig.h
index af1b8643e9be58a52e7c29b74f2514fd5a38db79..fb8a2a6b8b701206250412c7786e85a4ad94d7eb 100644
--- a/src/RIGv2/Configs/RadioConfig.h
+++ b/src/RIGv2/Configs/RadioConfig.h
@@ -39,8 +39,9 @@ constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0;
constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10;
-constexpr unsigned int CIRCULAR_BUFFER_SIZE = 30;
-constexpr unsigned int MAX_PACKETS_PER_FLUSH = 4;
+constexpr unsigned int CIRCULAR_BUFFER_SIZE = 30;
+// Measured empirically, used to determine how many packets to send in a flush
+constexpr unsigned int BITRATE = 4000; // [bits/s]
constexpr uint8_t MAV_SYSTEM_ID = MAV_SYSID_RIG;
constexpr uint8_t MAV_COMPONENT_ID = 0;
diff --git a/src/RIGv2/Configs/SensorsConfig.h b/src/RIGv2/Configs/SensorsConfig.h
index 397a5bd8ac3759b9bd86a8d95dfbab42d4be0ea9..ac9e7bea67ff55a6d2ee4a03fbf42bf729cd2237 100644
--- a/src/RIGv2/Configs/SensorsConfig.h
+++ b/src/RIGv2/Configs/SensorsConfig.h
@@ -27,6 +27,7 @@
#include <sensors/MAX31856/MAX31856.h>
#include <units/Frequency.h>
+#include <chrono>
#include <cstdint>
namespace RIGv2
@@ -38,6 +39,7 @@ namespace Config
namespace Sensors
{
+/* linter off */ using namespace std::chrono;
/* linter off */ using namespace Boardcore::Units::Frequency;
namespace ADS131M08
@@ -46,7 +48,6 @@ constexpr auto OSR = Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
constexpr bool GLOBAL_CHOP_MODE_EN = true;
constexpr Hertz PERIOD = 100_hz;
-constexpr bool ENABLED = true;
// Servo current sensor calibration data
// - A: 0.0 V: 2.520
@@ -62,6 +63,8 @@ constexpr float SERVO_CURRENT_ZERO = 2.520 / SERVO_CURRENT_SCALE;
namespace ADC_1
{
+constexpr bool ENABLED = true;
+
using Channel = Boardcore::ADS131M08Defs::Channel;
constexpr auto OX_VESSEL_PT_CHANNEL = Channel::CHANNEL_0;
@@ -70,25 +73,27 @@ constexpr auto N2_VESSEL_1_PT_CHANNEL = Channel::CHANNEL_2;
constexpr auto N2_VESSEL_2_PT_CHANNEL = Channel::CHANNEL_3;
constexpr auto SERVO_CURRENT_CHANNEL = Channel::CHANNEL_4;
constexpr auto OX_VESSEL_LC_CHANNEL = Channel::CHANNEL_5;
-constexpr auto OX_TANK_LC_CHANNEL = Channel::CHANNEL_6;
+constexpr auto ROCKET_LC_CHANNEL = Channel::CHANNEL_6;
constexpr auto N2_FILLING_PT_CHANNEL = Channel::CHANNEL_7;
-constexpr float CH0_SHUNT_RESISTANCE = 29.4048; // TODO: check
-constexpr float CH1_SHUNT_RESISTANCE = 29.5830; // TODO: check
-constexpr float CH2_SHUNT_RESISTANCE = 29.4973; // TODO: check
-constexpr float CH3_SHUNT_RESISTANCE = 29.8849; // TODO: check
-constexpr float CH7_SHUNT_RESISTANCE = 29.0; // TODO: measure
+constexpr float CH0_SHUNT_RESISTANCE = 29.283f;
+constexpr float CH1_SHUNT_RESISTANCE = 29.233f;
+constexpr float CH2_SHUNT_RESISTANCE = 29.268f;
+constexpr float CH3_SHUNT_RESISTANCE = 29.645f;
+constexpr float CH7_SHUNT_RESISTANCE = 29.708f;
} // namespace ADC_1
namespace ADC_2
{
+constexpr bool ENABLED = true;
+
using Channel = Boardcore::ADS131M08Defs::Channel;
-constexpr auto OX_TANK_TOP_PT_CHANNEL = Channel::CHANNEL_0;
-constexpr auto OX_TANK_BOTTOM_PT_CHANNEL = Channel::CHANNEL_1;
+constexpr auto OX_TANK_PT_CHANNEL = Channel::CHANNEL_0;
+constexpr auto N2_TANK_PT_CHANNEL = Channel::CHANNEL_1;
-constexpr float CH0_SHUNT_RESISTANCE = 29.0; // TODO: measure
-constexpr float CH1_SHUNT_RESISTANCE = 29.0; // TODO: measure
+constexpr float CH0_SHUNT_RESISTANCE = 29.685f;
+constexpr float CH1_SHUNT_RESISTANCE = 29.625f;
} // namespace ADC_2
namespace MAX31856
@@ -99,40 +104,36 @@ constexpr bool ENABLED = true;
namespace Trafag
{
-constexpr float OX_VESSEL_SHUNT_RESISTANCE = ADC_1::CH0_SHUNT_RESISTANCE;
-constexpr float OX_FILLING_SHUNT_RESISTANCE = ADC_1::CH1_SHUNT_RESISTANCE;
-constexpr float N2_VESSEL1_SHUNT_RESISTANCE = ADC_1::CH2_SHUNT_RESISTANCE;
-constexpr float N2_VESSEL2_SHUNT_RESISTANCE = ADC_1::CH3_SHUNT_RESISTANCE;
-constexpr float N2_FILLING_SHUNT_RESISTANCE = ADC_1::CH7_SHUNT_RESISTANCE;
-constexpr float OX_TANK_TOP_SHUNT_RESISTANCE = ADC_2::CH0_SHUNT_RESISTANCE;
-constexpr float OX_TANK_BOTTOM_SHUNT_RESISTANCE = ADC_2::CH1_SHUNT_RESISTANCE;
+constexpr float OX_VESSEL_SHUNT_RESISTANCE = ADC_1::CH0_SHUNT_RESISTANCE;
+constexpr float OX_FILLING_SHUNT_RESISTANCE = ADC_1::CH1_SHUNT_RESISTANCE;
+constexpr float N2_VESSEL1_SHUNT_RESISTANCE = ADC_1::CH2_SHUNT_RESISTANCE;
+constexpr float N2_VESSEL2_SHUNT_RESISTANCE = ADC_1::CH3_SHUNT_RESISTANCE;
+constexpr float N2_FILLING_SHUNT_RESISTANCE = ADC_1::CH7_SHUNT_RESISTANCE;
+constexpr float OX_TANK_SHUNT_RESISTANCE = ADC_2::CH0_SHUNT_RESISTANCE;
+constexpr float N2_TANK_SHUNT_RESISTANCE = ADC_2::CH1_SHUNT_RESISTANCE;
constexpr float MIN_CURRENT = 4;
constexpr float MAX_CURRENT = 20;
-// TODO: check depending on which trafags are used
-constexpr float OX_VESSEL_MAX_PRESSURE = 100; // bar
-constexpr float OX_FILLING_MAX_PRESSURE = 250; // bar
-constexpr float N2_VESSEL1_MAX_PRESSURE = 400; // bar
-constexpr float N2_VESSEL2_MAX_PRESSURE = 400; // bar
-constexpr float N2_FILLING_MAX_PRESSURE = 400; // bar
-constexpr float OX_TANK_TOP_MAX_PRESSURE = 250; // bar
-constexpr float OX_TANK_BOTTOM_MAX_PRESSURE = 100; // bar
+constexpr float OX_VESSEL_MAX_PRESSURE = 100; // bar
+constexpr float OX_FILLING_MAX_PRESSURE = 100; // bar
+constexpr float N2_VESSEL1_MAX_PRESSURE = 100; // bar
+constexpr float N2_VESSEL2_MAX_PRESSURE = 100; // bar
+constexpr float N2_FILLING_MAX_PRESSURE = 100; // bar
+constexpr float OX_TANK_MAX_PRESSURE = 100; // bar
+constexpr float N2_TANK_MAX_PRESSURE = 250; // bar
} // namespace Trafag
namespace LoadCell
{
-constexpr unsigned int CALIBRATE_SAMPLE_COUNT = 10;
-constexpr unsigned int CALIBRATE_SAMPLE_PERIOD = 40;
-
-// LC Tank sensor calibration data
-// - 1.866kg V: 0.000941
-// - 5.050kg V: 0.002550
-// - 6.916kg V: 0.003559
-constexpr float TANK_P0_VOLTAGE = 0.000941;
-constexpr float TANK_P0_MASS = 1.866;
-constexpr float TANK_P1_VOLTAGE = 0.003559;
-constexpr float TANK_P1_MASS = 6.916;
+constexpr auto CALIBRATE_SAMPLE_COUNT = 10;
+constexpr auto CALIBRATE_SAMPLE_PERIOD = 40ms;
+
+// Rocket ramp loadcell calibration data
+constexpr float ROCKET_P0_VOLTAGE = -0.0004273;
+constexpr float ROCKET_P0_MASS = 10.005;
+constexpr float ROCKET_P1_VOLTAGE = -0.0018125;
+constexpr float ROCKET_P1_MASS = 40.290;
/* OLD CALIBRATION DATA (before 07/09/2024, before new flipping)
// LC Vessel sensor calibration data
diff --git a/src/RIGv2/Radio/Radio.cpp b/src/RIGv2/Radio/Radio.cpp
index 472eb5d10ec967853d538710de7c789db9abd39a..26bce81fea300e75fa65c225faa5ee18abdd254b 100644
--- a/src/RIGv2/Radio/Radio.cpp
+++ b/src/RIGv2/Radio/Radio.cpp
@@ -22,6 +22,7 @@
#include "Radio.h"
+#include <ConRIGv2/Configs/RadioConfig.h>
#include <common/Events.h>
#include <common/Radio.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
@@ -98,27 +99,44 @@ bool Radio::start()
return true;
}
-void Radio::enqueuePacket(const mavlink_message_t& msg)
+void Radio::enqueueMessage(const mavlink_message_t& msg)
{
- queuedPackets.put(msg);
+ queuedMessages.put(msg);
}
-void Radio::flushPackets()
+void Radio::flushMessages()
{
- // Flush all packets of the queue
- size_t count =
- std::min(queuedPackets.count(), Config::Radio::MAX_PACKETS_PER_FLUSH);
- for (size_t i = 0; i < count; i++)
+ // Flush the maximum number of packets according to the bitrate
+ constexpr auto sendSlotDuration =
+ 1.0f / ConRIGv2::Config::Radio::PING_GSE_PERIOD.value(); // [s]
+ constexpr auto maxFlushSize =
+ static_cast<size_t>(Config::Radio::BITRATE / 8 * sendSlotDuration);
+
+ try
{
- try
- {
- mavDriver->enqueueMsg(queuedPackets.pop());
- }
- catch (...)
+ size_t bytesSent = 0;
+
+ while (!queuedMessages.isEmpty())
{
- // This shouldn't happen, but still try to prevent it
+ auto& message = queuedMessages.get();
+ auto messageLength = mavlink_msg_get_send_buffer_length(&message);
+ if (bytesSent + messageLength > maxFlushSize)
+ break;
+
+ bool enqueued = mavDriver->enqueueMsg(message);
+ // If we reached the maximum queue size, stop flushing
+ if (!enqueued)
+ break;
+
+ queuedMessages.pop();
+ bytesSent += messageLength;
}
}
+ catch (std::range_error& e)
+ {
+ // This shouldn't happen, but still try to prevent it
+ LOG_ERR(logger, "Error while flushing packets: %s", e.what());
+ }
}
void Radio::enqueueAck(const mavlink_message_t& msg)
@@ -127,7 +145,7 @@ void Radio::enqueueAck(const mavlink_message_t& msg)
mavlink_msg_ack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &ackMsg, msg.msgid,
msg.seq);
- enqueuePacket(ackMsg);
+ enqueueMessage(ackMsg);
}
void Radio::enqueueNack(const mavlink_message_t& msg, uint8_t errorId)
@@ -136,7 +154,7 @@ void Radio::enqueueNack(const mavlink_message_t& msg, uint8_t errorId)
mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &nackMsg,
msg.msgid, msg.seq, errorId);
- enqueuePacket(nackMsg);
+ enqueueMessage(nackMsg);
}
void Radio::enqueueWack(const mavlink_message_t& msg, uint8_t errorId)
@@ -145,7 +163,7 @@ void Radio::enqueueWack(const mavlink_message_t& msg, uint8_t errorId)
mavlink_msg_wack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &wackMsg,
msg.msgid, msg.seq, errorId);
- enqueuePacket(wackMsg);
+ enqueueMessage(wackMsg);
}
MavlinkStatus Radio::getMavStatus()
@@ -424,7 +442,7 @@ void Radio::enqueueRegistry()
}
}
- enqueuePacket(msg);
+ enqueueMessage(msg);
});
}
@@ -447,7 +465,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_msg_sensor_state_tm_encode(
Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg, &tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
}
return true;
@@ -477,7 +495,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_msg_sys_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -503,7 +521,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_msg_logger_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -531,7 +549,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_msg_mavlink_stats_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
&msg, &tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -546,7 +564,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.timestamp = TimestampTimer::getTimestamp();
// Sensors
- tm.rocket_mass = sensors->getOxTankWeight().load;
+ tm.rocket_mass = sensors->getRocketWeight().load;
tm.ox_vessel_mass = sensors->getOxVesselWeight().load;
tm.ox_vessel_pressure = sensors->getOxVesselPressure().pressure;
@@ -621,7 +639,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -636,9 +654,11 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.timestamp = TimestampTimer::getTimestamp();
// Sensors (either CAN or local)
- tm.ox_tank_top_pressure = sensors->getOxTankTopPressure().pressure;
+ tm.n2_tank_pressure = sensors->getN2TankPressure().pressure;
tm.ox_tank_bot_pressure =
sensors->getOxTankBottomPressure().pressure;
+ tm.ox_tank_top_pressure =
+ sensors->getCanOxTankTopPressure().pressure;
tm.combustion_chamber_pressure =
sensors->getCombustionChamberPressure().pressure;
tm.ox_tank_temperature =
@@ -662,7 +682,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -706,7 +726,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_msg_adc_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
data = getModule<Sensors>()->getADC2LastSample();
@@ -732,7 +752,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_msg_adc_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -746,12 +766,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "VesselPressure");
+ strcpy(tm.sensor_name, "OxVesselPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
&msg, &tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -764,12 +784,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "FillingPressure");
+ strcpy(tm.sensor_name, "OxFillingPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
&msg, &tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -782,12 +802,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "BottomTankPressure");
+ strcpy(tm.sensor_name, "OxTankBotPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
&msg, &tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -796,16 +816,16 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_message_t msg;
mavlink_pressure_tm_t tm;
- PressureData data = getModule<Sensors>()->getOxTankTopPressure();
+ PressureData data = getModule<Sensors>()->getCanOxTankTopPressure();
tm.timestamp = data.pressureTimestamp;
tm.pressure = data.pressure;
- strcpy(tm.sensor_name, "TopTankPressure");
+ strcpy(tm.sensor_name, "OxTankTopPressure");
mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
&msg, &tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -823,7 +843,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_msg_temp_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -836,12 +856,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
tm.timestamp = data.loadTimestamp;
tm.load = data.load;
- strcpy(tm.sensor_name, "VesselWeight");
+ strcpy(tm.sensor_name, "OxVesselWeight");
mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -854,12 +874,12 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
tm.timestamp = data.loadTimestamp;
tm.load = data.load;
- strcpy(tm.sensor_name, "TankWeight");
+ strcpy(tm.sensor_name, "OxTankWeight");
mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -877,7 +897,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
mavlink_msg_voltage_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID, &msg,
&tm);
- enqueuePacket(msg);
+ enqueueMessage(msg);
return true;
}
@@ -892,14 +912,13 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
{
// Acknowledge the state
enqueueAck(msg);
-
- // Flush all pending packets
- flushPackets();
-
// Send GSE and motor telemetry
enqueueSystemTm(MAV_GSE_ID);
enqueueSystemTm(MAV_MOTOR_ID);
+ // Flush all pending packets
+ flushMessages();
+
mavlink_conrig_state_tc_t state;
mavlink_msg_conrig_state_tc_decode(&msg, &state);
diff --git a/src/RIGv2/Radio/Radio.h b/src/RIGv2/Radio/Radio.h
index 37b61efd88fd975781b7d097e29bf8f486a774a1..935a9dcdb36f67417ffaa48e837a9c238ddcef4c 100644
--- a/src/RIGv2/Radio/Radio.h
+++ b/src/RIGv2/Radio/Radio.h
@@ -63,8 +63,8 @@ private:
void enqueueWack(const mavlink_message_t& msg, uint8_t errorId);
void enqueueNack(const mavlink_message_t& msg, uint8_t errorId);
- void enqueuePacket(const mavlink_message_t& msg);
- void flushPackets();
+ void enqueueMessage(const mavlink_message_t& msg);
+ void flushMessages();
void handleMessage(const mavlink_message_t& msg);
void handleCommand(const mavlink_message_t& msg);
@@ -80,7 +80,7 @@ private:
Boardcore::CircularBuffer<mavlink_message_t,
Config::Radio::CIRCULAR_BUFFER_SIZE>
- queuedPackets;
+ queuedMessages;
std::atomic<bool> started{false};
std::unique_ptr<Boardcore::SX1278Lora> radio;
diff --git a/src/RIGv2/Sensors/Sensors.cpp b/src/RIGv2/Sensors/Sensors.cpp
index a6f3aed8463e97951d09bc7171b119663598069a..89e22b6491d570854891dfbacd5a8ee3d65707e0 100644
--- a/src/RIGv2/Sensors/Sensors.cpp
+++ b/src/RIGv2/Sensors/Sensors.cpp
@@ -26,6 +26,9 @@
#include <drivers/timer/TimestampTimer.h>
#include <interfaces-impl/hwmapping.h>
+#include <chrono>
+
+using namespace std::chrono;
using namespace Boardcore;
using namespace miosix;
using namespace RIGv2;
@@ -37,18 +40,26 @@ bool Sensors::start()
if (Config::Sensors::InternalADC::ENABLED)
internalAdcInit();
- if (Config::Sensors::ADS131M08::ENABLED)
+ if (Config::Sensors::ADC_1::ENABLED)
{
adc1Init();
- adc2Init();
oxVesselPressureInit();
oxFillingPressureInit();
- oxTankTopPressureInit();
- oxTankBottomPressureInit();
+ n2Vessel1PressureInit();
+ n2Vessel2PressureInit();
+ n2FillingPressureInit();
oxVesselWeightInit();
+ rocketWeightInit();
oxTankWeightInit();
}
+ if (Config::Sensors::ADC_2::ENABLED)
+ {
+ adc2Init();
+ oxTankBottomPressureInit();
+ n2TankPressureInit();
+ }
+
if (Config::Sensors::MAX31856::ENABLED)
tc1Init();
@@ -112,29 +123,29 @@ PressureData Sensors::getN2FillingPressure()
: PressureData{};
}
-PressureData Sensors::getOxTankTopPressure()
+PressureData Sensors::getOxTankBottomPressure()
{
if (useCanData)
{
- return getCanOxTankTopPressure();
+ return getCanOxTankBottomPressure();
}
else
{
- return oxTankTopPressure ? oxTankTopPressure->getLastSample()
- : PressureData{};
+ return oxTankBottomPressure ? oxTankBottomPressure->getLastSample()
+ : PressureData{};
}
}
-PressureData Sensors::getOxTankBottomPressure()
+PressureData Sensors::getN2TankPressure()
{
if (useCanData)
{
- return getCanOxTankBottomPressure();
+ return getCanN2TankPressure();
}
else
{
- return oxTankBottomPressure ? oxTankBottomPressure->getLastSample()
- : PressureData{};
+ return n2TankPressure ? n2TankPressure->getLastSample()
+ : PressureData{};
}
}
@@ -156,18 +167,17 @@ TemperatureData Sensors::getOxTankTemperature()
LoadCellData Sensors::getOxVesselWeight()
{
- if (oxVesselWeight)
- return oxVesselWeight->getLastSample();
- else
- return {};
+ return oxVesselWeight ? oxVesselWeight->getLastSample() : LoadCellData{};
+}
+
+LoadCellData Sensors::getRocketWeight()
+{
+ return rocketWeight ? rocketWeight->getLastSample() : LoadCellData{};
}
LoadCellData Sensors::getOxTankWeight()
{
- if (oxTankWeight)
- return oxTankWeight->getLastSample();
- else
- return {};
+ return oxTankWeight ? oxTankWeight->getLastSample() : LoadCellData{};
}
CurrentData Sensors::getUmbilicalCurrent()
@@ -207,13 +217,19 @@ VoltageData Sensors::getMotorBatteryVoltage()
return VoltageData{};
}
-PressureData Sensors::getCanOxTankTopPressure()
+PressureData Sensors::getCanN2TankPressure()
{
Lock<FastMutex> lock{canMutex};
- return canOxTankBottomPressure;
+ return canN2TankPressure;
}
PressureData Sensors::getCanOxTankBottomPressure()
+{
+ Lock<FastMutex> lock{canMutex};
+ return canOxTankBottomPressure;
+}
+
+PressureData Sensors::getCanOxTankTopPressure()
{
Lock<FastMutex> lock{canMutex};
return canOxTankTopPressure;
@@ -237,18 +253,24 @@ VoltageData Sensors::getCanMotorBatteryVoltage()
return canMotorBatteryVoltage;
}
-void Sensors::setCanOxTankTopPressure(PressureData data)
+void Sensors::setCanOxTankBottomPressure(PressureData data)
{
Lock<FastMutex> lock{canMutex};
canOxTankBottomPressure = data;
}
-void Sensors::setCanOxTankBottomPressure(PressureData data)
+void Sensors::setCanOxTankTopPressure(PressureData data)
{
Lock<FastMutex> lock{canMutex};
canOxTankTopPressure = data;
}
+void Sensors::setCanN2TankPressure(PressureData data)
+{
+ Lock<FastMutex> lock{canMutex};
+ canN2TankPressure = data;
+}
+
void Sensors::setCanCombustionChamberPressure(PressureData data)
{
Lock<FastMutex> lock{canMutex};
@@ -280,7 +302,9 @@ void Sensors::calibrate()
oxVesselStats.add(oxVesselWeight->getLastSample().load);
oxTankStats.add(oxTankWeight->getLastSample().load);
- Thread::sleep(Config::Sensors::LoadCell::CALIBRATE_SAMPLE_PERIOD);
+ Thread::sleep(
+ milliseconds{Config::Sensors::LoadCell::CALIBRATE_SAMPLE_PERIOD}
+ .count());
}
oxVesselWeight->updateOffset(oxVesselStats.getStats().mean);
@@ -297,20 +321,21 @@ std::vector<SensorInfo> Sensors::getSensorInfos()
if (instance) \
infos.push_back(manager->getSensorInfo(instance.get())); \
else \
- infos.push_back(SensorInfo { #name, 0, nullptr, false })
+ infos.push_back(SensorInfo{name, 0ns, nullptr, false})
- PUSH_SENSOR_INFO(adc1, "ADC1");
- PUSH_SENSOR_INFO(adc2, "ADC2");
- PUSH_SENSOR_INFO(tc1, "TC1");
+ PUSH_SENSOR_INFO(adc1, "ADS131M08_1");
+ PUSH_SENSOR_INFO(adc2, "ADS131M08_2");
+ PUSH_SENSOR_INFO(tc1, "MAX31856_1");
PUSH_SENSOR_INFO(internalAdc, "InternalADC");
PUSH_SENSOR_INFO(oxVesselPressure, "OxVesselPressure");
PUSH_SENSOR_INFO(oxFillingPressure, "OxFillingPressure");
PUSH_SENSOR_INFO(n2Vessel1Pressure, "N2Vessel1Pressure");
PUSH_SENSOR_INFO(n2Vessel2Pressure, "N2Vessel2Pressure");
PUSH_SENSOR_INFO(n2FillingPressure, "N2FillingPressure");
- PUSH_SENSOR_INFO(oxTankTopPressure, "OxTankTopPressure");
PUSH_SENSOR_INFO(oxTankBottomPressure, "OxTankBotPressure");
+ PUSH_SENSOR_INFO(n2TankPressure, "N2TankPressure");
PUSH_SENSOR_INFO(oxVesselWeight, "OxVesselWeight");
+ PUSH_SENSOR_INFO(rocketWeight, "RocketWeight");
PUSH_SENSOR_INFO(oxTankWeight, "OxTankWeight");
return infos;
@@ -391,7 +416,7 @@ void Sensors::adc1Init()
.offset = 0,
.gain = 1.0};
- config.channelsConfig[(int)Config::Sensors::ADC_1::OX_TANK_LC_CHANNEL] = {
+ config.channelsConfig[(int)Config::Sensors::ADC_1::ROCKET_LC_CHANNEL] = {
.enabled = true,
.pga = ADS131M08Defs::PGA::PGA_32,
.offset = 0,
@@ -427,14 +452,13 @@ void Sensors::adc2Init()
channel.enabled = false;
// Configure all required channels
- config.channelsConfig[(int)Config::Sensors::ADC_2::OX_TANK_TOP_PT_CHANNEL] =
- {.enabled = true,
- .pga = ADS131M08Defs::PGA::PGA_1,
- .offset = 0,
- .gain = 1.0};
+ config.channelsConfig[(int)Config::Sensors::ADC_2::OX_TANK_PT_CHANNEL] = {
+ .enabled = true,
+ .pga = ADS131M08Defs::PGA::PGA_1,
+ .offset = 0,
+ .gain = 1.0};
- config.channelsConfig[(
- int)Config::Sensors::ADC_2::OX_TANK_BOTTOM_PT_CHANNEL] = {
+ config.channelsConfig[(int)Config::Sensors::ADC_2::N2_TANK_PT_CHANNEL] = {
.enabled = true,
.pga = ADS131M08Defs::PGA::PGA_1,
.offset = 0,
@@ -559,45 +583,44 @@ void Sensors::n2FillingPressureCallback()
sdLogger.log(N2FillingPressureData{getN2FillingPressure()});
}
-void Sensors::oxTankTopPressureInit()
+void Sensors::oxTankBottomPressureInit()
{
- oxTankTopPressure = std::make_unique<TrafagPressureSensor>(
+ oxTankBottomPressure = std::make_unique<TrafagPressureSensor>(
[this]()
{
auto sample = getADC2LastSample();
return sample.getVoltage(
- Config::Sensors::ADC_2::OX_TANK_TOP_PT_CHANNEL);
+ Config::Sensors::ADC_2::OX_TANK_PT_CHANNEL);
},
- Config::Sensors::Trafag::OX_TANK_TOP_SHUNT_RESISTANCE,
- Config::Sensors::Trafag::OX_TANK_TOP_MAX_PRESSURE,
+ Config::Sensors::Trafag::OX_TANK_SHUNT_RESISTANCE,
+ Config::Sensors::Trafag::OX_TANK_MAX_PRESSURE,
Config::Sensors::Trafag::MIN_CURRENT,
Config::Sensors::Trafag::MAX_CURRENT);
}
-void Sensors::oxTankTopPressureCallback()
+void Sensors::oxTankBottomPressureCallback()
{
- sdLogger.log(OxTankTopPressureData{oxTankTopPressure->getLastSample()});
+ sdLogger.log(OxTankPressureData{oxTankBottomPressure->getLastSample()});
}
-void Sensors::oxTankBottomPressureInit()
+void Sensors::n2TankPressureInit()
{
- oxTankBottomPressure = std::make_unique<TrafagPressureSensor>(
+ n2TankPressure = std::make_unique<TrafagPressureSensor>(
[this]()
{
auto sample = getADC2LastSample();
return sample.getVoltage(
- Config::Sensors::ADC_2::OX_TANK_BOTTOM_PT_CHANNEL);
+ Config::Sensors::ADC_2::N2_TANK_PT_CHANNEL);
},
- Config::Sensors::Trafag::OX_TANK_BOTTOM_SHUNT_RESISTANCE,
- Config::Sensors::Trafag::OX_TANK_BOTTOM_MAX_PRESSURE,
+ Config::Sensors::Trafag::N2_TANK_SHUNT_RESISTANCE,
+ Config::Sensors::Trafag::N2_TANK_MAX_PRESSURE,
Config::Sensors::Trafag::MIN_CURRENT,
Config::Sensors::Trafag::MAX_CURRENT);
}
-void Sensors::oxTankBottomPressureCallback()
+void Sensors::n2TankPressureCallback()
{
- sdLogger.log(
- OxTankBottomPressureData{oxTankBottomPressure->getLastSample()});
+ sdLogger.log(N2TankPressureData{n2TankPressure->getLastSample()});
}
void Sensors::oxVesselWeightInit()
@@ -620,19 +643,37 @@ void Sensors::oxVesselWeightCallback()
sdLogger.log(OxVesselWeightData{getOxVesselWeight()});
}
+void Sensors::rocketWeightInit()
+{
+ rocketWeight = std::make_unique<TwoPointAnalogLoadCell>(
+ [this]()
+ {
+ auto sample = getADC1LastSample();
+ return sample.getVoltage(Config::Sensors::ADC_1::ROCKET_LC_CHANNEL);
+ },
+ Config::Sensors::LoadCell::ROCKET_P0_VOLTAGE,
+ Config::Sensors::LoadCell::ROCKET_P0_MASS,
+ Config::Sensors::LoadCell::ROCKET_P1_VOLTAGE,
+ Config::Sensors::LoadCell::ROCKET_P1_MASS);
+}
+
+void Sensors::rocketWeightCallback()
+{
+ sdLogger.log(RocketWeightData{getRocketWeight()});
+}
+
void Sensors::oxTankWeightInit()
{
oxTankWeight = std::make_unique<TwoPointAnalogLoadCell>(
[this]()
{
auto sample = getADC1LastSample();
- return sample.getVoltage(
- Config::Sensors::ADC_1::OX_TANK_LC_CHANNEL);
+ return sample.getVoltage(Config::Sensors::ADC_1::ROCKET_LC_CHANNEL);
},
- Config::Sensors::LoadCell::TANK_P0_VOLTAGE,
- Config::Sensors::LoadCell::TANK_P0_MASS,
- Config::Sensors::LoadCell::TANK_P1_VOLTAGE,
- Config::Sensors::LoadCell::TANK_P1_MASS);
+ Config::Sensors::LoadCell::ROCKET_P0_VOLTAGE,
+ Config::Sensors::LoadCell::ROCKET_P0_MASS,
+ Config::Sensors::LoadCell::ROCKET_P1_VOLTAGE,
+ Config::Sensors::LoadCell::ROCKET_P1_MASS);
}
void Sensors::oxTankWeightCallback()
@@ -710,13 +751,6 @@ bool Sensors::sensorManagerInit()
map.emplace(std::make_pair(n2FillingPressure.get(), info));
}
- if (oxTankTopPressure)
- {
- SensorInfo info("OxTankTopPressure", Config::Sensors::ADS131M08::PERIOD,
- [this]() { oxTankTopPressureCallback(); });
- map.emplace(std::make_pair(oxTankTopPressure.get(), info));
- }
-
if (oxTankBottomPressure)
{
SensorInfo info("OxTankBotPressure", Config::Sensors::ADS131M08::PERIOD,
@@ -724,6 +758,13 @@ bool Sensors::sensorManagerInit()
map.emplace(std::make_pair(oxTankBottomPressure.get(), info));
}
+ if (n2TankPressure)
+ {
+ SensorInfo info("N2TankPressure", Config::Sensors::ADS131M08::PERIOD,
+ [this]() { n2TankPressureCallback(); });
+ map.emplace(std::make_pair(n2TankPressure.get(), info));
+ }
+
if (oxVesselWeight)
{
SensorInfo info("OxVesselWeight", Config::Sensors::ADS131M08::PERIOD,
@@ -731,6 +772,13 @@ bool Sensors::sensorManagerInit()
map.emplace(std::make_pair(oxVesselWeight.get(), info));
}
+ if (rocketWeight)
+ {
+ SensorInfo info("RocketWeight", Config::Sensors::ADS131M08::PERIOD,
+ [this]() { rocketWeightCallback(); });
+ map.emplace(std::make_pair(rocketWeight.get(), info));
+ }
+
if (oxTankWeight)
{
SensorInfo info("OxTankWeight", Config::Sensors::ADS131M08::PERIOD,
diff --git a/src/RIGv2/Sensors/Sensors.h b/src/RIGv2/Sensors/Sensors.h
index 06a5ceb1f46d841d454c63717cfd155ad9a94bd5..1c3a2dba87df1e64dbcb8e8f37818a3ff220ec81 100644
--- a/src/RIGv2/Sensors/Sensors.h
+++ b/src/RIGv2/Sensors/Sensors.h
@@ -64,12 +64,13 @@ public:
Boardcore::PressureData getN2Vessel1Pressure();
Boardcore::PressureData getN2Vessel2Pressure();
Boardcore::PressureData getN2FillingPressure();
- Boardcore::PressureData getOxTankTopPressure();
Boardcore::PressureData getOxTankBottomPressure();
+ Boardcore::PressureData getN2TankPressure();
Boardcore::PressureData getCombustionChamberPressure();
Boardcore::TemperatureData getOxTankTemperature();
Boardcore::LoadCellData getOxVesselWeight();
+ Boardcore::LoadCellData getRocketWeight();
Boardcore::LoadCellData getOxTankWeight();
Boardcore::CurrentData getUmbilicalCurrent();
@@ -77,16 +78,18 @@ public:
Boardcore::VoltageData getBatteryVoltage();
Boardcore::VoltageData getMotorBatteryVoltage();
- Boardcore::PressureData getCanOxTankTopPressure();
Boardcore::PressureData getCanOxTankBottomPressure();
+ Boardcore::PressureData getCanOxTankTopPressure();
+ Boardcore::PressureData getCanN2TankPressure();
Boardcore::PressureData getCanCombustionChamberPressure();
Boardcore::TemperatureData getCanTankTemperature();
Boardcore::VoltageData getCanMotorBatteryVoltage();
std::vector<Boardcore::SensorInfo> getSensorInfos();
- void setCanOxTankTopPressure(Boardcore::PressureData data);
void setCanOxTankBottomPressure(Boardcore::PressureData data);
+ void setCanOxTankTopPressure(Boardcore::PressureData data);
+ void setCanN2TankPressure(Boardcore::PressureData data);
void setCanCombustionChamberPressure(Boardcore::PressureData data);
void setCanOxTankTemperature(Boardcore::TemperatureData data);
void setCanMotorBatteryVoltage(Boardcore::VoltageData data);
@@ -108,15 +111,18 @@ private:
void n2FillingPressureInit();
void n2FillingPressureCallback();
- void oxTankTopPressureInit();
- void oxTankTopPressureCallback();
-
void oxTankBottomPressureInit();
void oxTankBottomPressureCallback();
+ void n2TankPressureInit();
+ void n2TankPressureCallback();
+
void oxVesselWeightInit();
void oxVesselWeightCallback();
+ void rocketWeightInit();
+ void rocketWeightCallback();
+
void oxTankWeightInit();
void oxTankWeightCallback();
@@ -142,8 +148,9 @@ private:
std::atomic<bool> useCanData{false};
miosix::FastMutex canMutex;
- Boardcore::PressureData canOxTankTopPressure;
Boardcore::PressureData canOxTankBottomPressure;
+ Boardcore::PressureData canOxTankTopPressure;
+ Boardcore::PressureData canN2TankPressure;
Boardcore::PressureData canCombustionChamberPressure;
// TODO: N2 tank pressure from CAN
Boardcore::TemperatureData canOxTankTemperature;
@@ -155,9 +162,10 @@ private:
std::unique_ptr<Boardcore::TrafagPressureSensor> n2Vessel1Pressure;
std::unique_ptr<Boardcore::TrafagPressureSensor> n2Vessel2Pressure;
std::unique_ptr<Boardcore::TrafagPressureSensor> n2FillingPressure;
- std::unique_ptr<Boardcore::TrafagPressureSensor> oxTankTopPressure;
std::unique_ptr<Boardcore::TrafagPressureSensor> oxTankBottomPressure;
+ std::unique_ptr<Boardcore::TrafagPressureSensor> n2TankPressure;
std::unique_ptr<Boardcore::TwoPointAnalogLoadCell> oxVesselWeight;
+ std::unique_ptr<Boardcore::TwoPointAnalogLoadCell> rocketWeight;
std::unique_ptr<Boardcore::TwoPointAnalogLoadCell> oxTankWeight;
// Digital sensors
diff --git a/src/RIGv2/Sensors/SensorsData.h b/src/RIGv2/Sensors/SensorsData.h
index c598c0a2a0f9d87e8fc19f7e0df682450a0a194b..ad36d2177c828ca97d50d0856eaf92c3f5032216 100644
--- a/src/RIGv2/Sensors/SensorsData.h
+++ b/src/RIGv2/Sensors/SensorsData.h
@@ -69,6 +69,16 @@ struct OxVesselWeightData : Boardcore::LoadCellData
OxVesselWeightData() {}
};
+struct RocketWeightData : Boardcore::LoadCellData
+{
+ explicit RocketWeightData(const Boardcore::LoadCellData& data)
+ : Boardcore::LoadCellData(data)
+ {
+ }
+
+ RocketWeightData() {}
+};
+
struct OxTankWeightData : Boardcore::LoadCellData
{
explicit OxTankWeightData(const Boardcore::LoadCellData& data)
@@ -129,24 +139,24 @@ struct N2FillingPressureData : Boardcore::PressureData
N2FillingPressureData() {}
};
-struct OxTankTopPressureData : Boardcore::PressureData
+struct OxTankPressureData : Boardcore::PressureData
{
- explicit OxTankTopPressureData(const Boardcore::PressureData& data)
+ explicit OxTankPressureData(const Boardcore::PressureData& data)
: Boardcore::PressureData(data)
{
}
- OxTankTopPressureData() {}
+ OxTankPressureData() {}
};
-struct OxTankBottomPressureData : Boardcore::PressureData
+struct N2TankPressureData : Boardcore::PressureData
{
- explicit OxTankBottomPressureData(const Boardcore::PressureData& data)
+ explicit N2TankPressureData(const Boardcore::PressureData& data)
: Boardcore::PressureData(data)
{
}
- OxTankBottomPressureData() {}
+ N2TankPressureData() {}
};
} // namespace RIGv2
diff --git a/src/RIGv2/rig-v2-adc-test.cpp b/src/RIGv2/rig-v2-adc-test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5dda0ffe7cbf0b5b609531a14a9f65eb94deb90a
--- /dev/null
+++ b/src/RIGv2/rig-v2-adc-test.cpp
@@ -0,0 +1,275 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <RIGv2/Actuators/Actuators.h>
+#include <RIGv2/BoardScheduler.h>
+#include <RIGv2/Buses.h>
+#include <RIGv2/CanHandler/CanHandler.h>
+#include <RIGv2/Radio/Radio.h>
+#include <RIGv2/Registry/Registry.h>
+#include <RIGv2/Sensors/Sensors.h>
+#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <RIGv2/StateMachines/TARS1/TARS1.h>
+#include <common/Events.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <diagnostic/StackLogger.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <events/EventData.h>
+#include <events/utils/EventSniffer.h>
+
+#include <chrono>
+#include <iomanip>
+#include <iostream>
+#include <thread>
+
+using namespace std::chrono;
+using namespace Boardcore;
+using namespace Common;
+using namespace RIGv2;
+using namespace miosix;
+
+int main()
+{
+ DependencyManager manager;
+
+ Buses* buses = new Buses();
+ BoardScheduler* scheduler = new BoardScheduler();
+
+ Sensors* sensors = new Sensors();
+ Actuators* actuators = new Actuators();
+ Registry* registry = new Registry();
+ CanHandler* canHandler = new CanHandler();
+ GroundModeManager* gmm = new GroundModeManager();
+ TARS1* tars1 = new TARS1();
+ Radio* radio = new Radio();
+
+ Logger& sdLogger = Logger::getInstance();
+ EventBroker& broker = EventBroker::getInstance();
+
+ // Setup event sniffer
+ EventSniffer sniffer(broker,
+ [&](uint8_t event, uint8_t topic)
+ {
+ EventData data{TimestampTimer::getTimestamp(),
+ event, topic};
+ sdLogger.log(data);
+ });
+
+ // Insert modules
+ bool initResult = manager.insert<Buses>(buses) &&
+ manager.insert<BoardScheduler>(scheduler) &&
+ manager.insert<Actuators>(actuators) &&
+ manager.insert<Sensors>(sensors) &&
+ manager.insert<Radio>(radio) &&
+ manager.insert<CanHandler>(canHandler) &&
+ manager.insert<Registry>(registry) &&
+ manager.insert<GroundModeManager>(gmm) &&
+ manager.insert<TARS1>(tars1) && manager.inject();
+
+ if (!initResult)
+ {
+ std::cout << "Failed to inject dependencies" << std::endl;
+ return 0;
+ }
+
+ // Status led indicators
+ // led1: Sensors ok
+ // led2: Radio ok
+ // led3: CanBus ok
+ // led4: Everything ok
+
+ // Start modules
+ std::cout << "Starting EventBroker" << std::endl;
+ if (!broker.start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start EventBroker ***" << std::endl;
+ }
+
+ std::cout << "Starting Registry" << std::endl;
+ if (!registry->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start Registry ***" << std::endl;
+ }
+
+ // Perform an initial registry load
+ std::cout << "Loading backed registry" << std::endl;
+ if (registry->load() != RegistryError::OK)
+ std::cout << "* Warning: could not load a saved registry *"
+ << std::endl;
+
+ std::cout << "Starting Actuators" << std::endl;
+ if (!actuators->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start Actuators ***" << std::endl;
+ }
+
+ std::cout << "Starting Sensors" << std::endl;
+ if (!sensors->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start Sensors ***" << std::endl;
+ }
+ else
+ {
+ led1On();
+ }
+
+ std::cout << "Starting Radio" << std::endl;
+ if (!radio->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start Radio ***" << std::endl;
+ }
+ else
+ {
+ led2On();
+ }
+
+ std::cout << "Starting CanHandler" << std::endl;
+ if (!canHandler->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start CanHandler ***" << std::endl;
+ }
+ else
+ {
+ led3On();
+ }
+
+ std::cout << "Starting GroundModeManager" << std::endl;
+ if (!gmm->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start GroundModeManager ***" << std::endl;
+ }
+
+ std::cout << "Starting TARS1" << std::endl;
+ if (!tars1->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start TARS1 ***" << std::endl;
+ }
+
+ std::cout << "Starting BoardScheduler" << std::endl;
+ if (!scheduler->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start BoardScheduler ***" << std::endl;
+ }
+
+ // Start logging when system boots
+ std::cout << "Starting Logger" << std::endl;
+ if (!sdLogger.start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start Logger ***" << std::endl;
+ }
+ else
+ {
+ sdLogger.resetStats();
+ std::cout << "Logger Ok!\n"
+ << "\tLog number: " << sdLogger.getStats().logNumber
+ << std::endl;
+ }
+
+ if (initResult)
+ {
+ broker.post(FMM_INIT_OK, TOPIC_MOTOR);
+ std::cout << "All good!" << std::endl;
+ led4On();
+ }
+ else
+ {
+ broker.post(FMM_INIT_ERROR, TOPIC_MOTOR);
+ std::cout << "*** Init failure ***" << std::endl;
+ }
+
+ std::cout << "Sensor status:" << std::endl;
+ for (auto info : sensors->getSensorInfos())
+ {
+ // The period being 0 means the sensor is disabled
+ auto statusStr = info.period == 0ns ? "Disabled"
+ : info.isInitialized ? "Ok"
+ : "Error";
+
+ std::cout << "\t" << std::setw(20) << std::left << info.id << " "
+ << statusStr << std::endl;
+ }
+
+ ADS131M08Data avg{};
+ size_t count = 0;
+
+ std::atomic<bool> reset{false};
+
+ std::thread inputHandler = std::thread(
+ [&]()
+ {
+ while (true)
+ {
+ char command{};
+ std::cin >> command;
+
+ if (command == 'r')
+ reset = true;
+ }
+ });
+
+ while (true)
+ {
+ Thread::sleep(10);
+
+ // Choose the ADC to sample here
+ auto sample = sensors->getADC1LastSample();
+
+ if (reset == true)
+ {
+ avg = {};
+ count = 0;
+ reset = false;
+ std::cout << "*** Resetting moving average ***\n";
+ }
+
+ // Perform moving average
+ count++;
+ for (int i = 0; i < 8; i++)
+ avg.voltage[i] += (sample.voltage[i] - avg.voltage[i]) / count;
+
+ if (count % 100 != 0)
+ continue;
+
+ std::cout << std::setw(7) << count;
+ for (int i = 0; i < 8; i++)
+ {
+ auto millivolts = avg.voltage[i] * 1000.0f;
+
+ std::cout << "| CH " << i << ": " << std::setw(10) << std::fixed
+ << millivolts << "mV ";
+ }
+ std::cout << '\n';
+ }
+
+ return 0;
+}
diff --git a/src/RIGv2/rig-v2-entry.cpp b/src/RIGv2/rig-v2-entry.cpp
index b8ec84e95f5c374fc1cbf0caed1bce28ce6375f1..6548c191f8c6863add690115be2c3471a51d389e 100644
--- a/src/RIGv2/rig-v2-entry.cpp
+++ b/src/RIGv2/rig-v2-entry.cpp
@@ -37,9 +37,11 @@
#include <events/EventData.h>
#include <events/utils/EventSniffer.h>
+#include <chrono>
#include <iomanip>
#include <iostream>
+using namespace std::chrono;
using namespace Boardcore;
using namespace Common;
using namespace RIGv2;
@@ -83,8 +85,6 @@ int main()
manager.insert<GroundModeManager>(gmm) &&
manager.insert<TARS1>(tars1) && manager.inject();
- manager.graphviz(std::cout);
-
if (!initResult)
{
std::cout << "Failed to inject dependencies" << std::endl;
@@ -115,7 +115,8 @@ int main()
// Perform an initial registry load
std::cout << "Loading backed registry" << std::endl;
if (registry->load() != RegistryError::OK)
- std::cout << "*** Failed to load backed registry ***" << std::endl;
+ std::cout << "* Warning: could not load a saved registry *"
+ << std::endl;
std::cout << "Starting Actuators" << std::endl;
if (!actuators->start())
@@ -208,11 +209,12 @@ int main()
std::cout << "Sensor status:" << std::endl;
for (auto info : sensors->getSensorInfos())
{
- auto statusStr = !info.isEnabled ? "Disabled"
+ // The period being 0 means the sensor is disabled
+ auto statusStr = info.period == 0ns ? "Disabled"
: info.isInitialized ? "Ok"
: "Error";
- std::cout << "\t" << std::setw(16) << std::left << info.id << " "
+ std::cout << "\t" << std::setw(20) << std::left << info.id << " "
<< statusStr << std::endl;
}