diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 97b48995973d463e157a5d7bc6b70b6c9eb64702..ab4872b2a0ccc323429989208e81fa317caee663 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -27,6 +27,7 @@ set(OBSW_INCLUDE_DIRS
set(MAIN_COMPUTER
src/boards/Main/Data/ABKTrajectorySet.cpp
src/boards/Main/PersistentVars/PersistentVars.cpp
+ src/boards/Main/HIL/HIL.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 51b1246597d22db78c519eea1975b9e3402e4142..541e0f000a85a474ce278148b4d98d9011696e92 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,
- getModule<PersistentVars>()->getHilMode(),
+ PersistentVars::getHilMode(),
stats.lastWriteError == 0,
});
},
diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main/CanHandler/CanHandler.h
index 5730c0b2ab70ca2ab097fbaef218b4cea8e6392f..60c7c47fef40d960e1cf4f9e3b21e35b8e1103e9 100644
--- a/src/boards/Main/CanHandler/CanHandler.h
+++ b/src/boards/Main/CanHandler/CanHandler.h
@@ -39,7 +39,7 @@ class Actuators;
class CanHandler
: public Boardcore::InjectableWithDeps<BoardScheduler, Actuators, Sensors,
- FlightModeManager, PersistentVars>
+ FlightModeManager>
{
public:
struct CanStatus
diff --git a/src/boards/Main/Configs/HILSimulationConfig.h b/src/boards/Main/Configs/HILSimulationConfig.h
index cbb6d9da138b1caccac66fb32110d0bd92520841..4e66179d1e49106e82886e514ba4232a9da937ff 100644
--- a/src/boards/Main/Configs/HILSimulationConfig.h
+++ b/src/boards/Main/Configs/HILSimulationConfig.h
@@ -36,11 +36,7 @@ namespace Config
namespace HIL
{
-// clang-format off
-// Indent to avoid the linter complaining about using namespace
- using namespace Boardcore::Units::Frequency;
- using namespace std::chrono_literals;
-// clang-format on
+/* linter off */ using namespace Boardcore::Units::Frequency;
constexpr bool IS_FULL_HIL = false;
constexpr bool ENABLE_HW = false;
diff --git a/src/boards/Main/HIL/HIL.cpp b/src/boards/Main/HIL/HIL.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..df612388640501e79055094cd3887cf154a81e6c
--- /dev/null
+++ b/src/boards/Main/HIL/HIL.cpp
@@ -0,0 +1,304 @@
+/* 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 "HIL.h"
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/Buses.h>
+#include <Main/Configs/HILSimulationConfig.h>
+#include <Main/Configs/MEAConfig.h>
+#include <Main/Configs/SensorsConfig.h>
+#include <Main/HIL/HILData.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+#include <hil/HIL.h>
+#include <interfaces-impl/hwmapping.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include "HILData.h"
+
+namespace Main
+{
+
+MainHILPhasesManager::MainHILPhasesManager(
+ std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
+ : Boardcore::HILPhasesManager<MainFlightPhases, SimulatorData,
+ ActuatorData>(getCurrentPosition)
+{
+ flagsFlightPhases = {{MainFlightPhases::SIM_FLYING, false},
+ {MainFlightPhases::SIM_ASCENT, false},
+ {MainFlightPhases::SIM_BURNING, false},
+ {MainFlightPhases::SIM_AEROBRAKES, false},
+ {MainFlightPhases::SIM_PARA1, false},
+ {MainFlightPhases::SIM_PARA2, false},
+ {MainFlightPhases::SIMULATION_STARTED, false},
+ {MainFlightPhases::CALIBRATION, false},
+ {MainFlightPhases::CALIBRATION_OK, false},
+ {MainFlightPhases::ARMED, false},
+ {MainFlightPhases::LIFTOFF_PIN_DETACHED, false},
+ {MainFlightPhases::LIFTOFF, false},
+ {MainFlightPhases::AEROBRAKES, false},
+ {MainFlightPhases::APOGEE, false},
+ {MainFlightPhases::PARA1, false},
+ {MainFlightPhases::PARA2, false},
+ {MainFlightPhases::SIMULATION_STOPPED, false}};
+
+ prev_flagsFlightPhases = flagsFlightPhases;
+
+ // Subscribe to all the topics
+ auto& eventBroker = Boardcore::EventBroker::getInstance();
+ eventBroker.subscribe(this, Common::TOPIC_ABK);
+ eventBroker.subscribe(this, Common::TOPIC_ADA);
+ eventBroker.subscribe(this, Common::TOPIC_MEA);
+ eventBroker.subscribe(this, Common::TOPIC_DPL);
+ eventBroker.subscribe(this, Common::TOPIC_CAN);
+ eventBroker.subscribe(this, Common::TOPIC_FLIGHT);
+ eventBroker.subscribe(this, Common::TOPIC_FMM);
+ eventBroker.subscribe(this, Common::TOPIC_FSR);
+ eventBroker.subscribe(this, Common::TOPIC_NAS);
+ eventBroker.subscribe(this, Common::TOPIC_TMTC);
+ eventBroker.subscribe(this, Common::TOPIC_MOTOR);
+ eventBroker.subscribe(this, Common::TOPIC_TARS);
+ eventBroker.subscribe(this, Common::TOPIC_ALT);
+}
+
+void MainHILPhasesManager::processFlagsImpl(
+ const SimulatorData& simulatorData,
+ std::vector<MainFlightPhases>& changed_flags)
+{
+ if (simulatorData.signal ==
+ static_cast<float>(HILSignal::SIMULATION_STARTED))
+ {
+ miosix::reboot();
+ }
+
+ if (simulatorData.signal ==
+ static_cast<float>(HILSignal::SIMULATION_STOPPED))
+ {
+ Boardcore::EventBroker::getInstance().post(Common::TMTC_FORCE_LANDING,
+ Common::TOPIC_TMTC);
+ }
+
+ if (simulatorData.signal ==
+ static_cast<float>(HILSignal::SIMULATION_FORCE_LAUNCH))
+ {
+ Boardcore::EventBroker::getInstance().post(Common::TMTC_ARM,
+ Common::TOPIC_TMTC);
+ Thread::sleep(250);
+ Boardcore::EventBroker::getInstance().post(Common::TMTC_FORCE_LAUNCH,
+ Common::TOPIC_TMTC);
+ }
+
+ // set true when the first packet from the simulator arrives
+ if (isSetTrue(MainFlightPhases::SIMULATION_STARTED))
+ {
+ t_start = Boardcore::TimestampTimer::getTimestamp();
+
+ printf("[HIL] ------- SIMULATION STARTED ! ------- \n");
+ changed_flags.push_back(MainFlightPhases::SIMULATION_STARTED);
+ }
+
+ if (flagsFlightPhases[MainFlightPhases::SIM_FLYING])
+ {
+ if (isSetTrue(MainFlightPhases::SIM_FLYING))
+ {
+ registerOutcomes(MainFlightPhases::SIM_FLYING);
+ printf("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n");
+ changed_flags.push_back(MainFlightPhases::SIM_FLYING);
+ }
+ }
+}
+
+void MainHILPhasesManager::printOutcomes()
+{
+ printf("OUTCOMES: (times dt from liftoff)\n\n");
+ printf("Simulation time: %.3f [sec]\n\n",
+ (double)(t_stop - t_start) / 1000000.0f);
+
+ printf("Motor stopped burning (simulation flag): \n");
+ outcomes[MainFlightPhases::SIM_BURNING].print(t_liftoff);
+
+ printf("Airbrakes exit shadowmode: \n");
+ outcomes[MainFlightPhases::AEROBRAKES].print(t_liftoff);
+
+ printf("Apogee: \n");
+ outcomes[MainFlightPhases::APOGEE].print(t_liftoff);
+
+ printf("Parachute 1: \n");
+ outcomes[MainFlightPhases::PARA1].print(t_liftoff);
+
+ printf("Parachute 2: \n");
+ outcomes[MainFlightPhases::PARA2].print(t_liftoff);
+
+ printf("Simulation Stopped: \n");
+ outcomes[MainFlightPhases::SIMULATION_STOPPED].print(t_liftoff);
+
+ // auto cpuMeter = Boardcore::CpuMeter::getCpuStats();
+ // printf("max cpu usage: %+.3f\n", cpuMeter.maxValue);
+ // printf("avg cpu usage: %+.3f\n", cpuMeter.mean);
+ // printf("min free heap: %+.3lu\n", cpuMeter.minFreeHeap);
+ // printf("max free stack:%+.3lu\n", cpuMeter.minFreeStack);
+}
+
+void MainHILPhasesManager::handleEventImpl(
+ const Boardcore::Event& e, std::vector<MainFlightPhases>& changed_flags)
+{
+ switch (e)
+ {
+ case Common::Events::FMM_INIT_ERROR:
+ printf("[HIL] ------- INIT FAILED ! ------- \n");
+ case Common::Events::FMM_INIT_OK:
+ setFlagFlightPhase(MainFlightPhases::CALIBRATION, true);
+ printf("[HIL] ------- CALIBRATION ! ------- \n");
+ changed_flags.push_back(MainFlightPhases::CALIBRATION);
+ break;
+ case Common::Events::FLIGHT_DISARMED:
+ setFlagFlightPhase(MainFlightPhases::CALIBRATION_OK, true);
+ printf("[HIL] CALIBRATION OK!\n");
+ changed_flags.push_back(MainFlightPhases::CALIBRATION_OK);
+ break;
+ case Common::Events::FLIGHT_ARMED:
+ setFlagFlightPhase(MainFlightPhases::ARMED, true);
+ printf("[HIL] ------- READY TO LAUNCH ! ------- \n");
+ changed_flags.push_back(MainFlightPhases::ARMED);
+ break;
+ case Common::Events::FLIGHT_LAUNCH_PIN_DETACHED:
+ setFlagFlightPhase(MainFlightPhases::LIFTOFF_PIN_DETACHED, true);
+ printf("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n");
+ changed_flags.push_back(MainFlightPhases::LIFTOFF_PIN_DETACHED);
+ break;
+ case Common::Events::FLIGHT_LIFTOFF:
+ case Common::Events::TMTC_FORCE_LAUNCH:
+ t_liftoff = Boardcore::TimestampTimer::getTimestamp();
+ printf("[HIL] ------- LIFTOFF -------: %f, %f \n",
+ getCurrentPosition().z, getCurrentPosition().vz);
+ changed_flags.push_back(MainFlightPhases::LIFTOFF);
+ break;
+ case Common::Events::FLIGHT_MOTOR_SHUTDOWN:
+ printf("[HIL] ------- SHUTDOWN -------: %f, %f \n",
+ getCurrentPosition().z, getCurrentPosition().vz);
+ changed_flags.push_back(MainFlightPhases::SHUTDOWN);
+ case Common::Events::ABK_SHADOW_MODE_TIMEOUT:
+ setFlagFlightPhase(MainFlightPhases::AEROBRAKES, true);
+ registerOutcomes(MainFlightPhases::AEROBRAKES);
+ printf("[HIL] ABK shadow mode timeout\n");
+ changed_flags.push_back(MainFlightPhases::AEROBRAKES);
+ break;
+ case Common::Events::ADA_SHADOW_MODE_TIMEOUT:
+ printf("[HIL] ADA shadow mode timeout\n");
+ break;
+ case Common::Events::ABK_DISABLE:
+ setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false);
+ printf("[HIL] ABK disabled\n");
+ break;
+ case Common::Events::FLIGHT_APOGEE_DETECTED:
+ case Common::Events::CAN_APOGEE_DETECTED:
+ setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false);
+ registerOutcomes(MainFlightPhases::APOGEE);
+ printf("[HIL] ------- APOGEE DETECTED ! ------- %f, %f \n",
+ getCurrentPosition().z, getCurrentPosition().vz);
+ changed_flags.push_back(MainFlightPhases::APOGEE);
+ break;
+ case Common::Events::FLIGHT_DROGUE_DESCENT:
+ case Common::Events::TMTC_FORCE_EXPULSION:
+ setFlagFlightPhase(MainFlightPhases::PARA1, true);
+ registerOutcomes(MainFlightPhases::PARA1);
+ printf("[HIL] ------- PARA1 ! -------%f, %f \n",
+ getCurrentPosition().z, getCurrentPosition().vz);
+ changed_flags.push_back(MainFlightPhases::PARA1);
+ break;
+ case Common::Events::FLIGHT_WING_DESCENT:
+ case Common::Events::FLIGHT_DPL_ALT_DETECTED:
+ case Common::Events::TMTC_FORCE_DEPLOYMENT:
+ setFlagFlightPhase(MainFlightPhases::PARA1, false);
+ setFlagFlightPhase(MainFlightPhases::PARA2, true);
+ registerOutcomes(MainFlightPhases::PARA2);
+ printf("[HIL] ------- PARA2 ! ------- %f, %f \n",
+ getCurrentPosition().z, getCurrentPosition().vz);
+ changed_flags.push_back(MainFlightPhases::PARA2);
+ break;
+ case Common::Events::FLIGHT_LANDING_DETECTED:
+ case Common::Events::TMTC_FORCE_LANDING:
+ t_stop = Boardcore::TimestampTimer::getTimestamp();
+ setFlagFlightPhase(MainFlightPhases::PARA2, false);
+ setFlagFlightPhase(MainFlightPhases::SIMULATION_STOPPED, true);
+ changed_flags.push_back(MainFlightPhases::SIMULATION_STOPPED);
+ registerOutcomes(MainFlightPhases::SIMULATION_STOPPED);
+ printf("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n",
+ (double)t_stop / 1000000.0f);
+ printOutcomes();
+ break;
+ default:
+ printf("%s event\n", Common::getEventString(e).c_str());
+ }
+}
+
+MainHIL::MainHIL()
+ : Boardcore::HIL<MainFlightPhases, SimulatorData, ActuatorData>(
+ nullptr, nullptr, [this]() { return updateActuatorData(); },
+ 1000 / Config::HIL::SIMULATION_RATE.value())
+{
+}
+
+bool MainHIL::start()
+{
+ auto* nas = getModule<NASController>();
+ auto& hilUsart = getModule<Buses>()->getHILUart();
+
+ hilPhasesManager = new MainHILPhasesManager(
+ [nas]()
+ { return Boardcore::TimedTrajectoryPoint(nas->getNASState()); });
+
+ hilTransceiver = new MainHILTransceiver(hilUsart, hilPhasesManager);
+
+ return Boardcore::HIL<MainFlightPhases, SimulatorData,
+ ActuatorData>::start();
+}
+
+ActuatorData MainHIL::updateActuatorData()
+{
+ auto actuators = getModule<Actuators>();
+
+ ADAStateHIL adaStateHIL{getModule<ADAController>()->getADAState(),
+ getModule<ADAController>()->getState()};
+
+ NASStateHIL nasStateHIL{getModule<NASController>()->getNASState(),
+ getModule<NASController>()->getState()};
+
+ AirBrakesStateHIL abkStateHIL{getModule<ABKController>()->getState()};
+
+ MEAStateHIL meaStateHIL{getModule<MEAController>()->getMEAState(),
+ getModule<MEAController>()->getState()};
+
+ ActuatorsStateHIL actuatorsStateHIL{
+ actuators->getServoPosition(ServosList::AIR_BRAKES_SERVO),
+ actuators->getServoPosition(ServosList::EXPULSION_SERVO),
+ (actuators->isCanServoOpen(ServosList::MAIN_VALVE) ? 1.f : 0.f),
+ (actuators->isCanServoOpen(ServosList::VENTING_VALVE) ? 1.f : 0.f),
+ static_cast<float>(miosix::gpios::mainDeploy::value())};
+
+ // Returning the feedback for the simulator
+ return ActuatorData{adaStateHIL, nasStateHIL, abkStateHIL, meaStateHIL,
+ actuatorsStateHIL};
+}
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/HIL/HIL.h b/src/boards/Main/HIL/HIL.h
index dc6cc12bfb84fd3788a6894aabeb5dd4df6b8ade..bb13a25605f67aface55aa81e324c6ef96f029e9 100644
--- a/src/boards/Main/HIL/HIL.h
+++ b/src/boards/Main/HIL/HIL.h
@@ -25,17 +25,16 @@
#include <Main/Actuators/Actuators.h>
#include <Main/Buses.h>
#include <Main/Configs/HILSimulationConfig.h>
-#include <Main/Configs/MEAConfig.h>
-#include <Main/Configs/SensorsConfig.h>
#include <Main/HIL/HILData.h>
+#include <Main/StateMachines/ABKController/ABKController.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
#include <common/Events.h>
-#include <events/EventBroker.h>
#include <hil/HIL.h>
-#include <interfaces-impl/hwmapping.h>
#include <utils/DependencyManager/DependencyManager.h>
-#include "HILData.h"
-
namespace Main
{
@@ -53,212 +52,17 @@ class MainHILPhasesManager
{
public:
explicit MainHILPhasesManager(
- std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
- : Boardcore::HILPhasesManager<MainFlightPhases, SimulatorData,
- ActuatorData>(getCurrentPosition)
- {
- flagsFlightPhases = {{MainFlightPhases::SIM_FLYING, false},
- {MainFlightPhases::SIM_ASCENT, false},
- {MainFlightPhases::SIM_BURNING, false},
- {MainFlightPhases::SIM_AEROBRAKES, false},
- {MainFlightPhases::SIM_PARA1, false},
- {MainFlightPhases::SIM_PARA2, false},
- {MainFlightPhases::SIMULATION_STARTED, false},
- {MainFlightPhases::CALIBRATION, false},
- {MainFlightPhases::CALIBRATION_OK, false},
- {MainFlightPhases::ARMED, false},
- {MainFlightPhases::LIFTOFF_PIN_DETACHED, false},
- {MainFlightPhases::LIFTOFF, false},
- {MainFlightPhases::AEROBRAKES, false},
- {MainFlightPhases::APOGEE, false},
- {MainFlightPhases::PARA1, false},
- {MainFlightPhases::PARA2, false},
- {MainFlightPhases::SIMULATION_STOPPED, false}};
-
- prev_flagsFlightPhases = flagsFlightPhases;
-
- // Subscribe to all the topics
- auto& eventBroker = Boardcore::EventBroker::getInstance();
- eventBroker.subscribe(this, Common::TOPIC_ABK);
- eventBroker.subscribe(this, Common::TOPIC_ADA);
- eventBroker.subscribe(this, Common::TOPIC_MEA);
- eventBroker.subscribe(this, Common::TOPIC_DPL);
- eventBroker.subscribe(this, Common::TOPIC_CAN);
- eventBroker.subscribe(this, Common::TOPIC_FLIGHT);
- eventBroker.subscribe(this, Common::TOPIC_FMM);
- eventBroker.subscribe(this, Common::TOPIC_FSR);
- eventBroker.subscribe(this, Common::TOPIC_NAS);
- eventBroker.subscribe(this, Common::TOPIC_TMTC);
- eventBroker.subscribe(this, Common::TOPIC_MOTOR);
- eventBroker.subscribe(this, Common::TOPIC_TARS);
- eventBroker.subscribe(this, Common::TOPIC_ALT);
- }
-
- void processFlagsImpl(const SimulatorData& simulatorData,
- std::vector<MainFlightPhases>& changed_flags) override
- {
- if (simulatorData.signal == 1)
- {
- miosix::reboot();
- }
-
- if (simulatorData.signal == 2)
- {
- Boardcore::EventBroker::getInstance().post(
- Common::TMTC_FORCE_LANDING, Common::TOPIC_TMTC);
- }
-
- if (simulatorData.signal == 3)
- {
- Boardcore::EventBroker::getInstance().post(
- Common::TMTC_FORCE_LAUNCH, Common::TOPIC_TMTC);
- }
-
- // set true when the first packet from the simulator arrives
- if (isSetTrue(MainFlightPhases::SIMULATION_STARTED))
- {
- t_start = Boardcore::TimestampTimer::getTimestamp();
-
- printf("[HIL] ------- SIMULATION STARTED ! ------- \n");
- changed_flags.push_back(MainFlightPhases::SIMULATION_STARTED);
- }
-
- if (flagsFlightPhases[MainFlightPhases::SIM_FLYING])
- {
- if (isSetTrue(MainFlightPhases::SIM_FLYING))
- {
- registerOutcomes(MainFlightPhases::SIM_FLYING);
- printf("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n");
- changed_flags.push_back(MainFlightPhases::SIM_FLYING);
- }
- }
- }
-
- void printOutcomes()
- {
- printf("OUTCOMES: (times dt from liftoff)\n\n");
- printf("Simulation time: %.3f [sec]\n\n",
- (double)(t_stop - t_start) / 1000000.0f);
-
- printf("Motor stopped burning (simulation flag): \n");
- outcomes[MainFlightPhases::SIM_BURNING].print(t_liftoff);
-
- printf("Airbrakes exit shadowmode: \n");
- outcomes[MainFlightPhases::AEROBRAKES].print(t_liftoff);
-
- printf("Apogee: \n");
- outcomes[MainFlightPhases::APOGEE].print(t_liftoff);
+ std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition);
- printf("Parachute 1: \n");
- outcomes[MainFlightPhases::PARA1].print(t_liftoff);
+ void processFlagsImpl(
+ const SimulatorData& simulatorData,
+ std::vector<MainFlightPhases>& changed_flags) override;
- printf("Parachute 2: \n");
- outcomes[MainFlightPhases::PARA2].print(t_liftoff);
-
- printf("Simulation Stopped: \n");
- outcomes[MainFlightPhases::SIMULATION_STOPPED].print(t_liftoff);
-
- // auto cpuMeter = Boardcore::CpuMeter::getCpuStats();
- // printf("max cpu usage: %+.3f\n", cpuMeter.maxValue);
- // printf("avg cpu usage: %+.3f\n", cpuMeter.mean);
- // printf("min free heap: %+.3lu\n", cpuMeter.minFreeHeap);
- // printf("max free stack:%+.3lu\n", cpuMeter.minFreeStack);
- }
+ void printOutcomes();
private:
void handleEventImpl(const Boardcore::Event& e,
- std::vector<MainFlightPhases>& changed_flags) override
- {
- switch (e)
- {
- case Common::Events::FMM_INIT_ERROR:
- printf("[HIL] ------- INIT FAILED ! ------- \n");
- case Common::Events::FMM_INIT_OK:
- setFlagFlightPhase(MainFlightPhases::CALIBRATION, true);
- printf("[HIL] ------- CALIBRATION ! ------- \n");
- changed_flags.push_back(MainFlightPhases::CALIBRATION);
- break;
- case Common::Events::FLIGHT_DISARMED:
- setFlagFlightPhase(MainFlightPhases::CALIBRATION_OK, true);
- printf("[HIL] CALIBRATION OK!\n");
- changed_flags.push_back(MainFlightPhases::CALIBRATION_OK);
- break;
- case Common::Events::FLIGHT_ARMED:
- setFlagFlightPhase(MainFlightPhases::ARMED, true);
- printf("[HIL] ------- READY TO LAUNCH ! ------- \n");
- changed_flags.push_back(MainFlightPhases::ARMED);
- break;
- case Common::Events::FLIGHT_LAUNCH_PIN_DETACHED:
- setFlagFlightPhase(MainFlightPhases::LIFTOFF_PIN_DETACHED,
- true);
- printf("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n");
- changed_flags.push_back(MainFlightPhases::LIFTOFF_PIN_DETACHED);
- break;
- case Common::Events::FLIGHT_LIFTOFF:
- case Common::Events::TMTC_FORCE_LAUNCH:
- t_liftoff = Boardcore::TimestampTimer::getTimestamp();
- printf("[HIL] ------- LIFTOFF -------: %f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(MainFlightPhases::LIFTOFF);
- break;
- case Common::Events::FLIGHT_MOTOR_SHUTDOWN:
- printf("[HIL] ------- SHUTDOWN -------: %f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(MainFlightPhases::SHUTDOWN);
- case Common::Events::ABK_SHADOW_MODE_TIMEOUT:
- setFlagFlightPhase(MainFlightPhases::AEROBRAKES, true);
- registerOutcomes(MainFlightPhases::AEROBRAKES);
- printf("[HIL] ABK shadow mode timeout\n");
- changed_flags.push_back(MainFlightPhases::AEROBRAKES);
- break;
- case Common::Events::ADA_SHADOW_MODE_TIMEOUT:
- printf("[HIL] ADA shadow mode timeout\n");
- break;
- case Common::Events::ABK_DISABLE:
- setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false);
- printf("[HIL] ABK disabled\n");
- break;
- case Common::Events::FLIGHT_APOGEE_DETECTED:
- case Common::Events::CAN_APOGEE_DETECTED:
- setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false);
- registerOutcomes(MainFlightPhases::APOGEE);
- printf("[HIL] ------- APOGEE DETECTED ! ------- %f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(MainFlightPhases::APOGEE);
- break;
- case Common::Events::FLIGHT_DROGUE_DESCENT:
- case Common::Events::TMTC_FORCE_EXPULSION:
- setFlagFlightPhase(MainFlightPhases::PARA1, true);
- registerOutcomes(MainFlightPhases::PARA1);
- printf("[HIL] ------- PARA1 ! -------%f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(MainFlightPhases::PARA1);
- break;
- case Common::Events::FLIGHT_WING_DESCENT:
- case Common::Events::FLIGHT_DPL_ALT_DETECTED:
- case Common::Events::TMTC_FORCE_DEPLOYMENT:
- setFlagFlightPhase(MainFlightPhases::PARA1, false);
- setFlagFlightPhase(MainFlightPhases::PARA2, true);
- registerOutcomes(MainFlightPhases::PARA2);
- printf("[HIL] ------- PARA2 ! ------- %f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(MainFlightPhases::PARA2);
- break;
- case Common::Events::FLIGHT_LANDING_DETECTED:
- case Common::Events::TMTC_FORCE_LANDING:
- t_stop = Boardcore::TimestampTimer::getTimestamp();
- setFlagFlightPhase(MainFlightPhases::PARA2, false);
- setFlagFlightPhase(MainFlightPhases::SIMULATION_STOPPED, true);
- changed_flags.push_back(MainFlightPhases::SIMULATION_STOPPED);
- registerOutcomes(MainFlightPhases::SIMULATION_STOPPED);
- printf("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n",
- (double)t_stop / 1000000.0f);
- printOutcomes();
- break;
- default:
- printf("%s event\n", Common::getEventString(e).c_str());
- }
- }
+ std::vector<MainFlightPhases>& changed_flags) override;
};
class MainHIL
@@ -266,58 +70,14 @@ class MainHIL
public Boardcore::InjectableWithDeps<Buses, Actuators, FlightModeManager,
ADAController, NASController,
MEAController, ABKController>
-
{
public:
- MainHIL()
- : Boardcore::HIL<MainFlightPhases, SimulatorData, ActuatorData>(
- nullptr, nullptr, [this]() { return updateActuatorData(); },
- 1000 / Config::HIL::SIMULATION_RATE.value())
- {
- }
-
- bool start() override
- {
- auto* nas = getModule<NASController>();
- auto& hilUsart = getModule<Buses>()->getHILUart();
-
- hilPhasesManager = new MainHILPhasesManager(
- [nas]()
- { return Boardcore::TimedTrajectoryPoint(nas->getNASState()); });
-
- hilTransceiver = new MainHILTransceiver(hilUsart, hilPhasesManager);
+ MainHIL();
- return Boardcore::HIL<MainFlightPhases, SimulatorData,
- ActuatorData>::start();
- }
+ bool start() override;
private:
- ActuatorData updateActuatorData()
- {
- auto actuators = getModule<Actuators>();
-
- ADAStateHIL adaStateHIL{getModule<ADAController>()->getADAState(),
- getModule<ADAController>()->getState()};
-
- NASStateHIL nasStateHIL{getModule<NASController>()->getNASState(),
- getModule<NASController>()->getState()};
-
- AirBrakesStateHIL abkStateHIL{getModule<ABKController>()->getState()};
-
- MEAStateHIL meaStateHIL{getModule<MEAController>()->getMEAState(),
- getModule<MEAController>()->getState()};
-
- ActuatorsStateHIL actuatorsStateHIL{
- actuators->getServoPosition(ServosList::AIR_BRAKES_SERVO),
- actuators->getServoPosition(ServosList::EXPULSION_SERVO),
- (actuators->isCanServoOpen(ServosList::MAIN_VALVE) ? 1.f : 0.f),
- (actuators->isCanServoOpen(ServosList::VENTING_VALVE) ? 1.f : 0.f),
- static_cast<float>(miosix::gpios::mainDeploy::value())};
-
- // Returning the feedback for the simulator
- return ActuatorData{adaStateHIL, nasStateHIL, abkStateHIL, meaStateHIL,
- actuatorsStateHIL};
- };
+ ActuatorData updateActuatorData();
};
} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/HIL/HILData.h b/src/boards/Main/HIL/HILData.h
index db77d29159241f3cf2ec49a13cb7ac0fcddaae50..8aa67209ca8a102afaedeefabf787f3ffd623ac2 100644
--- a/src/boards/Main/HIL/HILData.h
+++ b/src/boards/Main/HIL/HILData.h
@@ -25,26 +25,19 @@
#include <Main/Configs/HILSimulationConfig.h>
#include <sensors/HILSimulatorData.h>
-// FMM
-#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
-
// ADA
-#include <Main/StateMachines/ADAController/ADAController.h>
#include <Main/StateMachines/ADAController/ADAControllerData.h>
#include <algorithms/ADA/ADAData.h>
// NAS
-#include <Main/StateMachines/NASController/NASController.h>
#include <Main/StateMachines/NASController/NASControllerData.h>
#include <algorithms/NAS/NASState.h>
// ABK
-#include <Main/StateMachines/ABKController/ABKController.h>
#include <Main/StateMachines/ABKController/ABKControllerData.h>
#include <algorithms/AirBrakes/AirBrakesInterp.h>
// MEA
-#include <Main/StateMachines/MEAController/MEAController.h>
#include <Main/StateMachines/MEAController/MEAControllerData.h>
#include <algorithms/MEA/MEAData.h>
@@ -69,6 +62,13 @@ using MainPitotSimulatorData =
using MainTemperatureSimulatorData =
Boardcore::TemperatureSimulatorData<Config::HIL::N_DATA_TEMP>;
+enum class HILSignal : int
+{
+ SIMULATION_STARTED = 1,
+ SIMULATION_STOPPED = 2,
+ SIMULATION_FORCE_LAUNCH = 3
+};
+
enum class MainFlightPhases
{
SIM_FLYING,
@@ -108,7 +108,8 @@ struct ADAStateHIL
{
}
- ADAStateHIL(Boardcore::ADAState adaState, Main::ADAControllerState state)
+ ADAStateHIL(const Boardcore::ADAState& adaState,
+ const Main::ADAControllerState& state)
: mslAltitude(adaState.mslAltitude), aglAltitude(adaState.aglAltitude),
verticalSpeed(adaState.verticalSpeed),
apogeeDetected(state >= ADAControllerState::ACTIVE_DROGUE_DESCENT),
@@ -156,7 +157,8 @@ struct NASStateHIL
{
}
- NASStateHIL(Boardcore::NASState nasState, Main::NASControllerState state)
+ NASStateHIL(const Boardcore::NASState& nasState,
+ const Main::NASControllerState& state)
: n(nasState.n), e(nasState.e), d(nasState.d), vn(nasState.vn),
ve(nasState.ve), vd(nasState.vd), qx(nasState.qx), qy(nasState.qy),
qz(nasState.qz), qw(nasState.qw),
@@ -191,7 +193,7 @@ struct AirBrakesStateHIL
AirBrakesStateHIL() : updating(0) {}
- AirBrakesStateHIL(Main::ABKControllerState state)
+ explicit AirBrakesStateHIL(const Main::ABKControllerState& state)
: updating(state >= Main::ABKControllerState::ACTIVE)
{
}
@@ -217,7 +219,8 @@ struct MEAStateHIL
{
}
- MEAStateHIL(Boardcore::MEAState meaState, Main::MEAControllerState state)
+ MEAStateHIL(const Boardcore::MEAState& meaState,
+ const Main::MEAControllerState& state)
: correctedPressure(meaState.estimatedPressure),
estimatedMass(meaState.estimatedMass),
estimatedApogee(meaState.estimatedApogee),
@@ -314,9 +317,10 @@ struct ActuatorData
{
}
- ActuatorData(ADAStateHIL adaState, NASStateHIL nasState,
- AirBrakesStateHIL airBrakesState, MEAStateHIL meaState,
- ActuatorsStateHIL actuatorsState)
+ ActuatorData(const ADAStateHIL& adaState, const NASStateHIL& nasState,
+ const AirBrakesStateHIL& airBrakesState,
+ const MEAStateHIL& meaState,
+ const ActuatorsStateHIL& actuatorsState)
: adaState(adaState), nasState(nasState),
airBrakesState(airBrakesState), meaState(meaState),
actuatorsState(actuatorsState)
diff --git a/src/boards/Main/PersistentVars/PersistentVars.cpp b/src/boards/Main/PersistentVars/PersistentVars.cpp
index c07ec5df6ccb244f775f75be59c3136ccf196598..2ac4c53d915bc97a6607ebcf3a375931836168ba 100644
--- a/src/boards/Main/PersistentVars/PersistentVars.cpp
+++ b/src/boards/Main/PersistentVars/PersistentVars.cpp
@@ -22,21 +22,27 @@
#include "PersistentVars.h"
+#include <arch/common/drivers/stm32_bsram.h>
+#include <miosix.h>
+
using namespace miosix;
+static bool PRESERVE hilMode = false;
+
namespace Main
{
-bool PRESERVE PersistentVars::hilMode = false;
-
-PersistentVars::PersistentVars() {}
+namespace PersistentVars
+{
-void PersistentVars::setHilMode(bool _hilMode)
+void setHilMode(bool _hilMode)
{
BSRAM::EnableWriteLock l;
hilMode = _hilMode;
}
-bool PersistentVars::getHilMode() { return hilMode; }
+bool getHilMode() { return hilMode; }
+
+} // namespace PersistentVars
} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/PersistentVars/PersistentVars.h b/src/boards/Main/PersistentVars/PersistentVars.h
index 937aaf78aa509f555df27949f8a79991c9698e33..d533078bf4f74a3a4cd1331acb02cc4efcc97866 100644
--- a/src/boards/Main/PersistentVars/PersistentVars.h
+++ b/src/boards/Main/PersistentVars/PersistentVars.h
@@ -22,23 +22,16 @@
#pragma once
-#include <arch/common/drivers/stm32_bsram.h>
-#include <utils/DependencyManager/DependencyManager.h>
-
namespace Main
{
-class PersistentVars : public Boardcore::Injectable
+namespace PersistentVars
{
-public:
- PersistentVars();
- void setHilMode(bool _hilMode);
+void setHilMode(bool _hilMode);
- bool getHilMode();
+bool getHilMode();
-private:
- static bool PRESERVE hilMode;
-};
+} // namespace PersistentVars
} // 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 661723b096c51378944466ce2c88cef36b74fe2d..89e7630b0cd6d65a4a81f8d36dc06fbdcdedf973 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -740,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 = getModule<PersistentVars>()->getHilMode() ? 1 : 0;
+ tm.hil_state = 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 3702162be9c33a89be7c5ce6a8810e5dd3970ee3..fff2e20e830532f1aaa9243fa827a4a16d4e1d7d 100644
--- a/src/boards/Main/Radio/Radio.h
+++ b/src/boards/Main/Radio/Radio.h
@@ -47,11 +47,10 @@ 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, PersistentVars>
+class Radio : public Boardcore::InjectableWithDeps<
+ Buses, BoardScheduler, Actuators, PinHandler, CanHandler,
+ Sensors, FlightModeManager, ADAController, NASController,
+ MEAController, ABKController, StatsRecorder, AlgoReference>
{
public:
Radio() {}
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index 1fdc127889aacebcde7254f85e5472536357b094..f45fb98a13454fc3771855361fd7cec6d4a706bd 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -32,6 +32,22 @@ using namespace Common;
using namespace Boardcore;
using namespace miosix;
+void enterHilMode()
+{
+ PersistentVars::setHilMode(true);
+ reboot();
+}
+
+void exitHilMode()
+{
+ // Reboot only if in HIL mode
+ if (PersistentVars::getHilMode())
+ {
+ PersistentVars::setHilMode(false);
+ reboot();
+ }
+}
+
FlightModeManager::FlightModeManager()
: HSM{&FlightModeManager::state_on_ground, STACK_DEFAULT_FOR_PTHREAD,
Config::Scheduler::FMM_PRIORITY}
@@ -78,16 +94,12 @@ State FlightModeManager::state_on_ground(const Event& event)
getModule<CanHandler>()->sendEvent(
CanConfig::EventId::EXIT_HIL_MODE);
miosix::Thread::sleep(1000);
- // Fallthrough
+ exitHilMode();
+ return HANDLED;
}
case CAN_EXIT_HIL_MODE:
{
- // Reboot only if in HIL mode
- if (getModule<PersistentVars>()->getHilMode())
- {
- getModule<PersistentVars>()->setHilMode(false);
- miosix::reboot();
- }
+ exitHilMode();
return HANDLED;
}
default:
@@ -433,13 +445,13 @@ State FlightModeManager::state_test_mode(const Event& event)
getModule<CanHandler>()->sendEvent(
CanConfig::EventId::ENTER_HIL_MODE);
Thread::sleep(1000);
- // Fallthrough
+ enterHilMode();
+ return HANDLED;
}
case CAN_ENTER_HIL_MODE:
{
- getModule<PersistentVars>()->setHilMode(true);
- reboot();
- __builtin_unreachable();
+ enterHilMode();
+ return HANDLED;
}
default:
{
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index 409645cc84668ba576f61834cd0b906f7af0b96d..ef2ff49ee5556a9bb45e212475097ae91d56cf22 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -39,8 +39,7 @@ namespace Main
class FlightModeManager
: public Boardcore::InjectableWithDeps<Actuators, Sensors, CanHandler,
- StatsRecorder, AlgoReference,
- PersistentVars>,
+ StatsRecorder, AlgoReference>,
public Boardcore::HSM<FlightModeManager>
{
public:
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index 11c46d54a3e98f407fa1d4bfb2a15df029cbe994..2fb33cf7c8b380dd9285ef6c129d38b7a1ba11a0 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -62,9 +62,8 @@ int main()
bool initResult = true;
- PersistentVars *persistentVars = new PersistentVars();
- Buses *buses = new Buses();
- BoardScheduler *scheduler = new BoardScheduler();
+ Buses *buses = new Buses();
+ BoardScheduler *scheduler = new BoardScheduler();
Sensors *sensors;
Actuators *actuators = new Actuators();
@@ -81,7 +80,7 @@ int main()
MainHIL *hil = nullptr;
// HIL
- if (persistentVars->getHilMode())
+ if (PersistentVars::getHilMode())
{
std::cout << "MAIN SimulatorData: " << sizeof(SimulatorData)
<< ", ActuatorData: " << sizeof(ActuatorData) << std::endl;
@@ -109,8 +108,7 @@ int main()
});
// Insert modules
- initResult = initResult && manager.insert<PersistentVars>(persistentVars) &&
- manager.insert<Buses>(buses) &&
+ initResult = initResult && manager.insert<Buses>(buses) &&
manager.insert<BoardScheduler>(scheduler) &&
manager.insert<Sensors>(sensors) &&
manager.insert<Radio>(radio) &&
@@ -226,7 +224,7 @@ int main()
std::cout << "Error failed to start SD" << std::endl;
}
- if (persistentVars->getHilMode())
+ if (PersistentVars::getHilMode())
{
std::cout << "Starting HIL" << std::endl;
hil->start();