diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53388554c62a16673075b89389685826820750d8..4aa347cdf592935741311b6b54d4d52cb7edd373 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -112,7 +112,9 @@ add_executable(lyra-gs-entry
target_include_directories(lyra-gs-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(lyra-gs-entry stm32f767zi_lyra_gs)
-# Parafoil targets
+#-----------------------------------------------------------------------------#
+# Parafoil entrypoints #
+#-----------------------------------------------------------------------------#
add_executable(parafoil-progressive-rotation src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
target_include_directories(parafoil-progressive-rotation PRIVATE ${OBSW_INCLUDE_DIRS})
@@ -155,3 +157,7 @@ target_compile_definitions(parafoil-t-approach-dynamic PRIVATE
DYNAMIC_TARGET_LONGITUDE_OFFSET=50
)
sbs_target(parafoil-t-approach-dynamic stm32f429zi_death_stack_v2)
+
+add_executable(mockup-main src/MockupMain/mockup-entry.cpp ${MOCKUP_MAIN_COMPUTER})
+target_include_directories(mockup-main PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(mockup-main stm32f429zi_death_stack_v2)
\ No newline at end of file
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 71a0052121fd66e140d2988c9fba3841d73f6e0d..3e21ba5ca01b6a6ec7ad1a552d88a645a452f780 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -143,4 +143,14 @@ set(PARAFOIL_COMPUTER
src/Parafoil/Wing/WingAlgorithm.cpp
src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+)
+
+set(MOCKUP_MAIN_COMPUTER
+ src/MockupMain/PinHandler/PinHandler.cpp
+ src/MockupMain/Radio/Radio.cpp
+ src/MockupMain/Radio/MessageHandler.cpp
+ src/MockupMain/FlightStatsRecorder/FlightStatsRecorder.cpp
+ src/MockupMain/Sensors/Sensors.cpp
+ src/MockupMain/StateMachines/FlightModeManager/FlightModeManager.cpp
+ src/MockupMain/StateMachines/NASController/NASController.cpp
)
\ No newline at end of file
diff --git a/src/MockupMain/BoardScheduler.h b/src/MockupMain/BoardScheduler.h
new file mode 100644
index 0000000000000000000000000000000000000000..1769434b2c5254281ec7aa3ff57fb2a18d5817fc
--- /dev/null
+++ b/src/MockupMain/BoardScheduler.h
@@ -0,0 +1,120 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <diagnostic/PrintLogger.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace MockupMain
+{
+
+/**
+ * @brief The class that manages the task schedulers of the board.
+ * It takes care of handing out task schedulers to modules.
+ */
+class BoardScheduler : public Boardcore::Injectable
+{
+public:
+ /**
+ * @brief Enclosing struct to avoid polluting the BoardScheduler namespace.
+ */
+ struct Priority
+ {
+ /**
+ * @brief Priority levels for the board schedulers.
+ */
+ enum PriorityLevel
+ {
+ LOW = miosix::PRIORITY_MAX - 4,
+ MEDIUM = miosix::PRIORITY_MAX - 3,
+ HIGH = miosix::PRIORITY_MAX - 2,
+ CRITICAL = miosix::PRIORITY_MAX - 1,
+ };
+ };
+
+ Boardcore::TaskScheduler& nasController() { return critical; }
+ Boardcore::TaskScheduler& sensors() { return high; }
+ Boardcore::TaskScheduler& pinHandler() { return high; }
+ Boardcore::TaskScheduler& radio() { return medium; }
+
+ static Priority::PriorityLevel flightModeManagerPriority()
+ {
+ return Priority::MEDIUM;
+ }
+
+ static Priority::PriorityLevel nasControllerPriority()
+ {
+ return Priority::MEDIUM;
+ }
+
+ /**
+ * @brief Starts all the schedulers
+ */
+ [[nodiscard]] bool start()
+ {
+ if (!critical.start())
+ {
+ LOG_ERR(logger, "Critical priority scheduler failed to start");
+ return false;
+ }
+
+ if (!high.start())
+ {
+ LOG_ERR(logger, "High priority scheduler failed to start");
+ return false;
+ }
+
+ if (!medium.start())
+ {
+ LOG_ERR(logger, "Medium priority scheduler failed to start");
+ return false;
+ }
+
+ if (!low.start())
+ {
+ LOG_ERR(logger, "Low priority scheduler failed to start");
+ return false;
+ }
+
+ started = true;
+ return true;
+ }
+
+ /**
+ * @brief Returns if all the schedulers are up and running
+ */
+ bool isStarted() { return started; }
+
+private:
+ Boardcore::TaskScheduler critical{Priority::CRITICAL};
+ Boardcore::TaskScheduler high{Priority::HIGH};
+ Boardcore::TaskScheduler medium{Priority::MEDIUM};
+ Boardcore::TaskScheduler low{Priority::LOW};
+
+ std::atomic<bool> started{false};
+
+ Boardcore::PrintLogger logger =
+ Boardcore::Logging::getLogger("BoardScheduler");
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/Buses.h b/src/MockupMain/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..26de2b81ec5e67bb61851920e28eae4382822f3a
--- /dev/null
+++ b/src/MockupMain/Buses.h
@@ -0,0 +1,53 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <drivers/spi/SPIBus.h>
+#include <drivers/usart/USART.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace MockupMain
+{
+
+/**
+ * @brief Manages all the buses of the mockup board.
+ * It provides access to the buses used by the sensors and other peripherals.
+ */
+class Buses : public Boardcore::Injectable
+{
+public:
+ Boardcore::USART usart1;
+ Boardcore::USART usart2;
+ Boardcore::USART usart3;
+ Boardcore::USART uart4;
+
+ Boardcore::SPIBus spi1;
+ Boardcore::SPIBus spi2;
+
+ Buses()
+ : usart1(USART1, 115200), usart2(USART2, 115200),
+ usart3(USART3, 115200), uart4(UART4, 115200), spi1(SPI1), spi2(SPI2)
+ {
+ }
+};
+} // namespace MockupMain
diff --git a/src/MockupMain/Configs/FlightModeManagerConfig.h b/src/MockupMain/Configs/FlightModeManagerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..b41e8c9f2e4479eb760f08fb2f26b93397b1ea0d
--- /dev/null
+++ b/src/MockupMain/Configs/FlightModeManagerConfig.h
@@ -0,0 +1,43 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <units/Time.h>
+
+#include <chrono>
+
+namespace MockupMain
+{
+namespace Config
+{
+namespace FlightModeManager
+{
+
+/* linter-off */ using namespace Boardcore::Units::Time;
+
+constexpr auto LOGGING_DELAY = 5_s;
+constexpr auto CONTROL_DELAY = 5_s;
+
+} // namespace FlightModeManager
+} // namespace Config
+} // namespace MockupMain
diff --git a/src/MockupMain/Configs/NASConfig.h b/src/MockupMain/Configs/NASConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..09d9239c792ce4dbc0a4939df0583175b3937b98
--- /dev/null
+++ b/src/MockupMain/Configs/NASConfig.h
@@ -0,0 +1,82 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Niccolò Betto, Davide Basso
+ *
+ * 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 <algorithms/NAS/NASConfig.h>
+#include <common/ReferenceConfig.h>
+#include <units/Acceleration.h>
+#include <units/Frequency.h>
+#include <units/Time.h>
+
+namespace MockupMain
+{
+namespace Config
+{
+namespace NAS
+{
+
+/* linter off */ using namespace Boardcore::Units::Time;
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace Boardcore::Units::Acceleration;
+
+constexpr auto UPDATE_RATE = 50_hz;
+constexpr auto UPDATE_RATE_SECONDS = 0.02_s;
+
+constexpr int CALIBRATION_SAMPLES_COUNT = 20;
+constexpr auto CALIBRATION_SLEEP_TIME = 100_ms;
+
+static const Boardcore::NASConfig CONFIG = {
+ .T = UPDATE_RATE_SECONDS.value(),
+ .SIGMA_BETA = 0.0001,
+ .SIGMA_W = 0.0019,
+ .SIGMA_ACC = 0.202,
+ .SIGMA_MAG = 0.0047,
+ .SIGMA_GPS = {0.0447f, 0.0447f, 1.0f / 30.0f, 1.0f / 30.0f},
+ .SIGMA_BAR = 4.5097f,
+ .SIGMA_POS = 2.0,
+ .SIGMA_VEL = 1.0,
+ .SIGMA_PITOT = 1e-3,
+ .P_POS = 0.0,
+ .P_POS_VERTICAL = 0.0,
+ .P_VEL = 0.0,
+ .P_VEL_VERTICAL = 0.0,
+ .P_ATT = 0.1,
+ .P_BIAS = 0.01,
+ .SATS_NUM = 6.0,
+ .NED_MAG = Common::ReferenceConfig::nedMag};
+
+// Only use one out of every 50 samples (1 Hz)
+constexpr int MAGNETOMETER_DECIMATE = 50;
+
+// Maximum allowed acceleration to correct with GPS
+constexpr auto DISABLE_GPS_ACCELERATION_THRESHOLD = 34.0_mps2;
+
+// How much confidence to apply to the accelerometer to check if it is 1g
+constexpr auto ACCELERATION_1G_CONFIDENCE = 0.5_mps2;
+// How many samples will determine that we are in fact measuring gravity
+// acceleration
+constexpr int ACCELERATION_1G_SAMPLES = 20;
+
+} // namespace NAS
+} // namespace Config
+} // namespace MockupMain
diff --git a/src/MockupMain/Configs/PinHandlerConfig.h b/src/MockupMain/Configs/PinHandlerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..3af0c89c204ccfa355827cb8fe39120538e68c0b
--- /dev/null
+++ b/src/MockupMain/Configs/PinHandlerConfig.h
@@ -0,0 +1,51 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <utils/PinObserver/PinObserver.h>
+
+#include <chrono>
+
+namespace MockupMain
+{
+namespace Config
+{
+namespace PinHandler
+{
+
+/* linter-off */ using namespace std::chrono_literals;
+
+namespace PinObserver
+{
+constexpr auto PERIOD = 20ms;
+}
+
+namespace Expulsion
+{
+constexpr auto DETECTION_THRESHOLD = 20;
+constexpr auto TRIGGERING_TRANSITION = Boardcore::PinTransition::FALLING_EDGE;
+} // namespace Expulsion
+
+} // namespace PinHandler
+} // namespace Config
+} // namespace MockupMain
diff --git a/src/MockupMain/Configs/RadioConfig.h b/src/MockupMain/Configs/RadioConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..28249df624a17a3288817b77517e3fdbae49f1c2
--- /dev/null
+++ b/src/MockupMain/Configs/RadioConfig.h
@@ -0,0 +1,65 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <common/MavlinkOrion.h>
+#include <units/Frequency.h>
+#include <units/Time.h>
+
+namespace MockupMain
+{
+namespace Config
+{
+namespace Radio
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace Boardcore::Units::Time;
+
+constexpr auto LOW_RATE_TELEMETRY = 0.5_hz;
+constexpr auto HIGH_RATE_TELEMETRY = 4_hz;
+constexpr auto MESSAGE_QUEUE_SIZE = 10;
+
+namespace Mavlink
+{
+constexpr uint8_t SYSTEM_ID = SysIDs::MAV_SYSID_PAYLOAD;
+constexpr uint8_t COMPONENT_ID = 0;
+} // namespace Mavlink
+
+namespace MavlinkDriver
+{
+constexpr auto PKT_LENGTH = 255;
+constexpr auto PKT_QUEUE_SIZE = 20;
+constexpr auto MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+constexpr auto SLEEP_AFTER_SEND = 0_ms;
+constexpr auto MAX_PKT_AGE = 200_ms;
+} // namespace MavlinkDriver
+
+namespace Xbee
+{
+constexpr auto ENABLE_80KPS_DATA_RATE = true;
+constexpr auto TIMEOUT = 5000_ms;
+} // namespace Xbee
+
+} // namespace Radio
+} // namespace Config
+} // namespace MockupMain
diff --git a/src/MockupMain/Configs/SensorsConfig.h b/src/MockupMain/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..cbabea3652ca82cf0eab6235718de6b2f33cd3ca
--- /dev/null
+++ b/src/MockupMain/Configs/SensorsConfig.h
@@ -0,0 +1,145 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <drivers/adc/InternalADC.h>
+#include <sensors/ADS131M08/ADS131M08Defs.h>
+#include <sensors/BMX160/BMX160Config.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS3MDL/LIS3MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <units/Frequency.h>
+
+#include <chrono>
+
+namespace MockupMain
+{
+namespace Config
+{
+namespace Sensors
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace std::chrono_literals;
+
+namespace BMX160
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = 50_hz;
+
+constexpr auto CALIBRATION_DURATION = 2000ms;
+constexpr auto ACC_FSR = Boardcore::BMX160Config::AccelerometerRange::G_16;
+constexpr auto GYRO_FSR = Boardcore::BMX160Config::GyroscopeRange::DEG_1000;
+constexpr auto ACC_GYRO_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_100;
+constexpr auto MAG_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_100;
+
+static const Boardcore::AxisOrthoOrientation AXIS_ORIENTATION = {
+ Boardcore::Direction::POSITIVE_Z, Boardcore::Direction::POSITIVE_Y};
+
+constexpr auto TEMP_DIVIDER = 0;
+constexpr auto FIFO_WATERMARK = 40;
+
+constexpr auto FIFO_HEADER_SIZE = 1;
+constexpr auto ACC_DATA_SIZE = 6;
+constexpr auto GYRO_DATA_SIZE = 6;
+constexpr auto MAG_DATA_SIZE = 8;
+} // namespace BMX160
+
+namespace H3LIS331DL
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = 800_hz;
+constexpr auto ODR = Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_1000;
+constexpr auto BDU =
+ Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
+constexpr auto FSR = Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+} // namespace H3LIS331DL
+
+namespace LPS22DF
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = 20_hz;
+constexpr auto AVG = Boardcore::LPS22DF::AVG_4;
+constexpr auto ODR = Boardcore::LPS22DF::ODR_50;
+} // namespace LPS22DF
+
+namespace UBXGPS
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = 10_khz;
+} // namespace UBXGPS
+
+namespace ADS131M08
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = 1600_hz;
+constexpr auto OVERSAMPLING_RATIO =
+ Boardcore::ADS131M08Defs::OversamplingRatio::OSR_512;
+constexpr bool GLOBAL_CHOP_MODE = true;
+} // namespace ADS131M08
+
+namespace LIS3MDL
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = 100_hz;
+constexpr auto ODR = Boardcore::LIS3MDL::ODR_155_HZ;
+constexpr auto FSR = Boardcore::LIS3MDL::FS_4_GAUSS;
+} // namespace LIS3MDL
+
+namespace InternalADC
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = 100_hz;
+constexpr auto VBAT_CH = Boardcore::InternalADC::Channel::CH5;
+constexpr auto VBAT_COEFF = (150 + 40.2) / 40.2;
+} // namespace InternalADC
+
+namespace LoadCell
+{
+constexpr auto ENABLED = true;
+constexpr auto SAMPLING_RATE = InternalADC::SAMPLING_RATE;
+constexpr auto ADC_CHANNEL = Boardcore::ADS131M08Defs::Channel::CHANNEL_0;
+
+namespace Calibration
+{
+constexpr float P0_VOLTAGE = -488.47e-6;
+constexpr float P0_MASS = 3.272;
+constexpr float P1_VOLTAGE = -2.24e-6;
+constexpr float P1_MASS = 30.233;
+} // namespace Calibration
+
+} // namespace LoadCell
+
+namespace MagCalibration
+{
+constexpr auto FILE_ENABLED = true;
+constexpr auto SAMPLING_RATE = 10_hz;
+constexpr auto CALIBRATION_PATH = "/sd/bmx160_magnetometer_calibration.csv";
+} // namespace MagCalibration
+
+} // namespace Sensors
+
+} // namespace Config
+
+} // namespace MockupMain
diff --git a/src/MockupMain/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/MockupMain/FlightStatsRecorder/FlightStatsRecorder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d1e8d33c103e250ff97693cd8566d53ad75a21ac
--- /dev/null
+++ b/src/MockupMain/FlightStatsRecorder/FlightStatsRecorder.cpp
@@ -0,0 +1,120 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Niccolò Betto, Davide Basso
+ *
+ * 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 "FlightStatsRecorder.h"
+
+#include <MockupMain/Configs/NASConfig.h>
+#include <MockupMain/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <common/ReferenceConfig.h>
+#include <utils/AeroUtils/AeroUtils.h>
+
+using namespace Boardcore;
+using namespace Boardcore::Units::Speed;
+using namespace Boardcore::Units::Length;
+using namespace Boardcore::Units::Pressure;
+using namespace Boardcore::Units::Acceleration;
+using namespace miosix;
+using namespace Eigen;
+using namespace Common;
+
+namespace MockupMain
+{
+
+void FlightStatsRecorder::reset()
+{
+ Lock<FastMutex> lock{statsMutex};
+ stats = Stats{};
+}
+
+FlightStatsRecorder::Stats FlightStatsRecorder::getStats()
+{
+ Lock<FastMutex> lock{statsMutex};
+ return stats;
+}
+
+void FlightStatsRecorder::dropDetected(uint64_t ts)
+{
+ Lock<FastMutex> lock{statsMutex};
+ stats.dropTs = ts;
+}
+
+void FlightStatsRecorder::deploymentDetected(uint64_t ts, Meter alt)
+{
+ Lock<FastMutex> lock{statsMutex};
+ stats.dplTs = ts;
+ stats.dplAlt = alt;
+}
+
+void FlightStatsRecorder::updateAcc(const AccelerometerData& data)
+{
+ auto fmmState = getModule<FlightModeManager>()->getState();
+
+ // Do nothing if it was not dropped yet
+ if (fmmState != FlightModeManagerState::FLYING_DROGUE_DESCENT)
+ return;
+
+ auto acc = MeterPerSecondSquared{static_cast<Vector3f>(data).norm()};
+ Lock<FastMutex> lock{statsMutex};
+
+ if (fmmState != FlightModeManagerState::FLYING_DROGUE_DESCENT)
+ {
+ // Record this event only after drop, before deployment
+ if (acc > stats.dropMaxAcc)
+ {
+ stats.dropMaxAcc = acc;
+ stats.dropMaxAccTs = data.accelerationTimestamp;
+ }
+ }
+ else
+ {
+ // Record this event only after deployment
+ if (acc > stats.dplMaxAcc)
+ {
+ stats.dplMaxAcc = acc;
+ stats.dplMaxAccTs = data.accelerationTimestamp;
+ }
+ }
+}
+
+void FlightStatsRecorder::updateNas(const NASState& data, float refTemperature)
+{
+ auto fmmState = getModule<FlightModeManager>()->getState();
+
+ // Do nothing if it was not dropped yet
+ if (fmmState != FlightModeManagerState::FLYING_DROGUE_DESCENT)
+ return;
+
+ Lock<FastMutex> lock{statsMutex};
+
+ // Record this event only during flight
+ auto speed = MeterPerSecond{Vector3f{data.vn, data.vd, data.ve}.norm()};
+ auto alt = Meter{-data.d};
+
+ if (speed > stats.maxSpeed)
+ {
+ stats.maxSpeed = speed;
+ stats.maxSpeedAlt = alt;
+ stats.maxSpeedTs = data.timestamp;
+ }
+}
+
+} // namespace MockupMain
diff --git a/src/MockupMain/FlightStatsRecorder/FlightStatsRecorder.h b/src/MockupMain/FlightStatsRecorder/FlightStatsRecorder.h
new file mode 100644
index 0000000000000000000000000000000000000000..b6276ba14c31561b3bd3317cc11eb3ab9e621e1d
--- /dev/null
+++ b/src/MockupMain/FlightStatsRecorder/FlightStatsRecorder.h
@@ -0,0 +1,82 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Niccolò Betto, Davide Basso
+ *
+ * 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 <algorithms/NAS/NASState.h>
+#include <miosix.h>
+#include <sensors/SensorData.h>
+#include <units/Acceleration.h>
+#include <units/Length.h>
+#include <units/Pressure.h>
+#include <units/Speed.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace MockupMain
+{
+
+class FlightModeManager;
+
+class FlightStatsRecorder
+ : public Boardcore::InjectableWithDeps<FlightModeManager>
+{
+public:
+ struct Stats
+ {
+ // Drop
+ uint64_t dropTs = 0;
+
+ // Maximum acceleration after drop, before deployment
+ uint64_t dropMaxAccTs = 0;
+ Boardcore::Units::Acceleration::MeterPerSecondSquared dropMaxAcc{0};
+
+ // Maximum vertical speed
+ uint64_t maxSpeedTs = 0;
+ Boardcore::Units::Speed::MeterPerSecond maxSpeed{0};
+ Boardcore::Units::Length::Meter maxSpeedAlt{0};
+
+ // Deployment
+ uint64_t dplTs = 0;
+ Boardcore::Units::Length::Meter dplAlt{0};
+
+ // Maximum acceleration after deployment
+ uint64_t dplMaxAccTs = 0;
+ Boardcore::Units::Acceleration::MeterPerSecondSquared dplMaxAcc{0};
+ };
+
+ void reset();
+
+ Stats getStats();
+
+ void dropDetected(uint64_t ts);
+ void deploymentDetected(uint64_t ts, Boardcore::Units::Length::Meter alt);
+
+ void updateAcc(const Boardcore::AccelerometerData& data);
+ void updateNas(const Boardcore::NASState& data, float refTemperature);
+ void updatePressure(const Boardcore::PressureData& data);
+
+private:
+ miosix::FastMutex statsMutex;
+ Stats stats;
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/PinHandler/PinData.h b/src/MockupMain/PinHandler/PinData.h
new file mode 100644
index 0000000000000000000000000000000000000000..6266bcb574d592a8bf18c497f3016373675f109d
--- /dev/null
+++ b/src/MockupMain/PinHandler/PinData.h
@@ -0,0 +1,50 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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>
+
+namespace MockupMain
+{
+
+struct PinChangeData
+{
+ uint64_t timestamp = 0;
+ uint8_t pinId = 0;
+ bool lastState = false;
+ uint32_t changesCount = 0;
+
+ static std::string header()
+ {
+ return "timestamp,pinId,lastState,changesCount\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << static_cast<int>(pinId) << ","
+ << static_cast<int>(lastState) << "," << changesCount << "\n";
+ }
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/PinHandler/PinHandler.cpp b/src/MockupMain/PinHandler/PinHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7235935c742ffc1fbcf5c6851230e4c5ccf50a1f
--- /dev/null
+++ b/src/MockupMain/PinHandler/PinHandler.cpp
@@ -0,0 +1,107 @@
+
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 "PinHandler.h"
+
+#include <MockupMain/BoardScheduler.h>
+#include <MockupMain/Configs/PinHandlerConfig.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
+
+#include "PinData.h"
+
+using namespace Boardcore;
+using namespace Common;
+namespace config = MockupMain::Config::PinHandler;
+namespace hwmap = miosix::inputs;
+
+namespace MockupMain
+{
+
+const decltype(PinHandler::PIN_LIST) PinHandler::PIN_LIST = {
+ PinList::NOSECONE_PIN,
+};
+
+bool PinHandler::start()
+{
+ using namespace std::chrono;
+
+ auto& scheduler = getModule<BoardScheduler>()->pinHandler();
+
+ pinObserver = std::make_unique<PinObserver>(
+ scheduler, milliseconds{config::PinObserver::PERIOD}.count());
+
+ bool expulsionPinDetachResult = pinObserver->registerPinCallback(
+ hwmap::expulsion::getPin(),
+ [this](auto t) { onExpulsionPinTransition(t); },
+ config::Expulsion::DETECTION_THRESHOLD);
+
+ if (!expulsionPinDetachResult)
+ {
+ LOG_ERR(logger,
+ "Failed to register pin callback for the detach ramp pin");
+ return false;
+ }
+
+ started = true;
+ return true;
+}
+
+bool PinHandler::isStarted() { return started; }
+
+PinData PinHandler::getPinData(PinList pin)
+{
+ switch (pin)
+ {
+ case PinList::NOSECONE_PIN:
+ return pinObserver->getPinData(hwmap::expulsion::getPin());
+ default:
+ return PinData{};
+ }
+}
+
+void PinHandler::logPin(PinList pin)
+{
+ auto pinData = getPinData(pin);
+ auto pinChangeData = PinChangeData{
+ .timestamp = TimestampTimer::getTimestamp(),
+ .pinId = static_cast<uint8_t>(pin),
+ .lastState = pinData.lastState,
+ .changesCount = pinData.changesCount,
+ };
+ Logger::getInstance().log(pinChangeData);
+}
+
+void PinHandler::onExpulsionPinTransition(PinTransition transition)
+{
+ logPin(PinList::NOSECONE_PIN);
+ LOG_INFO(logger, "Expulsion Pin transition detected: {}",
+ static_cast<int>(transition));
+
+ if (transition == config::Expulsion::TRIGGERING_TRANSITION)
+ EventBroker::getInstance().post(FLIGHT_NC_DETACHED, TOPIC_FLIGHT);
+}
+
+} // namespace MockupMain
diff --git a/src/MockupMain/PinHandler/PinHandler.h b/src/MockupMain/PinHandler/PinHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..5373ac4891aac52bfaef9c5cdc003f979b811e32
--- /dev/null
+++ b/src/MockupMain/PinHandler/PinHandler.h
@@ -0,0 +1,71 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <common/MavlinkOrion.h>
+#include <diagnostic/PrintLogger.h>
+#include <utils/DependencyManager/DependencyManager.h>
+#include <utils/PinObserver/PinObserver.h>
+
+namespace MockupMain
+{
+class BoardScheduler;
+
+enum class PinList : uint8_t
+{
+ NOSECONE_PIN = 0,
+};
+
+/**
+ * @brief This class contains the handlers for the detach pins on the payload.
+ *
+ * It uses Boardcore's PinObserver to bind these functions to the GPIO pins.
+ * The handlers post an event on the EventBroker.
+ */
+class PinHandler : public Boardcore::InjectableWithDeps<BoardScheduler>
+{
+public:
+ static const std::array<PinList, 1> PIN_LIST;
+
+ [[nodiscard]] bool start();
+
+ bool isStarted();
+
+ /**
+ * @brief Returns information about the specified pin.
+ */
+ Boardcore::PinData getPinData(PinList pin);
+
+private:
+ void logPin(PinList pin);
+
+ void onExpulsionPinTransition(Boardcore::PinTransition transition);
+
+ std::unique_ptr<Boardcore::PinObserver> pinObserver;
+
+ std::atomic<bool> started{false};
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("PinHandler");
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/Radio/MessageHandler.cpp b/src/MockupMain/Radio/MessageHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4253cdcf46329d938c20757a972f29ce31279bb5
--- /dev/null
+++ b/src/MockupMain/Radio/MessageHandler.cpp
@@ -0,0 +1,839 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <MockupMain/BoardScheduler.h>
+#include <MockupMain/Configs/RadioConfig.h>
+#include <MockupMain/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <MockupMain/PinHandler/PinHandler.h>
+#include <MockupMain/Sensors/Sensors.h>
+#include <MockupMain/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <MockupMain/StateMachines/NASController/NASController.h>
+#include <common/Events.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <events/EventBroker.h>
+
+#include "Radio.h"
+
+using namespace Boardcore;
+using namespace Boardcore::Units::Speed;
+using namespace Boardcore::Units::Length;
+using namespace Boardcore::Units::Acceleration;
+using namespace Common;
+namespace config = MockupMain::Config::Radio;
+
+namespace MockupMain
+{
+
+void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg)
+{
+ switch (msg.msgid)
+ {
+ case MAVLINK_MSG_ID_PING_TC:
+ {
+ return enqueueAck(msg);
+ }
+
+ case MAVLINK_MSG_ID_COMMAND_TC:
+ {
+ return handleCommand(msg);
+ }
+
+ case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
+ {
+ auto tmId = static_cast<SystemTMList>(
+ mavlink_msg_system_tm_request_tc_get_tm_id(&msg));
+
+ if (!enqueueSystemTm(tmId))
+ return enqueueNack(msg);
+
+ return enqueueAck(msg);
+ }
+
+ case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
+ {
+ auto sensorId = static_cast<SensorsTMList>(
+ mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg));
+
+ if (!enqueueSensorsTm(sensorId))
+ return enqueueNack(msg);
+
+ return enqueueAck(msg);
+ }
+
+ case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC:
+ {
+ bool allowed = parent.getModule<FlightModeManager>()->isTestMode();
+ if (!allowed)
+ return enqueueNack(msg);
+
+ float altitude =
+ mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
+
+ parent.getModule<NASController>()->setReferenceAltitude(
+ Meter{altitude});
+ break;
+ }
+
+ case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
+ {
+ auto allowed = parent.getModule<FlightModeManager>()->isTestMode();
+ if (!allowed)
+ return enqueueNack(msg);
+
+ float temperature =
+ mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
+
+ parent.getModule<NASController>()->setReferenceTemperature(
+ temperature);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_SET_COORDINATES_TC:
+ {
+ auto allowed = parent.getModule<FlightModeManager>()->isTestMode();
+ if (!allowed)
+ return enqueueNack(msg);
+
+ float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
+ float longitude =
+ mavlink_msg_set_coordinates_tc_get_longitude(&msg);
+
+ parent.getModule<NASController>()->setReferenceCoordinates(
+ latitude, longitude);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_SET_ORIENTATION_QUAT_TC:
+ {
+ if (parent.getModule<NASController>()->getState() !=
+ NASControllerState::READY)
+ {
+ return enqueueNack(msg);
+ }
+
+ // Scalar first quaternion, W is the first element
+ auto quat = Eigen::Quaternionf{
+ mavlink_msg_set_orientation_quat_tc_get_quat_w(&msg),
+ mavlink_msg_set_orientation_quat_tc_get_quat_x(&msg),
+ mavlink_msg_set_orientation_quat_tc_get_quat_y(&msg),
+ mavlink_msg_set_orientation_quat_tc_get_quat_z(&msg),
+ };
+
+ parent.getModule<NASController>()->setOrientation(
+ quat.normalized());
+
+ float qNorm = quat.norm();
+ if (std::abs(qNorm - 1) > 0.001)
+ return enqueueWack(msg);
+ else
+ return enqueueAck(msg);
+ }
+
+ case MAVLINK_MSG_ID_RAW_EVENT_TC:
+ {
+ uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg);
+ uint8_t eventId = mavlink_msg_raw_event_tc_get_event_id(&msg);
+
+ bool testMode = parent.getModule<FlightModeManager>()->isTestMode();
+ // Raw events are allowed in test mode only
+ if (!testMode)
+ return enqueueNack(msg);
+
+ EventBroker::getInstance().post(topicId, eventId);
+ return enqueueAck(msg);
+ }
+
+ default:
+ {
+ return enqueueNack(msg);
+ }
+ }
+}
+
+void Radio::MavlinkBackend::handleCommand(const mavlink_message_t& msg)
+{
+ auto command = static_cast<MavCommandList>(
+ mavlink_msg_command_tc_get_command_id(&msg));
+
+ switch (command)
+ {
+ case MAV_CMD_START_LOGGING:
+ {
+ bool started = Logger::getInstance().start();
+ if (!started)
+ return enqueueNack(msg);
+
+ Logger::getInstance().resetStats();
+ return enqueueAck(msg);
+ }
+
+ case MAV_CMD_STOP_LOGGING:
+ {
+ Logger::getInstance().stop();
+ return enqueueAck(msg);
+ }
+
+ case MAV_CMD_SAVE_CALIBRATION:
+ {
+ bool testMode = parent.getModule<FlightModeManager>()->isTestMode();
+ // Save calibration data in test mode only
+ if (!testMode)
+ return enqueueNack(msg);
+
+ bool magResult = parent.getModule<Sensors>()->saveMagCalibration();
+ if (magResult)
+ return enqueueAck(msg);
+ else
+ return enqueueNack(msg);
+ }
+
+ default:
+ {
+ // Map the command to an event and post it, if it exists
+ auto event = mavCmdToEvent(command);
+ if (event == LAST_EVENT)
+ return enqueueNack(msg);
+
+ EventBroker::getInstance().post(event, TOPIC_TMTC);
+ return enqueueAck(msg);
+ }
+ }
+}
+
+void Radio::MavlinkBackend::enqueueAck(const mavlink_message_t& msg)
+{
+ mavlink_message_t ackMsg;
+ mavlink_msg_ack_tm_pack(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &ackMsg, msg.msgid,
+ msg.seq);
+ enqueueMessage(ackMsg);
+}
+
+void Radio::MavlinkBackend::enqueueNack(const mavlink_message_t& msg)
+{
+ mavlink_message_t nackMsg;
+ mavlink_msg_nack_tm_pack(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &nackMsg, msg.msgid,
+ msg.seq, 0);
+ enqueueMessage(nackMsg);
+}
+
+void Radio::MavlinkBackend::enqueueWack(const mavlink_message_t& msg)
+{
+ mavlink_message_t wackMsg;
+ mavlink_msg_wack_tm_pack(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &wackMsg, msg.msgid,
+ msg.seq, 0);
+ enqueueMessage(wackMsg);
+}
+
+bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
+{
+ switch (tmId)
+ {
+ case MAV_SYS_ID:
+ {
+ 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.radio = parent.isStarted();
+ tm.sensors = parent.getModule<Sensors>()->isStarted();
+ tm.actuators = false; // Not present on main mockup
+ tm.pin_handler = parent.getModule<PinHandler>()->isStarted();
+ tm.scheduler = parent.getModule<BoardScheduler>()->isStarted();
+ tm.can_handler = false; // Not present on main mockup
+
+ mavlink_msg_sys_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_LOGGER_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_logger_tm_t tm;
+
+ // Get the logger stats
+ auto stats = Logger::getInstance().getStats();
+
+ tm.timestamp = stats.timestamp;
+ tm.log_number = stats.logNumber;
+ tm.too_large_samples = stats.tooLargeSamples;
+ tm.dropped_samples = stats.droppedSamples;
+ tm.queued_samples = stats.queuedSamples;
+ tm.buffers_filled = stats.buffersFilled;
+ tm.buffers_written = stats.buffersWritten;
+ tm.writes_failed = stats.writesFailed;
+ tm.last_write_error = stats.lastWriteError;
+ tm.average_write_time = stats.averageWriteTime;
+ tm.max_write_time = stats.maxWriteTime;
+
+ mavlink_msg_logger_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg,
+ &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_MAVLINK_STATS_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_mavlink_stats_tm_t tm;
+
+ // Get the mavlink stats
+ auto stats = driver->getStatus();
+
+ tm.timestamp = stats.timestamp;
+ tm.n_send_queue = stats.nSendQueue;
+ tm.max_send_queue = stats.maxSendQueue;
+ tm.n_send_errors = stats.nSendErrors;
+ tm.msg_received = stats.mavStats.msg_received;
+ tm.buffer_overrun = stats.mavStats.buffer_overrun;
+ tm.parse_error = stats.mavStats.parse_error;
+ tm.parse_state = stats.mavStats.parse_state;
+ tm.packet_idx = stats.mavStats.packet_idx;
+ tm.current_rx_seq = stats.mavStats.current_rx_seq;
+ tm.current_tx_seq = stats.mavStats.current_tx_seq;
+ tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count;
+ tm.packet_rx_drop_count = stats.mavStats.packet_rx_drop_count;
+
+ mavlink_msg_mavlink_stats_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID,
+ &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_NAS_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_nas_tm_t tm;
+
+ auto state = parent.getModule<NASController>()->getState();
+ auto nasState = parent.getModule<NASController>()->getNasState();
+ auto ref = parent.getModule<NASController>()->getReferenceValues();
+
+ tm.timestamp = nasState.timestamp;
+ tm.nas_n = nasState.n;
+ tm.nas_e = nasState.e;
+ tm.nas_d = nasState.d;
+ tm.nas_vn = nasState.vn;
+ tm.nas_ve = nasState.ve;
+ tm.nas_vd = nasState.vd;
+ tm.nas_qx = nasState.qx;
+ tm.nas_qy = nasState.qy;
+ tm.nas_qz = nasState.qz;
+ tm.nas_qw = nasState.qw;
+ tm.nas_bias_x = nasState.bx;
+ tm.nas_bias_y = nasState.by;
+ tm.nas_bias_z = nasState.bz;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_temperature = ref.refTemperature;
+ tm.ref_latitude = ref.refLatitude;
+ tm.ref_longitude = ref.refLongitude;
+ tm.state = static_cast<uint8_t>(state);
+
+ mavlink_msg_nas_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_FLIGHT_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_payload_flight_tm_t tm;
+
+ auto* sensors = parent.getModule<Sensors>();
+ auto* nas = parent.getModule<NASController>();
+ auto* fmm = parent.getModule<FlightModeManager>();
+
+ auto imu = sensors->getBMX160WithCorrectionLastSample();
+ auto gps = sensors->getUBXGPSLastSample();
+ auto lps22df = sensors->getLPS22DFLastSample();
+ auto nasState = nas->getNasState();
+ // auto ref = nas->getReferenceValues();
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ // No digital pressure sensor, use static
+ tm.pressure_digi = lps22df.pressure;
+ tm.pressure_static = lps22df.pressure;
+ // No dynamic pressure sensor
+ tm.pressure_dynamic = -1.f;
+ // Not present on parafoil board
+ tm.airspeed_pitot = 0;
+ tm.altitude_agl = -nasState.d;
+
+ // Sensors
+ tm.acc_x = imu.accelerationX;
+ tm.acc_y = imu.accelerationY;
+ tm.acc_z = imu.accelerationZ;
+ tm.gyro_x = imu.angularSpeedX;
+ tm.gyro_y = imu.angularSpeedY;
+ tm.gyro_z = imu.angularSpeedZ;
+ tm.mag_x = imu.magneticFieldX;
+ tm.mag_y = imu.magneticFieldY;
+ tm.mag_z = imu.magneticFieldZ;
+ tm.gps_lat = gps.latitude;
+ tm.gps_lon = gps.longitude;
+ tm.gps_alt = gps.height;
+ tm.gps_fix = gps.fix;
+
+ // Servos
+ tm.left_servo_angle = -1.f;
+ tm.right_servo_angle = -1.f;
+
+ // Algorithms
+ tm.nas_n = nasState.n;
+ tm.nas_e = nasState.e;
+ tm.nas_d = nasState.d;
+ tm.nas_vn = nasState.vn;
+ tm.nas_ve = nasState.ve;
+ tm.nas_vd = nasState.vd;
+ tm.nas_qx = nasState.qx;
+ tm.nas_qy = nasState.qy;
+ tm.nas_qz = nasState.qz;
+ tm.nas_qw = nasState.qw;
+ tm.nas_bias_x = nasState.bx;
+ tm.nas_bias_y = nasState.by;
+ tm.nas_bias_z = nasState.bz;
+
+ // Wind estimation, used for Load Cell and ADC data
+
+ tm.wes_n =
+ parent.getModule<Sensors>()->getLoadCellLastSample().load;
+ tm.wes_e =
+ parent.getModule<Sensors>()
+ ->getADS131LastSample()
+ .getVoltage(
+ MockupMain::Config::Sensors::LoadCell::ADC_CHANNEL)
+ .voltage;
+
+ tm.battery_voltage = sensors->getBatteryVoltage().batVoltage;
+ // No integrated camera
+ tm.cam_battery_voltage = -1.f;
+ tm.temperature = lps22df.temperature;
+
+ // State machines
+ tm.fmm_state = static_cast<uint8_t>(fmm->getState());
+
+ mavlink_msg_payload_flight_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID,
+ &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_STATS_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_payload_stats_tm_t tm;
+
+ auto* nas = parent.getModule<NASController>();
+ auto* pinHandler = parent.getModule<PinHandler>();
+ auto* recorder = parent.getModule<FlightStatsRecorder>();
+ auto& logger = Logger::getInstance();
+
+ auto stats = recorder->getStats();
+ auto ref = nas->getReferenceValues();
+ auto cpuStats = CpuMeter::getCpuStats();
+ auto loggerStats = logger.getStats();
+
+ // Log CPU stats and reset them
+ CpuMeter::resetCpuStats();
+ logger.log(cpuStats);
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+
+ // No liftoff stats for parafoil, used for drop time
+ tm.liftoff_ts = stats.dropTs;
+ tm.liftoff_max_acc_ts = stats.dropMaxAccTs;
+ tm.liftoff_max_acc =
+ MeterPerSecondSquared{stats.dropMaxAcc}.value();
+
+ // Max speed stats
+ tm.max_speed_ts = stats.maxSpeedTs;
+ tm.max_speed = MeterPerSecond{stats.maxSpeed}.value();
+ tm.max_speed_altitude = Meter{stats.maxSpeedAlt}.value();
+
+ // Useless for parafoil
+ tm.max_mach_ts = 0;
+ tm.max_mach = -1.f;
+
+ // No apogee stats for parafoil
+ tm.apogee_ts = -1;
+ tm.apogee_max_acc_ts = 0;
+ tm.apogee_lat = -1.f;
+ tm.apogee_lon = -1.f;
+ tm.apogee_alt = -1.f;
+ tm.apogee_max_acc = -1.f;
+
+ // Wing stats
+ tm.wing_target_lat = -1.f;
+ tm.wing_target_lon = -1.f;
+ tm.wing_active_target_n = -1.f;
+ tm.wing_active_target_e = -1.f;
+ tm.wing_algorithm = 0;
+
+ // Deployment stats
+ tm.dpl_ts = stats.dplTs;
+ tm.dpl_max_acc_ts = stats.dplMaxAccTs;
+ tm.dpl_alt = Meter{stats.dplAlt}.value();
+ tm.dpl_max_acc = MeterPerSecondSquared{stats.dplMaxAcc}.value();
+
+ // NAS reference values
+ tm.ref_lat = ref.refLatitude;
+ tm.ref_lon = ref.refLongitude;
+ tm.ref_alt = ref.refAltitude;
+
+ tm.min_pressure = -1.f;
+
+ // CPU stats
+ tm.cpu_load = cpuStats.mean;
+ tm.free_heap = cpuStats.freeHeap;
+
+ // Logger stats
+ tm.log_good = (loggerStats.lastWriteError == 0);
+ tm.log_number = loggerStats.logNumber;
+
+ tm.nas_state = static_cast<uint8_t>(nas->getState());
+ tm.wnc_state = 0;
+
+ // Pins, some not present on parafoil board
+ tm.pin_nosecone =
+ pinHandler->getPinData(PinList::NOSECONE_PIN).lastState;
+ tm.pin_launch = 0;
+ tm.cutter_presence = 0;
+ // Can handler, not present on parafoil board
+ tm.main_board_state = 0;
+ tm.motor_board_state = 0;
+
+ tm.main_can_status = 0;
+ tm.motor_can_status = 0;
+ tm.rig_can_status = 0;
+
+ tm.hil_state = 0;
+
+ mavlink_msg_payload_stats_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID,
+ &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_PIN_OBS_ID:
+ {
+ auto pinHandler = parent.getModule<PinHandler>();
+
+ for (auto pin : PinHandler::PIN_LIST)
+ {
+ mavlink_message_t msg;
+ mavlink_pin_tm_t tm;
+
+ auto pinData = pinHandler->getPinData(pin);
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.pin_id = static_cast<uint8_t>(pin);
+ tm.last_change_timestamp = pinData.lastStateTimestamp;
+ tm.changes_counter = pinData.changesCount;
+ tm.current_state = pinData.lastState;
+
+ mavlink_msg_pin_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg,
+ &tm);
+ enqueueMessage(msg);
+ }
+
+ return true;
+ }
+
+ case MAV_SENSORS_STATE_ID:
+ {
+ auto sensors = parent.getModule<Sensors>()->getSensorInfo();
+ for (auto sensor : sensors)
+ {
+ mavlink_message_t msg;
+ mavlink_sensor_state_tm_t tm;
+
+ constexpr size_t maxNameLen =
+ sizeof(tm.sensor_name) / sizeof(tm.sensor_name[0]) - 1;
+
+ std::strncpy(tm.sensor_name, sensor.id.c_str(), maxNameLen);
+ tm.sensor_name[maxNameLen] = '\0'; // Ensure null-termination
+ tm.initialized = sensor.isInitialized;
+ tm.enabled = sensor.isEnabled;
+
+ mavlink_msg_sensor_state_tm_encode(
+ config::Mavlink::SYSTEM_ID, config::Mavlink::COMPONENT_ID,
+ &msg, &tm);
+ enqueueMessage(msg);
+ }
+
+ return true;
+ }
+
+ case MAV_REFERENCE_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_reference_tm_t tm;
+
+ auto ref = parent.getModule<NASController>()->getReferenceValues();
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.ref_altitude = ref.refAltitude;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_temperature = ref.refTemperature;
+ tm.ref_latitude = ref.refLatitude;
+ tm.ref_longitude = ref.refLongitude;
+ tm.msl_pressure = ref.mslPressure;
+ tm.msl_temperature = ref.mslTemperature;
+
+ mavlink_msg_reference_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg,
+ &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_CALIBRATION_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_calibration_tm_t tm;
+
+ auto cal = parent.getModule<Sensors>()->getCalibrationData();
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.gyro_bias_x = 0.0f;
+ tm.gyro_bias_y = 0.0f;
+ tm.gyro_bias_z = 0.0f;
+ tm.mag_bias_x = cal.magBiasX;
+ tm.mag_bias_y = cal.magBiasY;
+ tm.mag_bias_z = cal.magBiasZ;
+ tm.mag_scale_x = cal.magScaleX;
+ tm.mag_scale_y = cal.magScaleY;
+ tm.mag_scale_z = cal.magScaleZ;
+ tm.static_press_1_bias = 0.0f;
+ tm.static_press_1_scale = 0.0f;
+ tm.static_press_2_bias = 0.0f;
+ tm.static_press_2_scale = 0.0f;
+ tm.dpl_bay_press_bias = 0.0f;
+ tm.dpl_bay_press_scale = 0.0f;
+ tm.dynamic_press_bias = 0.0f;
+ tm.dynamic_press_scale = 0.0f;
+
+ mavlink_msg_calibration_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID,
+ &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ default:
+ return false;
+ }
+}
+
+bool Radio::MavlinkBackend::enqueueSensorsTm(SensorsTMList tmId)
+{
+ switch (tmId)
+ {
+ case MAV_GPS_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_gps_tm_t tm;
+
+ auto sample = parent.getModule<Sensors>()->getUBXGPSLastSample();
+
+ tm.fix = sample.fix;
+ tm.height = sample.height;
+ tm.latitude = sample.latitude;
+ tm.longitude = sample.longitude;
+ tm.n_satellites = sample.satellites;
+ tm.speed = sample.speed;
+ tm.timestamp = sample.gpsTimestamp;
+ tm.track = sample.track;
+ tm.vel_down = sample.velocityDown;
+ tm.vel_east = sample.velocityEast;
+ tm.vel_north = sample.velocityNorth;
+ strcpy(tm.sensor_name, "UBXGPS");
+
+ mavlink_msg_gps_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case SensorsTMList::MAV_BMX160_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_imu_tm_t tm;
+
+ auto imuData = parent.getModule<Sensors>()
+ ->getBMX160WithCorrectionLastSample();
+
+ strcpy(tm.sensor_name, "BMX160");
+ tm.acc_x = imuData.accelerationX;
+ tm.acc_y = imuData.accelerationY;
+ tm.acc_z = imuData.accelerationZ;
+ tm.gyro_x = imuData.angularSpeedX;
+ tm.gyro_y = imuData.angularSpeedY;
+ tm.gyro_z = imuData.angularSpeedZ;
+ tm.mag_x = imuData.magneticFieldX;
+ tm.mag_y = imuData.magneticFieldY;
+ tm.mag_z = imuData.magneticFieldZ;
+ tm.timestamp = imuData.accelerationTimestamp;
+
+ mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg, &tm);
+
+ return true;
+ }
+
+ case SensorsTMList::MAV_LPS22DF_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_pressure_tm_t tm;
+
+ auto pressureData =
+ parent.getModule<Sensors>()->getLPS22DFLastSample();
+
+ tm.timestamp = pressureData.pressureTimestamp;
+ strcpy(tm.sensor_name, "LPS22");
+ tm.pressure = pressureData.pressure;
+
+ mavlink_msg_pressure_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg,
+ &tm);
+
+ return true;
+ }
+
+ case MAV_BATTERY_VOLTAGE_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_voltage_tm_t tm;
+
+ auto data = parent.getModule<Sensors>()->getBatteryVoltage();
+
+ tm.voltage = data.voltage;
+ tm.timestamp = data.voltageTimestamp;
+ strcpy(tm.sensor_name, "BatteryVoltage");
+
+ mavlink_msg_voltage_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg,
+ &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_ADS131M08_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_adc_tm_t tm;
+
+ auto sample = parent.getModule<Sensors>()->getADS131LastSample();
+
+ tm.channel_0 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
+ tm.channel_1 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
+ tm.channel_2 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
+ tm.channel_3 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
+ tm.channel_4 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
+ tm.channel_5 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
+ tm.channel_6 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
+ tm.channel_7 =
+ sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
+ tm.timestamp = sample.timestamp;
+ strcpy(tm.sensor_name, "ADS131M08");
+
+ mavlink_msg_adc_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_LIS3MDL_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_imu_tm_t tm;
+
+ auto sample = parent.getModule<Sensors>()->getLIS3MDLLastSample();
+
+ tm.acc_x = 0;
+ tm.acc_y = 0;
+ tm.acc_z = 0;
+ tm.gyro_x = 0;
+ tm.gyro_y = 0;
+ tm.gyro_z = 0;
+ tm.mag_x = sample.magneticFieldX;
+ tm.mag_y = sample.magneticFieldY;
+ tm.mag_z = sample.magneticFieldZ;
+ tm.timestamp = sample.magneticFieldTimestamp;
+ strcpy(tm.sensor_name, "LIS3MDL");
+
+ mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ case MAV_H3LIS331DL_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_imu_tm_t tm;
+
+ auto sample = parent.getModule<Sensors>()->getH3LISLastSample();
+
+ tm.mag_x = 0;
+ tm.mag_y = 0;
+ tm.mag_z = 0;
+ tm.gyro_x = 0;
+ tm.gyro_y = 0;
+ tm.gyro_z = 0;
+ tm.acc_x = sample.accelerationX;
+ tm.acc_y = sample.accelerationY;
+ tm.acc_z = sample.accelerationZ;
+ tm.timestamp = sample.accelerationTimestamp;
+ strcpy(tm.sensor_name, "H3LIS331DL");
+
+ mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
+ default:
+ return false;
+ }
+}
+
+} // namespace MockupMain
diff --git a/src/MockupMain/Radio/Radio.cpp b/src/MockupMain/Radio/Radio.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82c68fb9486f17970acbaa75938902f3bd579b30
--- /dev/null
+++ b/src/MockupMain/Radio/Radio.cpp
@@ -0,0 +1,228 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Basso
+ *
+ * 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 "Radio.h"
+
+#include <MockupMain/BoardScheduler.h>
+#include <MockupMain/Buses.h>
+#include <common/Radio.h>
+#include <radio/Xbee/APIFramesLog.h>
+#include <radio/Xbee/ATCommands.h>
+
+using namespace Boardcore;
+using namespace Boardcore::Units::Time;
+using namespace Common;
+namespace config = MockupMain::Config::Radio;
+
+namespace
+{
+
+// Static radio instance that will handle the radio interrupts
+std::atomic<Xbee::Xbee*> staticTransceiver{nullptr};
+inline void handleDioIRQ()
+{
+ auto transceiver = staticTransceiver.load();
+ if (transceiver)
+ transceiver->handleATTNInterrupt();
+}
+
+} // namespace
+
+void __attribute__((used)) EXTI1_IRQHandlerImpl() { handleDioIRQ(); }
+
+namespace MockupMain
+{
+
+Radio::~Radio()
+{
+ auto transceiverPtr = transceiver.get();
+ staticTransceiver.compare_exchange_strong(transceiverPtr, nullptr);
+}
+
+bool Radio::start()
+{
+ auto& scheduler = getModule<BoardScheduler>()->radio();
+
+ SPIBusConfig config{};
+ config.clockDivider = SPI::ClockDivider::DIV_16;
+
+ transceiver = std::make_unique<Xbee::Xbee>(
+ getModule<Buses>()->spi2, config, miosix::xbee::cs::getPin(),
+ miosix::xbee::attn::getPin(), miosix::xbee::reset::getPin());
+ transceiver->setOnFrameReceivedListener([this](Xbee::APIFrame& frame)
+ { handleXbeeFrame(frame); });
+
+ Xbee::setDataRate(*transceiver, Config::Radio::Xbee::ENABLE_80KPS_DATA_RATE,
+ Millisecond{Config::Radio::Xbee::TIMEOUT}.value());
+
+ // Set the static instance for handling radio interrupts
+ staticTransceiver = transceiver.get();
+
+ // Initialize the Mavlink driver
+ radioMavlink.driver = std::make_unique<MavDriver>(
+ transceiver.get(), [this](MavDriver*, const mavlink_message_t& msg)
+ { handleRadioMessage(msg); },
+ Millisecond{config::MavlinkDriver::SLEEP_AFTER_SEND}.value(),
+ Millisecond{config::MavlinkDriver::MAX_PKT_AGE}.value());
+
+ if (!radioMavlink.driver->start())
+ {
+ LOG_ERR(logger, "Failed to initialize the Mavlink driver");
+ return false;
+ }
+
+ // Add the high rate telemetry task
+ auto highRateTask =
+ scheduler.addTask([this]() { enqueueHighRateTelemetry(); },
+ Config::Radio::HIGH_RATE_TELEMETRY);
+
+ if (highRateTask == 0)
+ {
+ LOG_ERR(logger, "Failed to add the high rate telemetry task");
+ return false;
+ }
+
+ auto lowRateTask =
+ scheduler.addTask([this]() { enqueueLowRateTelemetry(); },
+ Config::Radio::LOW_RATE_TELEMETRY);
+
+ if (lowRateTask == 0)
+ {
+ LOG_ERR(logger, "Failed to add the low rate telemetry task");
+ return false;
+ }
+
+ started = true;
+ return true;
+}
+
+bool Radio::isStarted() { return started; }
+
+void Radio::handleXbeeFrame(Boardcore::Xbee::APIFrame& frame)
+{
+ using namespace Xbee;
+ bool logged = false;
+ switch (frame.frameType)
+ {
+ case FTYPE_AT_COMMAND:
+ {
+ ATCommandFrameLog dest;
+ logged = ATCommandFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ Logger::getInstance().log(dest);
+ break;
+ }
+ case FTYPE_AT_COMMAND_RESPONSE:
+ {
+ ATCommandResponseFrameLog dest;
+ logged = ATCommandResponseFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ Logger::getInstance().log(dest);
+ break;
+ }
+ case FTYPE_MODEM_STATUS:
+ {
+ ModemStatusFrameLog dest;
+ logged = ModemStatusFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ Logger::getInstance().log(dest);
+ break;
+ }
+ case FTYPE_TX_REQUEST:
+ {
+ TXRequestFrameLog dest;
+ logged = TXRequestFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ Logger::getInstance().log(dest);
+ break;
+ }
+ case FTYPE_TX_STATUS:
+ {
+ TXStatusFrameLog dest;
+ logged = TXStatusFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ Logger::getInstance().log(dest);
+ break;
+ }
+ case FTYPE_RX_PACKET_FRAME:
+ {
+ RXPacketFrameLog dest;
+ logged = RXPacketFrameLog::toFrameType(frame, &dest);
+ if (logged)
+ Logger::getInstance().log(dest);
+ break;
+ }
+ }
+
+ if (!logged)
+ {
+ APIFrameLog api;
+ APIFrameLog::fromAPIFrame(frame, &api);
+ Logger::getInstance().log(api);
+ }
+}
+
+void Radio::handleRadioMessage(const mavlink_message_t& msg)
+{
+ radioMavlink.handleMessage(msg);
+}
+
+void Radio::enqueueHighRateTelemetry()
+{
+ radioMavlink.enqueueSystemTm(MAV_FLIGHT_ID);
+ radioMavlink.flushQueue();
+}
+
+void Radio::enqueueLowRateTelemetry()
+{
+ radioMavlink.enqueueSystemTm(MAV_STATS_ID);
+}
+
+void Radio::MavlinkBackend::enqueueMessage(const mavlink_message_t& msg)
+{
+ miosix::Lock<miosix::FastMutex> lock(mutex);
+
+ // Insert the message inside the queue only if there is enough space
+ if (index < queue.size())
+ {
+ queue[index] = msg;
+ index++;
+ }
+}
+
+void Radio::MavlinkBackend::flushQueue()
+{
+ Lock<FastMutex> lock(mutex);
+
+ for (uint32_t i = 0; i < index; i++)
+ driver->enqueueMsg(queue[i]);
+
+ // Reset the index
+ index = 0;
+}
+
+void Radio::MavlinkBackend::logStatus()
+{
+ Logger::getInstance().log(driver->getStatus());
+}
+
+} // namespace MockupMain
diff --git a/src/MockupMain/Radio/Radio.h b/src/MockupMain/Radio/Radio.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf7034436387a8d1cef0cd1c2bbb48352d4273c3
--- /dev/null
+++ b/src/MockupMain/Radio/Radio.h
@@ -0,0 +1,119 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Basso
+ *
+ * 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 <MockupMain/Configs/RadioConfig.h>
+#include <common/MavlinkOrion.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/SerialTransceiver/SerialTransceiver.h>
+#include <radio/Xbee/Xbee.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace MockupMain
+{
+using MavDriver =
+ Boardcore::MavlinkDriver<Config::Radio::MavlinkDriver::PKT_LENGTH,
+ Config::Radio::MavlinkDriver::PKT_QUEUE_SIZE,
+ Config::Radio::MavlinkDriver::MSG_LENGTH>;
+
+class BoardScheduler;
+class Sensors;
+class Buses;
+class FlightModeManager;
+class NASController;
+class PinHandler;
+class FlightStatsRecorder;
+
+class Radio
+ : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors, Buses,
+ FlightModeManager, NASController,
+ PinHandler, FlightStatsRecorder>
+{
+public:
+ /**
+ * @brief Unsets the static instance for handling radio interrupts, if the
+ * current one was set by this radio instance.
+ */
+ ~Radio();
+
+ /**
+ * @brief Initializes the radio and Mavlink driver, and sets the static
+ * instance for handling radio interrupts.
+ */
+ [[nodiscard]] bool start();
+
+ bool isStarted();
+
+private:
+ struct MavlinkBackend
+ {
+ Radio& parent; // Reference to the parent Radio instance
+
+ std::unique_ptr<MavDriver> driver;
+
+ std::array<mavlink_message_t, Config::Radio::MESSAGE_QUEUE_SIZE> queue;
+ size_t index = 0;
+ miosix::FastMutex mutex;
+
+ void handleMessage(const mavlink_message_t& msg);
+ void handleCommand(const mavlink_message_t& msg);
+
+ void enqueueAck(const mavlink_message_t& msg);
+ void enqueueNack(const mavlink_message_t& msg);
+ void enqueueWack(const mavlink_message_t& msg);
+
+ bool enqueueSystemTm(SystemTMList tmId);
+ bool enqueueSensorsTm(SensorsTMList sensorId);
+
+ /**
+ * @brief Enqueues a message in the message queue.
+ */
+ void enqueueMessage(const mavlink_message_t& msg);
+
+ /**
+ * @brief Flushes the message queue to the driver.
+ */
+ void flushQueue();
+
+ /**
+ * @brief Logs the status of MavlinkDriver and the transceiver
+ */
+ void logStatus();
+ };
+
+ void handleXbeeFrame(Boardcore::Xbee::APIFrame& frame);
+
+ void handleRadioMessage(const mavlink_message_t& msg);
+
+ void enqueueHighRateTelemetry();
+ void enqueueLowRateTelemetry();
+
+ std::unique_ptr<Boardcore::Xbee::Xbee> transceiver;
+ MavlinkBackend radioMavlink{.parent = *this};
+
+ std::atomic<bool> started{false};
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/Sensors/AnalogLoadCellSensor.h b/src/MockupMain/Sensors/AnalogLoadCellSensor.h
new file mode 100644
index 0000000000000000000000000000000000000000..3aa25b97f03a7bb7d759518397c70e9e56a4d43c
--- /dev/null
+++ b/src/MockupMain/Sensors/AnalogLoadCellSensor.h
@@ -0,0 +1,74 @@
+/* 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 <sensors/Sensor.h>
+#include <sensors/SensorData.h>
+
+#include <functional>
+
+// The class was copied from rig-dev, commit 5c9464e5
+
+namespace Boardcore
+{
+
+class AnalogLoadCellSensor : public Boardcore::Sensor<Boardcore::LoadCellData>
+{
+public:
+ AnalogLoadCellSensor(std::function<Boardcore::ADCData()> getVoltage,
+ float p0Voltage, float p0Mass, float p1Voltage,
+ float p1Mass)
+ : getVoltage{getVoltage}, p0Voltage{p0Voltage}, p0Mass{p0Mass},
+ p1Voltage{p1Voltage}, p1Mass{p1Mass}
+ {
+ }
+
+ bool init() override { return true; }
+
+ bool selfTest() override { return true; }
+
+ float voltageToMass(float voltage)
+ {
+ // Two point calibration
+ // m = dmass/dvoltage
+ float scale = (p1Mass - p0Mass) / (p1Voltage - p0Voltage);
+ float offset = p0Mass - scale * p0Voltage; // Calculate offset
+ return scale * voltage + offset;
+ }
+
+private:
+ Boardcore::LoadCellData sampleImpl() override
+ {
+ auto voltage = getVoltage();
+ return {voltage.voltageTimestamp, -voltageToMass(voltage.voltage)};
+ }
+
+ std::function<Boardcore::ADCData()> getVoltage;
+
+ float p0Voltage;
+ float p0Mass;
+ float p1Voltage;
+ float p1Mass;
+};
+
+} // namespace Boardcore
diff --git a/src/MockupMain/Sensors/SensorData.h b/src/MockupMain/Sensors/SensorData.h
new file mode 100644
index 0000000000000000000000000000000000000000..650609a8cef58dc42368198614d03926048c7aee
--- /dev/null
+++ b/src/MockupMain/Sensors/SensorData.h
@@ -0,0 +1,53 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <sensors/SensorData.h>
+
+namespace MockupMain
+{
+
+struct SensorCalibrationData
+{
+ uint64_t timestamp = 0;
+ float magBiasX = 0.0f;
+ float magBiasY = 0.0f;
+ float magBiasZ = 0.0f;
+ float magScaleX = 0.0f;
+ float magScaleY = 0.0f;
+ float magScaleZ = 0.0f;
+
+ static std::string header()
+ {
+ return "timestamp,magBiasX,magBiasY,magBiasZ,magScaleX,magScaleY,"
+ "magScaleZ\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << magBiasX << "," << magBiasY << "," << magBiasZ
+ << "," << magScaleX << "," << magScaleY << "," << magScaleZ << "\n";
+ }
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/Sensors/Sensors.cpp b/src/MockupMain/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f74e2d34caf96dab180a409dbe2187dddb3f07d6
--- /dev/null
+++ b/src/MockupMain/Sensors/Sensors.cpp
@@ -0,0 +1,530 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 "Sensors.h"
+
+#include <MockupMain/BoardScheduler.h>
+#include <MockupMain/Buses.h>
+#include <MockupMain/FlightStatsRecorder/FlightStatsRecorder.h>
+
+#include <chrono>
+
+using namespace Boardcore;
+using namespace Boardcore::Units::Frequency;
+namespace config = MockupMain::Config::Sensors;
+namespace hwmap = miosix::sensors;
+
+namespace MockupMain
+{
+
+bool Sensors::start()
+{
+ if (config::BMX160::ENABLED)
+ {
+ bmx160Init();
+ bmx160WithCorrectionInit();
+ }
+
+ if (config::LIS3MDL::ENABLED)
+ lis3mdlInit();
+
+ if (config::H3LIS331DL::ENABLED)
+ h3lisInit();
+
+ if (config::LPS22DF::ENABLED)
+ lps22Init();
+
+ if (config::UBXGPS::ENABLED)
+ ubxGpsInit();
+
+ if (config::ADS131M08::ENABLED)
+ ads131Init();
+
+ if (config::InternalADC::ENABLED)
+ internalADCInit();
+
+ if (!postSensorCreationHook())
+ {
+ LOG_ERR(logger, "Sensors post-creation hook failed");
+ return false;
+ }
+
+ if (!sensorManagerInit())
+ {
+ LOG_ERR(logger, "Failed to initialize sensor manager");
+ return false;
+ }
+
+ auto& scheduler = getModule<BoardScheduler>()->sensors();
+
+ magCalibrationTaskId = scheduler.addTask(
+ [this]()
+ {
+ auto mag = getLIS3MDLLastSample();
+
+ miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+ magCalibrator.feed(mag);
+ },
+ config::MagCalibration::SAMPLING_RATE);
+
+ if (magCalibrationTaskId == 0)
+ {
+ LOG_ERR(logger, "Failed to add magnetometer calibration task");
+ return false;
+ }
+
+ // Immediately disable the task, so that is enabled only when needed
+ // from FlightModeManager. It is calibrated during the pre-flight
+ // initialization phase.
+ scheduler.disableTask(magCalibrationTaskId);
+
+ started = true;
+ return true;
+}
+
+bool Sensors::isStarted() { return started; }
+
+void Sensors::calibrate()
+{
+ bmx160WithCorrection->startCalibration();
+
+ miosix::Thread::sleep(
+ std::chrono::milliseconds{config::BMX160::CALIBRATION_DURATION}
+ .count());
+
+ bmx160WithCorrection->stopCalibration();
+}
+
+SensorCalibrationData Sensors::getCalibrationData()
+{
+ miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+
+ return SensorCalibrationData{
+ .timestamp = TimestampTimer::getTimestamp(),
+ .magBiasX = magCalibration.getb().x(),
+ .magBiasY = magCalibration.getb().y(),
+ .magBiasZ = magCalibration.getb().z(),
+ .magScaleX = magCalibration.getA().x(),
+ .magScaleY = magCalibration.getA().y(),
+ .magScaleZ = magCalibration.getA().z(),
+ };
+}
+
+void Sensors::resetMagCalibrator()
+{
+ miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+ magCalibrator = SoftAndHardIronCalibration{};
+}
+
+void Sensors::enableMagCalibrator()
+{
+ getModule<BoardScheduler>()->sensors().enableTask(magCalibrationTaskId);
+}
+
+void Sensors::disableMagCalibrator()
+{
+ getModule<BoardScheduler>()->sensors().disableTask(magCalibrationTaskId);
+}
+
+bool Sensors::saveMagCalibration()
+{
+ miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+
+ SixParametersCorrector calibration = magCalibrator.computeResult();
+
+ // Check if the calibration is valid
+ if (!std::isnan(calibration.getb()[0]) &&
+ !std::isnan(calibration.getb()[1]) &&
+ !std::isnan(calibration.getb()[2]) &&
+ !std::isnan(calibration.getA()[0]) &&
+ !std::isnan(calibration.getA()[1]) &&
+ !std::isnan(calibration.getA()[2]))
+ {
+ // Its valid, save it and apply it
+ magCalibration = calibration;
+ return magCalibration.toFile(config::MagCalibration::CALIBRATION_PATH);
+ }
+ else
+ {
+ return false;
+ }
+}
+
+BMX160Data Sensors::getBMX160LastSample()
+{
+ return bmx160 ? bmx160->getLastSample() : BMX160Data{};
+}
+
+BMX160WithCorrectionData Sensors::getBMX160WithCorrectionLastSample()
+{
+ return bmx160WithCorrection ? bmx160WithCorrection->getLastSample()
+ : BMX160WithCorrectionData{};
+}
+
+LIS3MDLData Sensors::getLIS3MDLLastSample()
+{
+ return lis3mdl ? lis3mdl->getLastSample() : LIS3MDLData{};
+}
+
+H3LIS331DLData Sensors::getH3LISLastSample()
+{
+ return h3lis331dl ? h3lis331dl->getLastSample() : H3LIS331DLData{};
+}
+
+LPS22DFData Sensors::getLPS22DFLastSample()
+{
+ return lps22df ? lps22df->getLastSample() : LPS22DFData{};
+}
+
+UBXGPSData Sensors::getUBXGPSLastSample()
+{
+ return ubxgps ? ubxgps->getLastSample() : UBXGPSData{};
+}
+
+ADS131M08Data Sensors::getADS131LastSample()
+{
+ return ads131m08 ? ads131m08->getLastSample() : ADS131M08Data{};
+}
+
+LoadCellData Sensors::getLoadCellLastSample()
+{
+ return loadCell ? loadCell->getLastSample() : LoadCellData{};
+}
+
+InternalADCData Sensors::getInternalADCLastSample()
+{
+ return internalAdc ? internalAdc->getLastSample() : InternalADCData{};
+}
+
+BatteryVoltageSensorData Sensors::getBatteryVoltage()
+{
+ auto sample = getInternalADCLastSample();
+
+ BatteryVoltageSensorData data;
+ data.voltageTimestamp = sample.timestamp;
+ data.channelId = static_cast<uint8_t>(config::InternalADC::VBAT_CH);
+ data.voltage = sample.voltage[config::InternalADC::VBAT_CH];
+ data.batVoltage = sample.voltage[config::InternalADC::VBAT_CH] *
+ config::InternalADC::VBAT_COEFF;
+
+ return data;
+}
+
+std::vector<SensorInfo> Sensors::getSensorInfo()
+{
+ if (manager)
+ {
+ std::vector<SensorInfo> infos{};
+
+#define PUSH_SENSOR_INFO(instance, name) \
+ if (instance) \
+ infos.push_back(manager->getSensorInfo(instance.get())); \
+ else \
+ infos.push_back( \
+ SensorInfo { #name, config::name::SAMPLING_RATE, nullptr, false })
+
+ PUSH_SENSOR_INFO(bmx160, BMX160);
+ PUSH_SENSOR_INFO(bmx160WithCorrection, BMX160);
+ PUSH_SENSOR_INFO(h3lis331dl, H3LIS331DL);
+ PUSH_SENSOR_INFO(lis3mdl, LIS3MDL);
+ PUSH_SENSOR_INFO(lps22df, LPS22DF);
+ PUSH_SENSOR_INFO(ubxgps, UBXGPS);
+ PUSH_SENSOR_INFO(ads131m08, ADS131M08);
+ PUSH_SENSOR_INFO(internalAdc, InternalADC);
+
+#undef PUSH_SENSOR_INFO
+
+ return infos;
+ }
+ else
+ {
+ return {};
+ }
+}
+
+void Sensors::bmx160Init()
+{
+ SPIBusConfig spiConfig;
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_4;
+
+ BMX160Config config;
+ config.fifoMode = BMX160Config::FifoMode::DISABLED;
+ config.fifoWatermark = config::BMX160::FIFO_WATERMARK;
+ config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+ config.temperatureDivider = config::BMX160::TEMP_DIVIDER;
+
+ config.accelerometerRange = config::BMX160::ACC_FSR;
+ config.gyroscopeRange = config::BMX160::GYRO_FSR;
+
+ config.accelerometerDataRate = config::BMX160::ACC_GYRO_ODR;
+ config.gyroscopeDataRate = config::BMX160::ACC_GYRO_ODR;
+ config.magnetometerRate = config::BMX160::MAG_ODR;
+
+ config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+ bmx160 = std::make_unique<BMX160>(getModule<Buses>()->spi1,
+ hwmap::bmx160::cs::getPin(), config,
+ spiConfig);
+
+ LOG_INFO(logger, "BMX160 initialized!");
+}
+
+void Sensors::bmx160WithCorrectionInit()
+{
+ bmx160WithCorrection = std::make_unique<BMX160WithCorrection>(
+ bmx160.get(), config::BMX160::AXIS_ORIENTATION);
+
+ LOG_INFO(logger, "BMX160WithCorrection initialized!");
+}
+
+void Sensors::h3lisInit()
+{
+ SPIBusConfig spiConfig;
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
+
+ h3lis331dl = std::make_unique<H3LIS331DL>(
+ getModule<Buses>()->spi1, hwmap::h3lis331dl::cs::getPin(), spiConfig,
+ config::H3LIS331DL::ODR, config::H3LIS331DL::BDU,
+ config::H3LIS331DL::FSR);
+
+ LOG_INFO(logger, "H3LIS331DL initialized!");
+}
+
+void Sensors::lis3mdlInit()
+{
+ SPIBusConfig spiConfig;
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+
+ LIS3MDL::Config config;
+ config.odr = config::LIS3MDL::ODR;
+ config.scale = config::LIS3MDL::FSR;
+ config.temperatureDivider = 1;
+
+ lis3mdl = std::make_unique<LIS3MDL>(getModule<Buses>()->spi1,
+ hwmap::lis3mdl::cs::getPin(), spiConfig,
+ config);
+
+ LOG_INFO(logger, "LIS3MDL initialized!");
+}
+
+void Sensors::lps22Init()
+{
+ auto spiConfig = LPS22DF::getDefaultSPIConfig();
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
+
+ LPS22DF::Config config;
+ config.avg = config::LPS22DF::AVG;
+ config.odr = config::LPS22DF::ODR;
+
+ lps22df = std::make_unique<LPS22DF>(getModule<Buses>()->spi1,
+ hwmap::lps22df::cs::getPin(), spiConfig,
+ config);
+
+ LOG_INFO(logger, "LPS22DF initialized!");
+}
+
+void Sensors::ubxGpsInit()
+{
+ auto spiConfig = UBXGPSSpi::getDefaultSPIConfig();
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_64;
+
+ ubxgps = std::make_unique<UBXGPSSpi>(
+ getModule<Buses>()->spi1, hwmap::ubxgps::cs::getPin(), spiConfig,
+ Kilohertz{config::UBXGPS::SAMPLING_RATE}.value());
+
+ LOG_INFO(logger, "UBXGPS initialized!");
+}
+
+void Sensors::ads131Init()
+{
+ SPIBusConfig spiConfig;
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+
+ ADS131M08::Config config;
+ config.oversamplingRatio = config::ADS131M08::OVERSAMPLING_RATIO;
+ config.globalChopModeEnabled = config::ADS131M08::GLOBAL_CHOP_MODE;
+
+ ads131m08 = std::make_unique<ADS131M08>(getModule<Buses>()->spi1,
+ hwmap::ads131m08::cs::getPin(),
+ spiConfig, config);
+
+ LOG_INFO(logger, "ADS131M08 initialized!");
+}
+
+void Sensors::loadCellInit()
+{
+ loadCell = std::make_unique<AnalogLoadCellSensor>(
+ [this]()
+ {
+ auto sample = getADS131LastSample();
+ return sample.getVoltage(config::LoadCell::ADC_CHANNEL);
+ },
+ config::LoadCell::Calibration::P0_VOLTAGE,
+ config::LoadCell::Calibration::P0_MASS,
+ config::LoadCell::Calibration::P1_VOLTAGE,
+ config::LoadCell::Calibration::P1_MASS);
+
+ LOG_INFO(logger, "LoadCell initialized!");
+}
+
+void Sensors::internalADCInit()
+{
+ internalAdc = std::make_unique<InternalADC>(ADC3);
+ internalAdc->enableChannel(config::InternalADC::VBAT_CH);
+ internalAdc->enableTemperature();
+ internalAdc->enableVbat();
+
+ LOG_INFO(logger, "InternalADC initialized!");
+}
+
+void Sensors::bmx160Callback()
+{
+ auto fifoSize = bmx160->getLastFifoSize();
+ auto& fifo = bmx160->getLastFifo();
+
+ Logger::getInstance().log(bmx160->getTemperature());
+
+ for (auto i = 0; i < fifoSize; i++)
+ Logger::getInstance().log(fifo.at(i));
+
+ Logger::getInstance().log(bmx160->getFifoStats());
+}
+
+void Sensors::bmx160WithCorrectionCallback()
+{
+ BMX160WithCorrectionData lastSample = bmx160WithCorrection->getLastSample();
+
+ // Update acceleration stats
+ getModule<FlightStatsRecorder>()->updateAcc(lastSample);
+
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::h3lisCallback()
+{
+ H3LIS331DLData lastSample = h3lis331dl->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::lis3mdlCallback()
+{
+ LIS3MDLData lastSample = lis3mdl->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::lps22Callback()
+{
+ LPS22DFData lastSample = lps22df->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::ubxGpsCallback()
+{
+ UBXGPSData lastSample = ubxgps->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::ads131Callback()
+{
+ ADS131M08Data lastSample = ads131m08->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::loadCellCallback()
+{
+ LoadCellData lastSample = loadCell->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::internalADCCallback()
+{
+ InternalADCData lastSample = internalAdc->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+bool Sensors::sensorManagerInit()
+{
+ SensorManager::SensorMap_t map;
+
+ if (bmx160)
+ {
+ SensorInfo info{"BMX160", config::BMX160::SAMPLING_RATE,
+ [this]() { bmx160Callback(); }};
+ map.emplace(bmx160.get(), info);
+ }
+
+ if (bmx160WithCorrection)
+ {
+ SensorInfo info{"BMX160WithCorrection", config::BMX160::SAMPLING_RATE,
+ [this]() { bmx160WithCorrectionCallback(); }};
+ map.emplace(bmx160WithCorrection.get(), info);
+ }
+
+ if (h3lis331dl)
+ {
+ SensorInfo info{"H3LIS331DL", config::H3LIS331DL::SAMPLING_RATE,
+ [this]() { h3lisCallback(); }};
+ map.emplace(h3lis331dl.get(), info);
+ }
+
+ if (lis3mdl)
+ {
+ SensorInfo info{"LIS3MDL", config::LIS3MDL::SAMPLING_RATE,
+ [this]() { lis3mdlCallback(); }};
+ map.emplace(lis3mdl.get(), info);
+ }
+
+ if (lps22df)
+ {
+ SensorInfo info{"LPS22DF", config::LPS22DF::SAMPLING_RATE,
+ [this]() { lps22Callback(); }};
+ map.emplace(lps22df.get(), info);
+ }
+
+ if (ubxgps)
+ {
+ SensorInfo info{"UBXGPS", config::UBXGPS::SAMPLING_RATE,
+ [this]() { ubxGpsCallback(); }};
+ map.emplace(ubxgps.get(), info);
+ }
+
+ if (ads131m08)
+ {
+ SensorInfo info{"ADS131M08", config::ADS131M08::SAMPLING_RATE,
+ [this]() { ads131Callback(); }};
+ map.emplace(ads131m08.get(), info);
+ }
+
+ if (internalAdc)
+ {
+ SensorInfo info{"InternalADC", config::InternalADC::SAMPLING_RATE,
+ [this]() { internalADCCallback(); }};
+ map.emplace(internalAdc.get(), info);
+ }
+
+ auto& scheduler = getModule<BoardScheduler>()->sensors();
+ manager = std::make_unique<SensorManager>(map, &scheduler);
+ return manager->start();
+}
+
+} // namespace MockupMain
diff --git a/src/MockupMain/Sensors/Sensors.h b/src/MockupMain/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c18acd0217358eaba202f943e6735a7d30763bf
--- /dev/null
+++ b/src/MockupMain/Sensors/Sensors.h
@@ -0,0 +1,188 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <MockupMain/Configs/SensorsConfig.h>
+#include <drivers/adc/InternalADC.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/BMX160/BMX160WithCorrection.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS3MDL/LIS3MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+#include <sensors/analog/BatteryVoltageSensor.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include "AnalogLoadCellSensor.h"
+#include "SensorData.h"
+
+namespace MockupMain
+{
+class BoardScheduler;
+class Buses;
+class FlightStatsRecorder;
+
+/**
+ * @brief Manages all the sensors of the parafoil board.
+ */
+class Sensors : public Boardcore::InjectableWithDeps<BoardScheduler, Buses,
+ FlightStatsRecorder>
+{
+public:
+ [[nodiscard]] bool start();
+
+ /**
+ * @brief Returns whether all enabled sensors are started and running.
+ */
+ bool isStarted();
+
+ /**
+ * @brief Calibrates the sensors that need calibration.
+ *
+ * @note This function is blocking.
+ */
+ void calibrate();
+
+ /**
+ * @brief Returns the current sensors calibration parameters.
+ *
+ * @internal Ensure mutexes are unlocked before calling this function.
+ */
+ SensorCalibrationData getCalibrationData();
+
+ /**
+ * @brief Resets the magnetometer calibration.
+ */
+ void resetMagCalibrator();
+
+ /**
+ * @brief Enables the magnetometer calibration task.
+ */
+ void enableMagCalibrator();
+
+ /**
+ * @brief Disables the magnetometer calibration task.
+ */
+ void disableMagCalibrator();
+
+ /**
+ * @brief Saves the current magnetometer calibration to file, overwriting
+ * the previous one, and sets it as the active calibration.
+ *
+ * @return Whether the calibration was saved successfully.
+ */
+ bool saveMagCalibration();
+
+ Boardcore::BMX160Data getBMX160LastSample();
+ Boardcore::BMX160WithCorrectionData getBMX160WithCorrectionLastSample();
+ Boardcore::H3LIS331DLData getH3LISLastSample();
+ Boardcore::LIS3MDLData getLIS3MDLLastSample();
+ Boardcore::LPS22DFData getLPS22DFLastSample();
+ Boardcore::UBXGPSData getUBXGPSLastSample();
+ Boardcore::ADS131M08Data getADS131LastSample();
+ Boardcore::InternalADCData getInternalADCLastSample();
+ Boardcore::LoadCellData getLoadCellLastSample();
+
+ Boardcore::BatteryVoltageSensorData getBatteryVoltage();
+
+ /**
+ * @brief Returns information about all sensors managed by this class
+ */
+ std::vector<Boardcore::SensorInfo> getSensorInfo();
+
+protected:
+ /**
+ * @brief A function that is called after all sensors have been created but
+ * before they are inserted into the sensor manager.
+ *
+ * It can be overridden by subclasses to perform additional setup on the
+ * sensors.
+ *
+ * @return Whether the additional setup was successful. If false is
+ * returned, initialization will stop immediately after returning from this
+ * function and the sensors will not be started.
+ */
+ virtual bool postSensorCreationHook() { return true; }
+
+ std::unique_ptr<Boardcore::BMX160> bmx160;
+ std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl;
+ std::unique_ptr<Boardcore::LIS3MDL> lis3mdl;
+ std::unique_ptr<Boardcore::LPS22DF> lps22df;
+ std::unique_ptr<Boardcore::UBXGPSSpi> ubxgps;
+ std::unique_ptr<Boardcore::ADS131M08> ads131m08;
+ std::unique_ptr<Boardcore::InternalADC> internalAdc;
+
+ std::unique_ptr<Boardcore::BMX160WithCorrection> bmx160WithCorrection;
+ std::unique_ptr<Boardcore::AnalogLoadCellSensor> loadCell;
+
+ std::unique_ptr<Boardcore::SensorManager> manager;
+
+private:
+ /**
+ * Sensors initialization and callback functions.
+ */
+
+ void bmx160Init();
+ void bmx160Callback();
+
+ void bmx160WithCorrectionInit();
+ void bmx160WithCorrectionCallback();
+
+ void h3lisInit();
+ void h3lisCallback();
+
+ void lis3mdlInit();
+ void lis3mdlCallback();
+
+ void lps22Init();
+ void lps22Callback();
+
+ void ubxGpsInit();
+ void ubxGpsCallback();
+
+ void ads131Init();
+ void ads131Callback();
+
+ void loadCellInit();
+ void loadCellCallback();
+
+ void internalADCInit();
+ void internalADCCallback();
+
+ bool sensorManagerInit();
+
+ // Live calibration of the magnetomer
+ miosix::FastMutex magCalibrationMutex;
+ Boardcore::SoftAndHardIronCalibration magCalibrator;
+ Boardcore::SixParametersCorrector magCalibration;
+ size_t magCalibrationTaskId = 0;
+
+ std::atomic<bool> started{false};
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/MockupMain/StateMachines/FlightModeManager/FlightModeManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08fe16fa0f93425d53b9941fe3ea109f0a402c51
--- /dev/null
+++ b/src/MockupMain/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -0,0 +1,550 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 "FlightModeManager.h"
+
+#include <MockupMain/BoardScheduler.h>
+#include <MockupMain/Configs/FlightModeManagerConfig.h>
+#include <MockupMain/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <MockupMain/Sensors/Sensors.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Common;
+using namespace Boardcore::Units::Time;
+namespace config = MockupMain::Config::FlightModeManager;
+
+namespace MockupMain
+{
+
+FlightModeManager::FlightModeManager()
+ : HSM(&FlightModeManager::PreFlight, miosix::STACK_DEFAULT_FOR_PTHREAD,
+ BoardScheduler::flightModeManagerPriority())
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+ EventBroker::getInstance().subscribe(this, TOPIC_FMM);
+ EventBroker::getInstance().subscribe(this, TOPIC_TMTC);
+ EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+}
+
+FlightModeManager::~FlightModeManager()
+{
+ EventBroker::getInstance().unsubscribe(this);
+}
+
+FlightModeManagerState FlightModeManager::getState() { return state; }
+
+bool FlightModeManager::isTestMode() const
+{
+ return state == FlightModeManagerState::READY_TEST_MODE;
+}
+
+State FlightModeManager::PreFlight(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+
+ case EV_INIT:
+ {
+ return transition(&FlightModeManager::PreFlightInit);
+ }
+
+ case TMTC_START_LOGGING:
+ {
+ Logger::getInstance().start();
+ return HANDLED;
+ }
+
+ case TMTC_STOP_LOGGING:
+ {
+ Logger::getInstance().stop();
+ return HANDLED;
+ }
+
+ case TMTC_RESET_BOARD:
+ {
+ Logger::getInstance().stop();
+ miosix::reboot();
+ __builtin_unreachable();
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::PreFlightInit(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(FlightModeManagerState::PRE_FLIGHT_INIT);
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::PreFlight);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case FMM_INIT_OK:
+ {
+ return transition(&FlightModeManager::PreFlightInitDone);
+ }
+
+ case FMM_INIT_ERROR:
+ {
+ return transition(&FlightModeManager::PreFlightInitError);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::PreFlightInitError(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(FlightModeManagerState::PRE_FLIGHT_INIT_ERROR);
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::PreFlight);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case TMTC_FORCE_INIT:
+ {
+ return transition(&FlightModeManager::PreFlightInitDone);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::PreFlightInitDone(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(FlightModeManagerState::PRE_FLIGHT_INIT_DONE);
+ EventBroker::getInstance().post(FMM_CALIBRATE, TOPIC_FMM);
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::PreFlight);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case FMM_CALIBRATE:
+ {
+ return transition(&FlightModeManager::PreFlightSensorCalibration);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::PreFlightSensorCalibration(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(
+ FlightModeManagerState::PRE_FLIGHT_ALGORITHM_CALIBRATION);
+
+ // Wait for sensors to stabilize before calibration
+ // The first few LPS28DFW samples contain garbage data
+ miosix::Thread::sleep(100);
+ getModule<Sensors>()->calibrate();
+
+ EventBroker::getInstance().post(FMM_ALGOS_CALIBRATE, TOPIC_FMM);
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::PreFlight);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case FMM_ALGOS_CALIBRATE:
+ {
+ return transition(
+ &FlightModeManager::PreFlightAlgorithmCalibration);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::PreFlightAlgorithmCalibration(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(
+ FlightModeManagerState::PRE_FLIGHT_ALGORITHM_CALIBRATION);
+ // Calibrate after a delay to allow calibrated sensors to stabilize
+ EventBroker::getInstance().postDelayed(NAS_CALIBRATE, TOPIC_NAS,
+ 100);
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::PreFlight);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case NAS_READY:
+ {
+ EventBroker::getInstance().post(FMM_READY, TOPIC_FMM);
+ return transition(&FlightModeManager::Ready);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::Ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(FlightModeManagerState::READY);
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case TMTC_ENTER_TEST_MODE:
+ {
+ return transition(&FlightModeManager::ReadyTestMode);
+ }
+
+ case TMTC_CALIBRATE:
+ {
+ return transition(&FlightModeManager::PreFlightSensorCalibration);
+ }
+
+ case FLIGHT_NC_DETACHED:
+ case TMTC_FORCE_EXPULSION:
+ {
+ return transition(&FlightModeManager::FlyingDrogueDescent);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::ReadyTestMode(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(FlightModeManagerState::READY_TEST_MODE);
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::Ready);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case TMTC_FORCE_LANDING:
+ {
+ return transition(&FlightModeManager::Landed);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::Flying(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+
+ case EV_INIT:
+ {
+ return transition(&FlightModeManager::FlyingDrogueDescent);
+ }
+
+ case TMTC_FORCE_LANDING:
+ {
+ return transition(&FlightModeManager::Landed);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::FlyingDrogueDescent(const Event& event)
+{
+ static uint16_t controlDelayId;
+
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(FlightModeManagerState::FLYING_DROGUE_DESCENT);
+ // Send the event to the WingController
+ controlDelayId = EventBroker::getInstance().postDelayed(
+ FLIGHT_DROGUE_DESCENT, TOPIC_FLIGHT,
+ Millisecond{config::CONTROL_DELAY}.value());
+
+ getModule<FlightStatsRecorder>()->dropDetected(
+ TimestampTimer::getTimestamp());
+
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ EventBroker::getInstance().removeDelayed(controlDelayId);
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::Flying);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case TMTC_FORCE_LANDING:
+ {
+ return transition(&FlightModeManager::Landed);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::Landed(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(FlightModeManagerState::LANDED);
+
+ EventBroker::getInstance().post(FLIGHT_LANDING_DETECTED,
+ TOPIC_FLIGHT);
+ EventBroker::getInstance().postDelayed(
+ FMM_STOP_LOGGING, TOPIC_FMM,
+ Millisecond{config::LOGGING_DELAY}.value());
+
+ return HANDLED;
+ }
+
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+
+ case FMM_STOP_LOGGING:
+ {
+ Logger::getInstance().stop();
+ return HANDLED;
+ }
+
+ case TMTC_RESET_BOARD:
+ {
+ Logger::getInstance().stop();
+ miosix::reboot();
+ __builtin_unreachable();
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+void FlightModeManager::updateState(FlightModeManagerState newState)
+{
+ state = newState;
+
+ auto status = FlightModeManagerStatus{
+ .timestamp = TimestampTimer::getTimestamp(),
+ .state = newState,
+ };
+ Logger::getInstance().log(status);
+}
+
+} // namespace MockupMain
diff --git a/src/MockupMain/StateMachines/FlightModeManager/FlightModeManager.h b/src/MockupMain/StateMachines/FlightModeManager/FlightModeManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..896b7b2615d6789fd305eb7b6e43705bbdec33d4
--- /dev/null
+++ b/src/MockupMain/StateMachines/FlightModeManager/FlightModeManager.h
@@ -0,0 +1,154 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <events/HSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include "FlightModeManagerData.h"
+
+namespace MockupMain
+{
+class Sensors;
+class Actuators;
+class FlightStatsRecorder;
+
+/**
+ * State machine that manages the flight modes of the Parafoil.
+ *
+ * HSM Schema:
+ *
+ * PreFlight
+ * ├── PreFlightInit
+ * ├── PreFlightInitError
+ * ├── PreFlightInitDone
+ * ├── PreFlightSensorCalibration
+ * └── PreFlightAlgorithmCalibration
+ *
+ * Ready
+ * └── ReadyTestMode
+ *
+ * Flying
+ * └── FlyingDrogueDescent
+ *
+ * Landed
+ */
+class FlightModeManager
+ : public Boardcore::HSM<FlightModeManager>,
+ public Boardcore::InjectableWithDeps<Sensors, Actuators,
+ FlightStatsRecorder>
+{
+public:
+ FlightModeManager();
+ ~FlightModeManager();
+
+ /**
+ * @brief Returns the current state of the FlightModeManager.
+ */
+ FlightModeManagerState getState();
+
+ bool isTestMode() const;
+
+ /**
+ * @brief Super state for when the parafoil has not yet ready for launch.
+ */
+ Boardcore::State PreFlight(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the parafoil is initializing.
+ *
+ * Super state: PreFlight
+ */
+ Boardcore::State PreFlightInit(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the init has failed
+ *
+ * Super state: PreFlight
+ */
+ Boardcore::State PreFlightInitError(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the init is done and a calibration event is
+ * thrown
+ *
+ * Super state: PreFlight
+ */
+ Boardcore::State PreFlightInitDone(const Boardcore::Event& event);
+
+ /**
+ * @brief Calibration of all sensors.
+ *
+ * Super state: PreFlight
+ */
+ Boardcore::State PreFlightSensorCalibration(const Boardcore::Event& event);
+
+ /**
+ * @brief Calibration of all algorithms.
+ *
+ * Super state: PreFlight
+ */
+ Boardcore::State PreFlightAlgorithmCalibration(
+ const Boardcore::Event& event);
+
+ /**
+ * @brief Super state in which the parafoil is waiting to be dropped.
+ */
+ Boardcore::State Ready(const Boardcore::Event& event);
+
+ /**
+ * @brief The parafoil will accept specific telecommands otherwise
+ * considered risky.
+ *
+ * Super state: Ready
+ */
+ Boardcore::State ReadyTestMode(const Boardcore::Event& event);
+
+ /**
+ * @brief Super state for when the Parafoil is flying.
+ */
+ Boardcore::State Flying(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the parafoil wing is opened and starts guiding
+ * itself
+ *
+ * Super state: Flying
+ */
+ Boardcore::State FlyingDrogueDescent(const Boardcore::Event& event);
+
+ /**
+ * @brief The parafoil ended the flight and closes the log.
+ */
+ Boardcore::State Landed(const Boardcore::Event& event);
+
+private:
+ void updateState(FlightModeManagerState newState);
+
+ std::atomic<FlightModeManagerState> state{
+ FlightModeManagerState::PRE_FLIGHT};
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("FMM");
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/MockupMain/StateMachines/FlightModeManager/FlightModeManagerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..89ce930c0511d9610fbd1dba4f4851676ab213a2
--- /dev/null
+++ b/src/MockupMain/StateMachines/FlightModeManager/FlightModeManagerData.h
@@ -0,0 +1,60 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <ostream>
+#include <string>
+
+namespace MockupMain
+{
+
+enum class FlightModeManagerState : uint8_t
+{
+ PRE_FLIGHT = 0,
+ PRE_FLIGHT_INIT,
+ PRE_FLIGHT_INIT_ERROR,
+ PRE_FLIGHT_INIT_DONE,
+ PRE_FLIGHT_SENSOR_CALIBRATION,
+ PRE_FLIGHT_ALGORITHM_CALIBRATION,
+ PRE_FLIGHT_DISARMED,
+ READY = 8, // ARMED
+ READY_TEST_MODE = 7, // TEST_MODE
+ FLYING_DROGUE_DESCENT = 10, // DROGUE_DESCENT
+ LANDED = 12, // LANDED
+};
+
+struct FlightModeManagerStatus
+{
+ uint64_t timestamp = 0;
+ FlightModeManagerState state = FlightModeManagerState::PRE_FLIGHT;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)state << "\n";
+ }
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/StateMachines/NASController/NASController.cpp b/src/MockupMain/StateMachines/NASController/NASController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9247a757ddfe0b780bf5fa1102adac18d25c925
--- /dev/null
+++ b/src/MockupMain/StateMachines/NASController/NASController.cpp
@@ -0,0 +1,370 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Mor, Davide Basso
+ *
+ * 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 <MockupMain/BoardScheduler.h>
+#include <MockupMain/Configs/NASConfig.h>
+#include <MockupMain/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <MockupMain/Sensors/Sensors.h>
+#include <MockupMain/StateMachines/NASController/NASController.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <common/Events.h>
+#include <common/ReferenceConfig.h>
+#include <events/EventBroker.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+using namespace Boardcore;
+using namespace Boardcore::Units::Time;
+using namespace Boardcore::Units::Length;
+using namespace Boardcore::Units::Acceleration;
+using namespace Eigen;
+using namespace Common;
+namespace config = MockupMain::Config::NAS;
+
+namespace MockupMain
+{
+
+NASController::NASController()
+ : FSM(&NASController::Init, miosix::STACK_DEFAULT_FOR_PTHREAD,
+ BoardScheduler::nasControllerPriority()),
+ nas(config::CONFIG)
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
+
+bool NASController::start()
+{
+ // Setup the NAS
+ Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+ // Create the initial quaternion
+ Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+
+ // Set the initial quaternion inside the matrix
+ x(NAS::IDX_QUAT + 0) = q(0);
+ x(NAS::IDX_QUAT + 1) = q(1);
+ x(NAS::IDX_QUAT + 2) = q(2);
+ x(NAS::IDX_QUAT + 3) = q(3);
+
+ // Set the NAS x matrix
+ nas.setX(x);
+ // Set the initial reference values from the default ones
+ nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
+
+ auto& scheduler = getModule<BoardScheduler>()->nasController();
+ // Add the task to the scheduler
+ auto task = scheduler.addTask([this] { update(); }, config::UPDATE_RATE,
+ TaskScheduler::Policy::RECOVER);
+
+ if (task == 0)
+ {
+ LOG_ERR(logger, "Failed to add NAS update task");
+ return false;
+ }
+
+ if (!FSM::start())
+ {
+ LOG_ERR(logger, "Failed to start NASController FSM active object");
+ return false;
+ }
+
+ started = true;
+ return true;
+}
+
+NASState NASController::getNasState()
+{
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+ return nas.getState();
+}
+
+ReferenceValues NASController::getReferenceValues()
+{
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+ return nas.getReferenceValues();
+}
+
+NASControllerState NASController::getState() { return state; }
+
+void NASController::setOrientation(const Eigen::Quaternionf& quat)
+{
+ Lock<FastMutex> lock{nasMutex};
+
+ auto x = nas.getX();
+ x(NAS::IDX_QUAT + 0) = quat.x();
+ x(NAS::IDX_QUAT + 1) = quat.y();
+ x(NAS::IDX_QUAT + 2) = quat.z();
+ x(NAS::IDX_QUAT + 3) = quat.w();
+ nas.setX(x);
+}
+
+void NASController::Init(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(NASControllerState::INIT);
+ break;
+ }
+
+ case NAS_CALIBRATE:
+ {
+ transition(&NASController::Calibrating);
+ break;
+ }
+ }
+}
+
+void NASController::Calibrating(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(NASControllerState::CALIBRATING);
+ calibrate();
+ break;
+ }
+
+ case NAS_READY:
+ {
+ transition(&NASController::Ready);
+ break;
+ }
+ }
+}
+
+// State skipped on entry because we don't have Armed and Disarmed
+// states in the FMM
+void NASController::Ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(NASControllerState::READY);
+ transition(&NASController::Active);
+ break;
+ }
+ }
+}
+
+void NASController::Active(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(NASControllerState::ACTIVE);
+ break;
+ }
+
+ case NAS_CALIBRATE:
+ {
+ transition(&NASController::Calibrating);
+ break;
+ }
+
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&NASController::End);
+ break;
+ }
+ }
+}
+
+void NASController::End(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateState(NASControllerState::END);
+ break;
+ }
+ }
+}
+
+void NASController::calibrate()
+{
+ Sensors* sensors = getModule<Sensors>();
+
+ Vector3f accSum = Vector3f::Zero();
+ Vector3f magSum = Vector3f::Zero();
+ float baroSum = 0.0f;
+
+ for (int i = 0; i < config::CALIBRATION_SAMPLES_COUNT; i++)
+ {
+ BMX160Data imu = sensors->getBMX160WithCorrectionLastSample();
+ PressureData baro = sensors->getLPS22DFLastSample();
+
+ accSum +=
+ Vector3f{imu.accelerationX, imu.accelerationY, imu.accelerationZ};
+ magSum += Vector3f{imu.magneticFieldX, imu.magneticFieldY,
+ imu.magneticFieldZ};
+
+ baroSum += baro.pressure;
+
+ Thread::sleep(Millisecond{config::CALIBRATION_SLEEP_TIME}.value());
+ }
+
+ Vector3f meanAcc = accSum / config::CALIBRATION_SAMPLES_COUNT;
+ meanAcc.normalize();
+ Vector3f meanMag = magSum / config::CALIBRATION_SAMPLES_COUNT;
+ meanMag.normalize();
+ float meanBaro = baroSum / config::CALIBRATION_SAMPLES_COUNT;
+
+ // Use the triad to compute initial state
+ StateInitializer init;
+ init.triad(meanAcc, meanMag, ReferenceConfig::nedMag);
+
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+ // Compute reference values
+ ReferenceValues reference = nas.getReferenceValues();
+ reference.refPressure = meanBaro;
+ reference.refAltitude = Aeroutils::relAltitude(
+ reference.refPressure, reference.mslPressure, reference.mslTemperature);
+
+ // Also update the reference with the GPS if we have fix
+ UBXGPSData gps = sensors->getUBXGPSLastSample();
+ if (gps.fix == 3)
+ {
+ // Don't use the GPS altitude because it is not reliable
+ reference.refLatitude = gps.latitude;
+ reference.refLongitude = gps.longitude;
+ }
+
+ // Update the algorithm reference values
+ nas.setX(init.getInitX());
+ nas.setReferenceValues(reference);
+
+ EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
+}
+
+void NASController::update()
+{
+ // Update the NAS state only if the FSM is active
+ if (state != NASControllerState::ACTIVE)
+ return;
+
+ auto* sensors = getModule<Sensors>();
+
+ auto imu = sensors->getBMX160WithCorrectionLastSample();
+ auto gps = sensors->getUBXGPSLastSample();
+ auto baro = sensors->getLPS22DFLastSample();
+
+ // Calculate acceleration
+ Vector3f acc = static_cast<AccelerometerData>(imu);
+ auto accLength = MeterPerSecondSquared{acc.norm()};
+
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+ // Perform initial NAS prediction
+ nas.predictGyro(imu);
+ nas.predictAcc(imu);
+
+ // NOTE: Magnetometer correction has been disabled
+
+ if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 &&
+ accLength < Config::NAS::DISABLE_GPS_ACCELERATION_THRESHOLD)
+ {
+ nas.correctGPS(gps);
+ }
+
+ if (lastBaroTimestamp < baro.pressureTimestamp)
+ nas.correctBaro(baro.pressure);
+
+ // Correct with accelerometer if the acceleration is in specs
+ if (lastAccTimestamp < imu.accelerationTimestamp && acc1g)
+ nas.correctAcc(imu);
+
+ // Check if the accelerometer is measuring 1g
+ if (accLength < (MeterPerSecondSquared{G{1}} +
+ Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) &&
+ accLength > (MeterPerSecondSquared{G{1}} -
+ Config::NAS::ACCELERATION_1G_CONFIDENCE / 2))
+ {
+ if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES)
+ acc1gSamplesCount++;
+ else
+ acc1g = true;
+ }
+ else
+ {
+ acc1gSamplesCount = 0;
+ acc1g = false;
+ }
+
+ lastGyroTimestamp = imu.angularSpeedTimestamp;
+ lastAccTimestamp = imu.accelerationTimestamp;
+ lastMagTimestamp = imu.magneticFieldTimestamp;
+ lastGpsTimestamp = gps.gpsTimestamp;
+ lastBaroTimestamp = baro.pressureTimestamp;
+
+ auto state = nas.getState();
+ auto ref = nas.getReferenceValues();
+
+ getModule<FlightStatsRecorder>()->updateNas(state, ref.refTemperature);
+ Logger::getInstance().log(state);
+}
+
+void NASController::updateState(NASControllerState newState)
+{
+ state = newState;
+
+ auto status = NASControllerStatus{
+ .timestamp = TimestampTimer::getTimestamp(),
+ .state = newState,
+ };
+ Logger::getInstance().log(status);
+}
+
+void NASController::setReferenceAltitude(Meter altitude)
+{
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+ auto ref = nas.getReferenceValues();
+ ref.refAltitude = altitude.value();
+ nas.setReferenceValues(ref);
+}
+
+void NASController::setReferenceTemperature(float temperature)
+{
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+ auto ref = nas.getReferenceValues();
+ ref.refTemperature = temperature;
+ nas.setReferenceValues(ref);
+}
+
+void NASController::setReferenceCoordinates(float latitude, float longitude)
+{
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+ auto ref = nas.getReferenceValues();
+ ref.refLatitude = latitude;
+ ref.refLongitude = longitude;
+ nas.setReferenceValues(ref);
+}
+
+} // namespace MockupMain
diff --git a/src/MockupMain/StateMachines/NASController/NASController.h b/src/MockupMain/StateMachines/NASController/NASController.h
new file mode 100644
index 0000000000000000000000000000000000000000..688cb1479ce126924ab0f905fec801cc14b68c73
--- /dev/null
+++ b/src/MockupMain/StateMachines/NASController/NASController.h
@@ -0,0 +1,106 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Basso
+ *
+ * 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 <algorithms/NAS/NAS.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include "NASControllerData.h"
+
+namespace MockupMain
+{
+class BoardScheduler;
+class Sensors;
+class FlightStatsRecorder;
+
+class NASController
+ : public Boardcore::FSM<NASController>,
+ public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
+ FlightStatsRecorder>
+{
+public:
+ /**
+ * @brief Initializes the NAS controller.
+ *
+ * Sets the initial FSM state to idle, subscribes to NAS and flight events
+ * and initializes the NAS algorithm.
+ */
+ NASController();
+
+ /**
+ * @brief Adds the NAS update function into the scheduler and starts the FSM
+ * thread
+ */
+ bool start() override;
+
+ Boardcore::NASState getNasState();
+ Boardcore::ReferenceValues getReferenceValues();
+
+ NASControllerState getState();
+
+ void setOrientation(const Eigen::Quaternionf& orientation);
+
+ void setReferenceAltitude(Boardcore::Units::Length::Meter altitude);
+ void setReferenceTemperature(float temperature);
+ void setReferenceCoordinates(float latitude, float longitude);
+
+private:
+ void calibrate();
+
+ /**
+ * @brief Update the NAS estimation
+ */
+ void update();
+
+ // FSM states
+ void Init(const Boardcore::Event& event);
+ void Calibrating(const Boardcore::Event& event);
+ void Ready(const Boardcore::Event& event);
+ void Active(const Boardcore::Event& event);
+ void End(const Boardcore::Event& event);
+
+ void updateState(NASControllerState newState);
+
+ std::atomic<NASControllerState> state{NASControllerState::INIT};
+
+ Boardcore::NAS nas; ///< The NAS algorithm instance
+ miosix::FastMutex nasMutex;
+
+ int magDecimateCount = 0;
+ int acc1gSamplesCount = 0;
+ bool acc1g = false;
+
+ uint64_t lastGyroTimestamp = 0;
+ uint64_t lastAccTimestamp = 0;
+ uint64_t lastMagTimestamp = 0;
+ uint64_t lastGpsTimestamp = 0;
+ uint64_t lastBaroTimestamp = 0;
+
+ std::atomic<bool> started{false};
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("NAS");
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/StateMachines/NASController/NASControllerData.h b/src/MockupMain/StateMachines/NASController/NASControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..c60bfce9668d49766f207c92e46f2962b3095258
--- /dev/null
+++ b/src/MockupMain/StateMachines/NASController/NASControllerData.h
@@ -0,0 +1,54 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Basso
+ *
+ * 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 <ostream>
+#include <string>
+
+namespace MockupMain
+{
+
+enum class NASControllerState : uint8_t
+{
+ INIT = 0,
+ CALIBRATING,
+ READY,
+ ACTIVE,
+ END
+};
+
+struct NASControllerStatus
+{
+ uint64_t timestamp = 0;
+ NASControllerState state = NASControllerState::INIT;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)state << "\n";
+ }
+};
+
+} // namespace MockupMain
diff --git a/src/MockupMain/mockup-entry.cpp b/src/MockupMain/mockup-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ea06da34b5cc5ddfb6ff4a99a39999a7b441190
--- /dev/null
+++ b/src/MockupMain/mockup-entry.cpp
@@ -0,0 +1,174 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <MockupMain/BoardScheduler.h>
+#include <MockupMain/Buses.h>
+#include <MockupMain/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <MockupMain/PinHandler/PinHandler.h>
+#include <MockupMain/Radio/Radio.h>
+#include <MockupMain/Sensors/Sensors.h>
+#include <MockupMain/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <MockupMain/StateMachines/NASController/NASController.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <diagnostic/PrintLogger.h>
+#include <diagnostic/StackLogger.h>
+#include <events/EventBroker.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include <iomanip>
+#include <iostream>
+
+/**
+ * @brief Starts a module and checks if it started correctly.
+ * Must be followed by a semicolon or a block of code.
+ * The block of code will be executed only if the module started correctly.
+ *
+ * @example START_MODULE(sensors) { miosix::ledOn(); }
+ */
+#define START_MODULE(module) \
+ std::cout << "Starting " #module << std::endl; \
+ if (!module->start()) \
+ { \
+ initResult = false; \
+ std::cerr << "*** Failed to start " #module " ***" << std::endl; \
+ } \
+ else
+
+/**
+ * @brief Starts a singleton and checks if it started correctly.
+ * Must be followed by a semicolon or a block of code.
+ * The block of code will be executed only if the singleton started correctly.
+ *
+ * @example `START_SINGLETON(Logger) { miosix::ledOn(); }`
+ */
+#define START_SINGLETON(singleton) \
+ std::cout << "Starting " #singleton << std::endl; \
+ if (!singleton::getInstance().start()) \
+ { \
+ initResult = false; \
+ std::cerr << "*** Failed to start " #singleton " ***" << std::endl; \
+ }
+
+// Build type string for printing during startup
+#if defined(DEBUG)
+#define BUILD_TYPE "Debug"
+#else
+#define BUILD_TYPE "Release"
+#endif
+
+using namespace Boardcore;
+using namespace MockupMain;
+using namespace Common;
+
+int main()
+{
+ miosix::ledOff();
+ std::cout << "MockupMain Entrypoint " << "(" << BUILD_TYPE << ")"
+ << " by Skyward Experimental Rocketry" << std::endl;
+
+ // cppcheck-suppress unreadVariable
+ auto logger = Logging::getLogger("Parafoil");
+ DependencyManager depman{};
+
+ std::cout << "Instantiating modules" << std::endl;
+ bool initResult = true;
+
+ // Core components
+ auto buses = new Buses();
+ initResult &= depman.insert(buses);
+ auto scheduler = new BoardScheduler();
+ initResult &= depman.insert(scheduler);
+
+ // Global state machine
+ auto flightModeManager = new FlightModeManager();
+ initResult &= depman.insert(flightModeManager);
+
+ // Attitude estimation
+ auto nasController = new NASController();
+ initResult &= depman.insert(nasController);
+
+ // Sensors
+ auto sensors = new Sensors();
+ initResult &= depman.insert(sensors);
+ auto pinHandler = new PinHandler();
+ initResult &= depman.insert(pinHandler);
+
+ // Radio
+ auto radio = new Radio();
+ initResult &= depman.insert(radio);
+
+ // Stats recorder
+ auto flightStatsRecorder = new FlightStatsRecorder();
+ initResult &= depman.insert(flightStatsRecorder);
+
+ std::cout << "Injecting module dependencies" << std::endl;
+ initResult &= depman.inject();
+
+ START_SINGLETON(Logger)
+ {
+ std::cout << "Logger Ok!\n"
+ << "\tLog number: "
+ << Logger::getInstance().getCurrentLogNumber() << std::endl;
+ }
+ START_SINGLETON(EventBroker);
+
+ START_MODULE(pinHandler);
+ START_MODULE(radio);
+ START_MODULE(nasController);
+ START_MODULE(flightModeManager);
+
+ START_MODULE(scheduler);
+
+ START_MODULE(sensors);
+
+ if (initResult)
+ {
+ EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+ // Turn on the initialization led on the CU
+ miosix::ledOn();
+ std::cout << "Parafoil initialization Ok!" << std::endl;
+ }
+ else
+ {
+ EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
+ std::cerr << "*** Parafoil initialization error ***" << std::endl;
+ }
+
+ std::cout << "Sensors status:" << std::endl;
+ auto sensorInfo = sensors->getSensorInfo();
+ for (const auto& info : sensorInfo)
+ {
+ std::cout << "\t" << std::setw(16) << std::left << info.id << " "
+ << (info.isInitialized ? "Ok" : "Error") << "\n";
+ }
+ std::cout.flush();
+
+ // Collect stack usage statistics
+ while (true)
+ {
+ StackLogger::getInstance().log();
+ Thread::sleep(1000);
+ }
+
+ return 0;
+}