diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index cbdccca2a01c990308245275350930a16d9b4c34..ac0c876bc981f542ded4bf4bcef87ce48af68af8 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -72,6 +72,7 @@ set(RIG_V2_COMPUTER
src/boards/RIGv2/Radio/Radio.cpp
src/boards/RIGv2/Sensors/Sensors.cpp
src/boards/RIGv2/Actuators/Actuators.cpp
+ src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
)
set(CON_RIG_COMPUTER
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index 9e98aa18cc48cb33e8b5845e40f777e1d3ebcfa1..ddf68abd5e4f956b3af3337987a4269484a0fdf0 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -25,6 +25,8 @@
#include <RIGv2/Actuators/Actuators.h>
#include <RIGv2/Buses.h>
#include <RIGv2/Sensors/Sensors.h>
+#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <common/Events.h>
#include <common/Radio.h>
#include <events/EventBroker.h>
#include <radio/SX1278/SX1278Frontends.h>
@@ -196,7 +198,8 @@ void Radio::handleMessage(const mavlink_message_t& msg)
ServosList servo = static_cast<ServosList>(
mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
- if (modules.get<Actuators>()->wiggleServo(servo))
+ if (modules.get<GroundModeManager>()->isDisarmed() &&
+ modules.get<Actuators>()->wiggleServo(servo))
{
sendAck(msg);
}
@@ -288,7 +291,14 @@ void Radio::handleCommand(const mavlink_message_t& msg)
case MAV_CMD_CALIBRATE:
{
- modules.get<Sensors>()->calibrate();
+ EventBroker::getInstance().post(TMTC_CALIBRATE, TOPIC_MOTOR);
+ sendAck(msg);
+ break;
+ }
+
+ case MAV_CMD_FORCE_INIT:
+ {
+ EventBroker::getInstance().post(TMTC_FORCE_INIT, TOPIC_MOTOR);
sendAck(msg);
break;
}
@@ -312,11 +322,11 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
mavlink_sys_tm_t tm;
tm.timestamp = TimestampTimer::getTimestamp();
- tm.logger = Logger::getInstance().isStarted();
- tm.event_broker = EventBroker::getInstance().isRunning();
+ tm.logger = Logger::getInstance().isStarted() ? 1 : 0;
+ tm.event_broker = EventBroker::getInstance().isRunning() ? 1 : 0;
// What? Why is this here? Of course the radio is started!
- tm.radio = isStarted();
- tm.sensors = modules.get<Sensors>()->isStarted();
+ tm.radio = isStarted() ? 1 : 0;
+ tm.sensors = modules.get<Sensors>()->isStarted() ? 1 : 0;
tm.board_scheduler = 0; // TODO(davide.mor): No BoardScheduler yet
mavlink_msg_sys_tm_encode(Config::Radio::MAV_SYSTEM_ID,
@@ -395,6 +405,8 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
actuators->isServoOpen(ServosList::RELEASE_VALVE) ? 1 : 0;
tm.main_valve_state =
actuators->isServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
+ tm.arming_state = modules.get<GroundModeManager>()->isArmed();
+ tm.ignition_state = modules.get<GroundModeManager>()->isIgniting();
// TODO(davide.mor): Add the rest of these
mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID,
@@ -458,7 +470,9 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
if (oldConrigState.arm_switch == 0 && state.arm_switch == 1)
{
// The ARM switch was pressed
- // TODO(davide.mor): Arm the system
+ // TODO(davide.mor): Notify everybody of a manual actuation
+
+ EventBroker::getInstance().post(TMTC_ARM, TOPIC_MOTOR);
lastManualActuation = currentTime;
}
@@ -519,7 +533,9 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
// Special case for disarming, that can be done bypassing the timeout
if (oldConrigState.arm_switch == 1 && state.arm_switch == 0)
{
- // TODO(davide.mor): Disarm the system
+ // TODO(davide.mor): Notify everybody of a manual actuation
+
+ EventBroker::getInstance().post(TMTC_DISARM, TOPIC_MOTOR);
lastManualActuation = currentTime;
}
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a3d6944797961734f90fa9d2726aa2fe59bc721
--- /dev/null
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
@@ -0,0 +1,150 @@
+/* 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 "GroundModeManager.h"
+
+#include <RIGv2/Sensors/Sensors.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+// TODO(davide.mor): Remove TimestampTimer
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+using namespace miosix;
+using namespace Common;
+using namespace RIGv2;
+
+GroundModeManager::GroundModeManager() : FSM(&GroundModeManager::state_idle)
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
+}
+
+bool GroundModeManager::isArmed()
+{
+ return testState(&GroundModeManager::state_armed);
+}
+bool GroundModeManager::isDisarmed()
+{
+ return testState(&GroundModeManager::state_disarmed);
+}
+bool GroundModeManager::isIgniting()
+{
+ return testState(&GroundModeManager::state_igniting);
+}
+
+void GroundModeManager::state_idle(const Boardcore::Event &event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(GroundModeManagerState::STATE_IDLE);
+ break;
+ }
+
+ case FMM_INIT_ERROR:
+ {
+ transition(&GroundModeManager::state_init_err);
+ break;
+ }
+
+ case FMM_INIT_OK:
+ {
+ transition(&GroundModeManager::state_disarmed);
+ break;
+ }
+ }
+}
+
+void GroundModeManager::state_init_err(const Boardcore::Event &event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(GroundModeManagerState::STATE_INIT_ERR);
+ break;
+ }
+
+ case TMTC_FORCE_INIT:
+ {
+ transition(&GroundModeManager::state_disarmed);
+ break;
+ }
+ }
+}
+
+void GroundModeManager::state_disarmed(const Boardcore::Event &event)
+{
+ ModuleManager &modules = ModuleManager::getInstance();
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(GroundModeManagerState::STATE_DISARMED);
+ break;
+ }
+
+ case TMTC_ARM:
+ {
+ transition(&GroundModeManager::state_armed);
+ break;
+ }
+
+ case TMTC_CALIBRATE:
+ {
+ modules.get<Sensors>()->calibrate();
+
+ // TODO(davide.mor): Also send CAN command
+ break;
+ }
+ }
+}
+
+void GroundModeManager::state_armed(const Boardcore::Event &event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(GroundModeManagerState::STATE_ARMED);
+ break;
+ }
+
+ case TMTC_DISARM:
+ {
+ transition(&GroundModeManager::state_disarmed);
+ break;
+ }
+ }
+}
+
+void GroundModeManager::state_igniting(const Boardcore::Event &event)
+{
+ // TODO(davide.mor): Not yet implemented
+}
+
+void GroundModeManager::logStatus(GroundModeManagerState newState)
+{
+ GroundModeManagerData data = {TimestampTimer::getTimestamp(), newState};
+ sdLogger.log(data);
+}
\ No newline at end of file
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..68ecb4855898109747d36399e1d67246b6336b47
--- /dev/null
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
@@ -0,0 +1,59 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <logger/Logger.h>
+#include <atomic>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIGv2
+{
+
+class GroundModeManager : public Boardcore::Module,
+ public Boardcore::FSM<GroundModeManager>
+{
+public:
+ GroundModeManager();
+
+ bool isArmed();
+ bool isDisarmed();
+ bool isIgniting();
+
+private:
+ void state_idle(const Boardcore::Event &event);
+ void state_init_err(const Boardcore::Event &event);
+ void state_disarmed(const Boardcore::Event &event);
+ void state_armed(const Boardcore::Event &event);
+ void state_igniting(const Boardcore::Event &event);
+
+ void logStatus(GroundModeManagerState newState);
+
+ Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
+};
+
+} // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb83cd754fc9e244046c992cf18cde3702d24621
--- /dev/null
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
@@ -0,0 +1,65 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <iostream>
+#include <string>
+
+namespace RIGv2
+{
+
+enum GroundModeManagerState : uint8_t
+{
+ UNINIT = 0,
+ STATE_IDLE = 1,
+ STATE_INIT_ERR = 2,
+ STATE_DISARMED = 3,
+ STATE_ARMED = 4,
+ STATE_IGNITING = 5,
+};
+
+struct GroundModeManagerData
+{
+ uint64_t timestamp;
+ GroundModeManagerState state;
+
+ GroundModeManagerData()
+ : timestamp{0}, state{GroundModeManagerState::UNINIT}
+ {
+ }
+
+ GroundModeManagerData(uint64_t timestamp, GroundModeManagerState state)
+ : timestamp{timestamp}, state{state}
+ {
+ }
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)state << "\n";
+ }
+};
+
+} // namespace RIGv2
\ No newline at end of file
diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp
index 15f02567cdcbab1578b2fe93084f5689d18afc48..0becedcc2d5ec66a9a747f121d6625dc6b4aafb8 100644
--- a/src/entrypoints/RIGv2/rig-v2-entry.cpp
+++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp
@@ -24,10 +24,14 @@
#include <RIGv2/Buses.h>
#include <RIGv2/Radio/Radio.h>
#include <RIGv2/Sensors/Sensors.h>
+#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <common/Events.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
#include <diagnostic/StackLogger.h>
+#include <events/EventBroker.h>
using namespace Boardcore;
+using namespace Common;
using namespace RIGv2;
using namespace miosix;
@@ -41,12 +45,14 @@ int main()
TaskScheduler *scheduler1 = new TaskScheduler(3);
TaskScheduler *scheduler2 = new TaskScheduler(4);
- Buses *buses = new Buses();
- Sensors *sensors = new Sensors(*scheduler1);
- Actuators *actuators = new Actuators(*scheduler2);
- Radio *radio = new Radio();
+ Buses *buses = new Buses();
+ Sensors *sensors = new Sensors(*scheduler1);
+ Actuators *actuators = new Actuators(*scheduler2);
+ GroundModeManager *gmm = new GroundModeManager();
+ Radio *radio = new Radio();
- Logger &sdLogger = Logger::getInstance();
+ Logger &sdLogger = Logger::getInstance();
+ EventBroker &broker = EventBroker::getInstance();
bool initResult = true;
@@ -75,7 +81,25 @@ int main()
LOG_ERR(logger, "Error failed to insert Radio");
}
+ if (!modules.insert<GroundModeManager>(gmm))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error failed to insert GroundModeManager");
+ }
+
// Start modules
+ if (sdLogger.testSDCard())
+ {
+ initResult = false;
+ LOG_ERR(logger, "SD card test failed");
+ }
+
+ if (broker.start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Failed to start EventBroker");
+ }
+
if (!sensors->start())
{
initResult = false;
@@ -94,17 +118,28 @@ int main()
LOG_ERR(logger, "Error failed to start Radio module");
}
+ if (!gmm->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error failed to start GroundModeManager module");
+ }
+
if (!scheduler1->start() || !scheduler2->start())
{
initResult = false;
LOG_ERR(logger, "Error failed to start scheduler");
}
- if (!initResult)
+ if (initResult)
{
- // TODO(davide.mor): What to do in case of not good?
+ broker.post(FMM_INIT_OK, TOPIC_MOTOR);
LOG_INFO(logger, "All good!");
}
+ else
+ {
+ broker.post(FMM_INIT_ERROR, TOPIC_MOTOR);
+ LOG_ERR(logger, "Init failure!");
+ }
// Periodic statistics
while (true)
diff --git a/src/scripts/logdecoder/RIGv2/logdecoder.cpp b/src/scripts/logdecoder/RIGv2/logdecoder.cpp
index eab10f51c11e5b25893a3eeb31029ae54057ad06..75bbe6dc64885e7e6a9c6dd99a6212a1dae4fc6f 100644
--- a/src/scripts/logdecoder/RIGv2/logdecoder.cpp
+++ b/src/scripts/logdecoder/RIGv2/logdecoder.cpp
@@ -22,6 +22,7 @@
#include <RIGv2/Sensors/SensorsData.h>
#include <RIGv2/Actuators/ActuatorsData.h>
+#include <RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h>
#include <logger/Deserializer.h>
#include <logger/LogTypes.h>
#include <tscpp/stream.h>
@@ -54,6 +55,7 @@ void registerTypes(Deserializer& ds)
ds.registerType<ADCsData>();
ds.registerType<TCsData>();
ds.registerType<ActuatorsData>();
+ ds.registerType<GroundModeManagerData>();
}
void showUsage(const string& cmdName)