From 93b7ff2416e2581285c8c3927703969f9dd85709 Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Mon, 5 Aug 2024 15:09:49 +0200
Subject: [PATCH] [Main][HIL] Included the persistent hil flag set by
telecommands
---
cmake/dependencies.cmake | 1 +
src/boards/Main/CanHandler/CanHandler.cpp | 2 +-
src/boards/Main/CanHandler/CanHandler.h | 3 +-
.../Main/PersistentVars/PersistentVars.cpp | 42 ++++++
.../Main/PersistentVars/PersistentVars.h | 44 ++++++
src/boards/Main/Radio/Radio.cpp | 3 +-
src/boards/Main/Radio/Radio.h | 9 +-
.../FlightModeManager/FlightModeManager.cpp | 30 ++++
.../FlightModeManager/FlightModeManager.h | 4 +-
src/entrypoints/Main/main-entry.cpp | 133 +++++++++---------
10 files changed, 197 insertions(+), 74 deletions(-)
create mode 100644 src/boards/Main/PersistentVars/PersistentVars.cpp
create mode 100644 src/boards/Main/PersistentVars/PersistentVars.h
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 7ddbe6d3c..97b489959 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -26,6 +26,7 @@ set(OBSW_INCLUDE_DIRS
set(MAIN_COMPUTER
src/boards/Main/Data/ABKTrajectorySet.cpp
+ src/boards/Main/PersistentVars/PersistentVars.cpp
src/boards/Main/Sensors/Sensors.cpp
src/boards/Main/AlgoReference/AlgoReference.cpp
src/boards/Main/Radio/Radio.cpp
diff --git a/src/boards/Main/CanHandler/CanHandler.cpp b/src/boards/Main/CanHandler/CanHandler.cpp
index 4280c1326..51b124659 100644
--- a/src/boards/Main/CanHandler/CanHandler.cpp
+++ b/src/boards/Main/CanHandler/CanHandler.cpp
@@ -73,7 +73,7 @@ bool CanHandler::start()
static_cast<int16_t>(stats.logNumber),
static_cast<uint8_t>(state),
state == FlightModeManagerState::ARMED,
- false, // TODO: HIL
+ getModule<PersistentVars>()->getHilMode(),
stats.lastWriteError == 0,
});
},
diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main/CanHandler/CanHandler.h
index ef649514f..5730c0b2a 100644
--- a/src/boards/Main/CanHandler/CanHandler.h
+++ b/src/boards/Main/CanHandler/CanHandler.h
@@ -24,6 +24,7 @@
#include <Main/BoardScheduler.h>
#include <Main/Configs/CanHandlerConfig.h>
+#include <Main/PersistentVars/PersistentVars.h>
#include <Main/Sensors/Sensors.h>
#include <common/CanConfig.h>
#include <common/Mavlink.h>
@@ -38,7 +39,7 @@ class Actuators;
class CanHandler
: public Boardcore::InjectableWithDeps<BoardScheduler, Actuators, Sensors,
- FlightModeManager>
+ FlightModeManager, PersistentVars>
{
public:
struct CanStatus
diff --git a/src/boards/Main/PersistentVars/PersistentVars.cpp b/src/boards/Main/PersistentVars/PersistentVars.cpp
new file mode 100644
index 000000000..c07ec5df6
--- /dev/null
+++ b/src/boards/Main/PersistentVars/PersistentVars.cpp
@@ -0,0 +1,42 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 "PersistentVars.h"
+
+using namespace miosix;
+
+namespace Main
+{
+
+bool PRESERVE PersistentVars::hilMode = false;
+
+PersistentVars::PersistentVars() {}
+
+void PersistentVars::setHilMode(bool _hilMode)
+{
+ BSRAM::EnableWriteLock l;
+ hilMode = _hilMode;
+}
+
+bool PersistentVars::getHilMode() { return hilMode; }
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/PersistentVars/PersistentVars.h b/src/boards/Main/PersistentVars/PersistentVars.h
new file mode 100644
index 000000000..937aaf78a
--- /dev/null
+++ b/src/boards/Main/PersistentVars/PersistentVars.h
@@ -0,0 +1,44 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 <arch/common/drivers/stm32_bsram.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Main
+{
+
+class PersistentVars : public Boardcore::Injectable
+{
+public:
+ PersistentVars();
+
+ void setHilMode(bool _hilMode);
+
+ bool getHilMode();
+
+private:
+ static bool PRESERVE hilMode;
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index ad5f828d6..661723b09 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -22,6 +22,7 @@
#include "Radio.h"
+#include <Main/PersistentVars/PersistentVars.h>
#include <common/Events.h>
#include <common/Radio.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
@@ -739,7 +740,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.motor_can_status = canStatus.isMotorConnected() ? 1 : 0;
tm.rig_can_status = canStatus.isRigConnected() ? 1 : 0;
- tm.hil_state = 0; // TODO
+ tm.hil_state = getModule<PersistentVars>()->getHilMode() ? 1 : 0;
mavlink_msg_rocket_stats_tm_encode(Config::Radio::MAV_SYSTEM_ID,
Config::Radio::MAV_COMPONENT_ID,
diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h
index fff2e20e8..3702162be 100644
--- a/src/boards/Main/Radio/Radio.h
+++ b/src/boards/Main/Radio/Radio.h
@@ -47,10 +47,11 @@ using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
Config::Radio::MAV_OUT_QUEUE_SIZE,
Config::Radio::MAV_MAX_LENGTH>;
-class Radio : public Boardcore::InjectableWithDeps<
- Buses, BoardScheduler, Actuators, PinHandler, CanHandler,
- Sensors, FlightModeManager, ADAController, NASController,
- MEAController, ABKController, StatsRecorder, AlgoReference>
+class Radio
+ : public Boardcore::InjectableWithDeps<
+ Buses, BoardScheduler, Actuators, PinHandler, CanHandler, Sensors,
+ FlightModeManager, ADAController, NASController, MEAController,
+ ABKController, StatsRecorder, AlgoReference, PersistentVars>
{
public:
Radio() {}
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index bc00b2aef..1fdc12788 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -73,6 +73,23 @@ State FlightModeManager::state_on_ground(const Event& event)
reboot();
return HANDLED;
}
+ case TMTC_EXIT_HIL_MODE:
+ {
+ getModule<CanHandler>()->sendEvent(
+ CanConfig::EventId::EXIT_HIL_MODE);
+ miosix::Thread::sleep(1000);
+ // Fallthrough
+ }
+ case CAN_EXIT_HIL_MODE:
+ {
+ // Reboot only if in HIL mode
+ if (getModule<PersistentVars>()->getHilMode())
+ {
+ getModule<PersistentVars>()->setHilMode(false);
+ miosix::reboot();
+ }
+ return HANDLED;
+ }
default:
{
return UNHANDLED;
@@ -411,6 +428,19 @@ State FlightModeManager::state_test_mode(const Event& event)
CanConfig::EventId::EXIT_TEST_MODE);
return transition(&FlightModeManager::state_disarmed);
}
+ case TMTC_ENTER_HIL_MODE:
+ {
+ getModule<CanHandler>()->sendEvent(
+ CanConfig::EventId::ENTER_HIL_MODE);
+ Thread::sleep(1000);
+ // Fallthrough
+ }
+ case CAN_ENTER_HIL_MODE:
+ {
+ getModule<PersistentVars>()->setHilMode(true);
+ reboot();
+ __builtin_unreachable();
+ }
default:
{
return UNHANDLED;
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index 1690320ef..409645cc8 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -25,6 +25,7 @@
#include <Main/Actuators/Actuators.h>
#include <Main/AlgoReference/AlgoReference.h>
#include <Main/CanHandler/CanHandler.h>
+#include <Main/PersistentVars/PersistentVars.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StatsRecorder/StatsRecorder.h>
#include <events/EventBroker.h>
@@ -38,7 +39,8 @@ namespace Main
class FlightModeManager
: public Boardcore::InjectableWithDeps<Actuators, Sensors, CanHandler,
- StatsRecorder, AlgoReference>,
+ StatsRecorder, AlgoReference,
+ PersistentVars>,
public Boardcore::HSM<FlightModeManager>
{
public:
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index 784455b1a..f4c419299 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -26,6 +26,7 @@
#include <Main/Buses.h>
#include <Main/CanHandler/CanHandler.h>
#include <Main/Configs/HILSimulationConfig.h>
+#include <Main/PersistentVars/PersistentVars.h>
#include <Main/PinHandler/PinHandler.h>
#include <Main/Radio/Radio.h>
#include <Main/Sensors/HILSensors.h>
@@ -53,8 +54,6 @@ using namespace Boardcore;
using namespace Main;
using namespace Common;
-constexpr bool hilSimulationActive = true;
-
int main()
{
ledOff();
@@ -63,8 +62,9 @@ int main()
bool initResult = true;
- Buses *buses = new Buses();
- BoardScheduler *scheduler = new BoardScheduler();
+ PersistentVars *persistentVars = new PersistentVars();
+ Buses *buses = new Buses();
+ BoardScheduler *scheduler = new BoardScheduler();
Sensors *sensors;
Actuators *actuators = new Actuators();
@@ -81,7 +81,7 @@ int main()
HILConfig::MainHIL *hil = nullptr;
// HIL
- if (hilSimulationActive)
+ if (persistentVars->getHilMode())
{
hil = new HILConfig::MainHIL();
@@ -107,7 +107,8 @@ int main()
});
// Insert modules
- initResult = initResult && manager.insert<Buses>(buses) &&
+ initResult = initResult && manager.insert<PersistentVars>(persistentVars) &&
+ manager.insert<Buses>(buses) &&
manager.insert<BoardScheduler>(scheduler) &&
manager.insert<Sensors>(sensors) &&
manager.insert<Radio>(radio) &&
@@ -136,56 +137,6 @@ int main()
// led3: CanBus ok
// led4: Everything ok
- if (hilSimulationActive)
- {
- printf("starting hil\n");
- hil->start();
-
- if (!HILConfig::IS_FULL_HIL)
- {
- hil->registerToFlightPhase(
- HILConfig::MainFlightPhases::SHUTDOWN, [&]()
- { actuators->setCanServoOpen(ServosList::MAIN_VALVE, false); });
- }
-
- hil->registerToFlightPhase(HILConfig::MainFlightPhases::LIFTOFF,
- [&]()
- {
- printf("liftoff\n");
- if (!HILConfig::IS_FULL_HIL)
- {
- printf("open main valve\n");
-
- // TODO: add can message sending
- actuators->setCanServoOpen(
- ServosList::MAIN_VALVE, true);
- }
- });
-
- hil->registerToFlightPhase(HILConfig::MainFlightPhases::ARMED,
- [&]()
- {
- printf("ARMED\n");
- // Comment this if we want to trigger
- // liftoff by hand
- EventBroker::getInstance().post(
- Events::FLIGHT_LAUNCH_PIN_DETACHED,
- Topics::TOPIC_FLIGHT);
- });
-
- hil->registerToFlightPhase(HILConfig::MainFlightPhases::CALIBRATION_OK,
- [&]()
- {
- TRACE("ARM COMMAND SENT\n");
- EventBroker::getInstance().post(
- Events::TMTC_ARM,
- Topics::TOPIC_TMTC);
- });
-
- printf("Waiting start simulation\n");
- hil->waitStartSimulation();
- }
-
if (!broker.start())
{
initResult = false;
@@ -198,16 +149,6 @@ int main()
std::cout << "Error failed to start Actuators module" << std::endl;
}
- if (!sensors->start())
- {
- initResult = false;
- std::cout << "Error failed to start Sensors module" << std::endl;
- }
- else
- {
- led1On();
- }
-
if (!radio->start())
{
initResult = false;
@@ -283,6 +224,66 @@ int main()
std::cout << "Error failed to start SD" << std::endl;
}
+ if (persistentVars->getHilMode())
+ {
+ printf("starting hil\n");
+ hil->start();
+
+ if (!HILConfig::IS_FULL_HIL)
+ {
+ hil->registerToFlightPhase(
+ HILConfig::MainFlightPhases::SHUTDOWN, [&]()
+ { actuators->setCanServoOpen(ServosList::MAIN_VALVE, false); });
+ }
+
+ hil->registerToFlightPhase(HILConfig::MainFlightPhases::LIFTOFF,
+ [&]()
+ {
+ printf("liftoff\n");
+ if (!HILConfig::IS_FULL_HIL)
+ {
+ printf("open main valve\n");
+
+ // TODO: add can message sending
+ actuators->setCanServoOpen(
+ ServosList::MAIN_VALVE, true);
+ }
+ });
+
+ hil->registerToFlightPhase(HILConfig::MainFlightPhases::ARMED,
+ [&]()
+ {
+ printf("ARMED\n");
+ // Comment this if we want to trigger
+ // liftoff by hand
+ EventBroker::getInstance().post(
+ Events::FLIGHT_LAUNCH_PIN_DETACHED,
+ Topics::TOPIC_FLIGHT);
+ });
+
+ hil->registerToFlightPhase(HILConfig::MainFlightPhases::CALIBRATION_OK,
+ [&]()
+ {
+ TRACE("ARM COMMAND SENT\n");
+ EventBroker::getInstance().post(
+ Events::TMTC_ARM,
+ Topics::TOPIC_TMTC);
+ });
+
+ printf("Waiting start simulation\n");
+ hil->waitStartSimulation();
+ }
+
+ if (!sensors->start())
+ {
+ initResult = false;
+ std::cout << "Error failed to start Sensors module" << std::endl;
+ }
+ else
+ {
+ led1On();
+ }
+
if (initResult)
{
EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
--
GitLab