diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 15e54a0346b313371b936c7395834a970304fefd..d68ffbe1ce3db7349a6c6e1c9b3bdfeea49c7b29 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -114,7 +114,7 @@
"name": "stm32f767zi_death_stack_v4",
"cStandard": "c11",
"cppStandard": "c++14",
- "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+ "compilerPath": null,
"defines": [
"${defaultDefines}",
"_ARCH_CORTEXM7_STM32F7",
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 9f619c4c25cfc6271094ec17fa8889e1437a618a..b6b414798079ef19106c44c7252da132a6b6999d 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -193,5 +193,6 @@
"CMAKE_C_COMPILER_LAUNCHER": "ccache",
"CMAKE_CXX_COMPILER_LAUNCHER": "ccache"
},
- "compilerexplorer.compilationDirectory": "${workspaceFolder}/build"
+ "compilerexplorer.compilationDirectory": "${workspaceFolder}/build",
+ "C_Cpp.default.compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++"
}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e2a8cd631c663dc8ae3d578c786b9537531ef75f..a3d1c0d95c5290e7b130fb5a023132dda2eea5fe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -64,10 +64,6 @@ target_include_directories(payload-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
target_compile_definitions(payload-entry-euroc PRIVATE EUROC)
sbs_target(payload-entry-euroc stm32f767zi_lyra_biscotto)
-add_executable(parafoil-entry src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
-target_include_directories(parafoil-entry PRIVATE ${OBSW_INCLUDE_DIRS})
-sbs_target(parafoil-entry stm32f429zi_death_stack_v2)
-
add_executable(motor-entry src/Motor/motor-entry.cpp ${MOTOR_SOURCES})
target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(motor-entry stm32f767zi_lyra_motor)
@@ -115,3 +111,30 @@ 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
+
+add_executable(parafoil-progressive-rotation src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
+target_include_directories(parafoil-progressive-rotation PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-progressive-rotation PRIVATE ALGORITHM_PROGRESSIVE_ROTATION MILANO)
+sbs_target(parafoil-progressive-rotation stm32f429zi_death_stack_v2)
+
+add_executable(parafoil-guided-milano src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
+target_include_directories(parafoil-guided-milano PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-guided-milano PRIVATE ALGORITHM_CLOSED_LOOP MILANO)
+sbs_target(parafoil-guided-milano stm32f429zi_death_stack_v2)
+
+add_executable(parafoil-guided-jesolo src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
+target_include_directories(parafoil-guided-jesolo PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-guided-jesolo PRIVATE ALGORITHM_CLOSED_LOOP JESOLO)
+sbs_target(parafoil-guided-jesolo stm32f429zi_death_stack_v2)
+
+add_executable(parafoil-t-approach-jesolo src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
+target_include_directories(parafoil-t-approach-jesolo PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-t-approach-jesolo PRIVATE ALGORITHM_EARLY_MANEUVER JESOLO)
+sbs_target(parafoil-t-approach-jesolo stm32f429zi_death_stack_v2)
+
+add_executable(parafoil-t-approach-milano src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
+target_include_directories(parafoil-t-approach-milano PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-t-approach-milano PRIVATE ALGORITHM_EARLY_MANEUVER MILANO)
+sbs_target(parafoil-t-approach-milano stm32f429zi_death_stack_v2)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 80f147ebead7a4a31ff720ed0104df4389a147d1..71a0052121fd66e140d2988c9fba3841d73f6e0d 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -112,7 +112,7 @@ set(GROUNDSTATION_NOKIA
src/Groundstation/Nokia/Hub.cpp
)
-set (LYRA_GS
+set(LYRA_GS
src/Groundstation/LyraGS/Radio/Radio.cpp
src/Groundstation/LyraGS/Ports/Ethernet.cpp
src/Groundstation/LyraGS/BoardStatus.cpp
@@ -130,6 +130,9 @@ set(PARAFOIL_COMPUTER
src/Parafoil/Actuators/Actuators.cpp
src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
src/Parafoil/PinHandler/PinHandler.cpp
+ src/Parafoil/Radio/Radio.cpp
+ src/Parafoil/Radio/MessageHandler.cpp
+ src/Parafoil/FlightStatsRecorder/FlightStatsRecorder.cpp
src/Parafoil/Sensors/Sensors.cpp
src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
src/Parafoil/StateMachines/NASController/NASController.cpp
diff --git a/src/Parafoil/Actuators/Actuators.cpp b/src/Parafoil/Actuators/Actuators.cpp
index 1aa20a6e73db4fdaff41cd0fcc4ac625d29ce534..06dd1c262caf69a51ccadfd9589cca8b8c51f29e 100644
--- a/src/Parafoil/Actuators/Actuators.cpp
+++ b/src/Parafoil/Actuators/Actuators.cpp
@@ -29,6 +29,7 @@
using namespace miosix;
using namespace Boardcore;
using namespace Boardcore::Units::Angle;
+using namespace Boardcore::Units::Time;
namespace config = Parafoil::Config::Actuators;
namespace Parafoil
@@ -38,14 +39,14 @@ Actuators::Actuators()
{
leftServo.servo = std::make_unique<Servo>(
config::LeftServo::TIMER, config::LeftServo::PWM_CH,
- config::LeftServo::MIN_PULSE.value(),
- config::LeftServo::MAX_PULSE.value());
+ config::LeftServo::MIN_PULSE.value<Microsecond>(),
+ config::LeftServo::MAX_PULSE.value<Microsecond>());
leftServo.fullRangeAngle = config::LeftServo::ROTATION;
rightServo.servo = std::make_unique<Servo>(
config::RightServo::TIMER, config::RightServo::PWM_CH,
- config::RightServo::MIN_PULSE.value(),
- config::RightServo::MAX_PULSE.value());
+ config::RightServo::MIN_PULSE.value<Microsecond>(),
+ config::RightServo::MAX_PULSE.value<Microsecond>());
rightServo.fullRangeAngle = config::RightServo::ROTATION;
}
@@ -84,8 +85,8 @@ bool Actuators::setServoAngle(ServosList servoId, Degree angle)
miosix::Lock<miosix::FastMutex> lock(actuator->mutex);
- actuator->servo->setPosition(
- (angle / actuator->fullRangeAngle.value()).value());
+ actuator->servo->setPosition(angle.value<Degree>() /
+ actuator->fullRangeAngle.value<Degree>());
return true;
}
diff --git a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
index 7806a1a6c01e4176a5f78d089927962544e69d40..bce21a162d73a050d6d0f0ea36aab70eb41891a2 100644
--- a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
+++ b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
@@ -29,6 +29,7 @@
#include <events/EventBroker.h>
using namespace Boardcore;
+using namespace Boardcore::Units::Length;
using namespace Common;
namespace config = Parafoil::Config;
@@ -64,9 +65,9 @@ void AltitudeTrigger::disable() { running = false; }
bool AltitudeTrigger::isEnabled() { return running; }
-void AltitudeTrigger::setDeploymentAltitude(float altitude)
+void AltitudeTrigger::setDeploymentAltitude(Meter altitude)
{
- targetAltitude = altitude;
+ targetAltitude = altitude.value<Meter>();
}
void AltitudeTrigger::update()
diff --git a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
index f5514b0889f4845dc4477624ba83335323d87165..816989fe5621ea72e9031731a56669351e8e7ba1 100644
--- a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
+++ b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, 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
@@ -65,7 +65,7 @@ public:
/**
* @return Set the altitude of the AltitudeTrigger
*/
- void setDeploymentAltitude(float altitude);
+ void setDeploymentAltitude(Boardcore::Units::Length::Meter altitude);
private:
// Update method that posts a FLIGHT_WING_ALT_PASSED when the correct
@@ -80,7 +80,8 @@ private:
int confidence = 0;
std::atomic<float> targetAltitude{
- Config::AltitudeTrigger::DEPLOYMENT_ALTITUDE.value()};
+ Config::AltitudeTrigger::DEPLOYMENT_ALTITUDE
+ .value<Boardcore::Units::Length::Meter>()};
};
} // namespace Parafoil
diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h
index 53a885511600d374961ecaf88bf8cfce72e9ff3a..0111a5b4435af656990a917737d72c142ce4b479 100644
--- a/src/Parafoil/BoardScheduler.h
+++ b/src/Parafoil/BoardScheduler.h
@@ -55,10 +55,12 @@ public:
Boardcore::TaskScheduler& nasController() { return critical; }
Boardcore::TaskScheduler& sensors() { return high; }
Boardcore::TaskScheduler& pinHandler() { return high; }
+ Boardcore::TaskScheduler& radio() { return medium; }
Boardcore::TaskScheduler& altitudeTrigger() { return medium; }
Boardcore::TaskScheduler& wingController() { return medium; }
Boardcore::TaskScheduler& windEstimation() { return low; }
Boardcore::TaskScheduler& actuators() { return low; }
+ Boardcore::TaskScheduler& flightStatsRecorder() { return low; }
static Priority::PriorityLevel flightModeManagerPriority()
{
diff --git a/src/Parafoil/Configs/NASConfig.h b/src/Parafoil/Configs/NASConfig.h
index 6a9e041dcbbd471d71a8659ae36e9029b6a59d8f..1c4e4728ba4441d3afd09c56ac7468fe9148bf2f 100644
--- a/src/Parafoil/Configs/NASConfig.h
+++ b/src/Parafoil/Configs/NASConfig.h
@@ -46,7 +46,7 @@ constexpr int CALIBRATION_SAMPLES_COUNT = 20;
constexpr auto CALIBRATION_SLEEP_TIME = 100_ms;
static const Boardcore::NASConfig CONFIG = {
- .T = UPDATE_RATE_SECONDS.value(),
+ .T = UPDATE_RATE_SECONDS.value<Second>(),
.SIGMA_BETA = 0.0001,
.SIGMA_W = 0.0019,
.SIGMA_ACC = 0.202,
diff --git a/src/Parafoil/Configs/RadioConfig.h b/src/Parafoil/Configs/RadioConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..de4ccca21c220fd96b09fd1c9078f09d82677ccc
--- /dev/null
+++ b/src/Parafoil/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 Parafoil
+{
+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 Parafoil
diff --git a/src/Parafoil/Configs/SensorsConfig.h b/src/Parafoil/Configs/SensorsConfig.h
index 3147326619b4fd54a39ed6346670f5c5661301ca..659e5537b8f4bcde9ea06387dc5048198083fca6 100644
--- a/src/Parafoil/Configs/SensorsConfig.h
+++ b/src/Parafoil/Configs/SensorsConfig.h
@@ -87,7 +87,7 @@ constexpr auto ODR = Boardcore::LPS22DF::ODR_100;
namespace UBXGPS
{
constexpr auto ENABLED = true;
-constexpr auto SAMPLING_RATE = 10_hz;
+constexpr auto SAMPLING_RATE = 10_khz;
} // namespace UBXGPS
namespace ADS131M08
diff --git a/src/Parafoil/Configs/WingConfig.h b/src/Parafoil/Configs/WingConfig.h
index f1fde1c1aa8a4062f6f53552ac522473809d6869..afa6ad074a599f1325f644ae25dfa9d6a326439e 100644
--- a/src/Parafoil/Configs/WingConfig.h
+++ b/src/Parafoil/Configs/WingConfig.h
@@ -71,15 +71,15 @@ constexpr auto TARGET_LAT = 45.5013853;
constexpr auto TARGET_LON = 9.1544219;
#endif
-#if defined(CLOSED_LOOP)
+#if defined(ALGORITHM_CLOSED_LOOP)
constexpr auto ALGORITHM = AlgorithmId::CLOSED_LOOP;
-#elif EARLY_MANEUVER
+#elif defined(ALGORITHM_EARLY_MANEUVER)
constexpr auto ALGORITHM = AlgorithmId::EARLY_MANEUVER;
-#elif SEQUENCE
+#elif defined(ALGORITHM_SEQUENCE)
constexpr auto ALGORITHM = AlgorithmId::SEQUENCE;
-#elif ROTATION
+#elif defined(ALGORITHM_ROTATION)
constexpr auto ALGORITHM = AlgorithmId::ROTATION;
-#elif PROGRESSIVE_ROTATION
+#elif defined(ALGORITHM_PROGRESSIVE_ROTATION)
constexpr auto ALGORITHM = AlgorithmId::PROGRESSIVE_ROTATION;
#else
constexpr auto ALGORITHM = AlgorithmId::CLOSED_LOOP;
diff --git a/src/Parafoil/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/Parafoil/FlightStatsRecorder/FlightStatsRecorder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9399454e8bab9cef8d94898f15d45c6272017877
--- /dev/null
+++ b/src/Parafoil/FlightStatsRecorder/FlightStatsRecorder.cpp
@@ -0,0 +1,129 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * 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 <Parafoil/Configs/NASConfig.h>
+#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Parafoil/StateMachines/WingController/WingController.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 Parafoil
+{
+
+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();
+ auto wcState = getModule<WingController>()->getState();
+
+ // Do nothing if it was not dropped yet
+ if (fmmState != FlightModeManagerState::FLYING_WING_DESCENT)
+ return;
+
+ auto acc = MeterPerSecondSquared{static_cast<Vector3f>(data).norm()};
+ Lock<FastMutex> lock{statsMutex};
+
+ if (wcState == WingControllerState::IDLE)
+ {
+ // Record this event only after drop, before deployment
+ if (acc > stats.dropMaxAcc)
+ {
+ stats.dropMaxAcc = acc;
+ stats.dropMaxAccTs = data.accelerationTimestamp;
+ }
+ }
+ else if (wcState == WingControllerState::FLYING_DEPLOYMENT ||
+ wcState == WingControllerState::FLYING_CONTROLLED_DESCENT)
+ {
+ // 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();
+ auto wcState = getModule<WingController>()->getState();
+
+ // Do nothing if it was not dropped yet
+ if (fmmState != FlightModeManagerState::FLYING_WING_DESCENT)
+ return;
+
+ Lock<FastMutex> lock{statsMutex};
+
+ if (wcState == WingControllerState::IDLE ||
+ wcState == WingControllerState::FLYING_DEPLOYMENT ||
+ wcState == WingControllerState::FLYING_CONTROLLED_DESCENT)
+ {
+ // 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 Parafoil
diff --git a/src/Parafoil/FlightStatsRecorder/FlightStatsRecorder.h b/src/Parafoil/FlightStatsRecorder/FlightStatsRecorder.h
new file mode 100644
index 0000000000000000000000000000000000000000..ccd7a3fdc96f3979c7ad6fde4ef7e68c43addc9a
--- /dev/null
+++ b/src/Parafoil/FlightStatsRecorder/FlightStatsRecorder.h
@@ -0,0 +1,83 @@
+/* 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 Parafoil
+{
+
+class WingController;
+class FlightModeManager;
+
+class FlightStatsRecorder
+ : public Boardcore::InjectableWithDeps<WingController, 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 Parafoil
diff --git a/src/Parafoil/Radio/MessageHandler.cpp b/src/Parafoil/Radio/MessageHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3c0a4c51ea6f43f84db15a1cc1bae0b1eadf6a61
--- /dev/null
+++ b/src/Parafoil/Radio/MessageHandler.cpp
@@ -0,0 +1,977 @@
+/* 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 <Parafoil/Actuators/Actuators.h>
+#include <Parafoil/AltitudeTrigger/AltitudeTrigger.h>
+#include <Parafoil/BoardScheduler.h>
+#include <Parafoil/Configs/RadioConfig.h>
+#include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Parafoil/PinHandler/PinHandler.h>
+#include <Parafoil/Sensors/Sensors.h>
+#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Parafoil/StateMachines/NASController/NASController.h>
+#include <Parafoil/StateMachines/WingController/WingController.h>
+#include <Parafoil/WindEstimation/WindEstimation.h>
+#include <common/Events.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <events/EventBroker.h>
+
+#include "Radio.h"
+
+using namespace Boardcore;
+using namespace Boardcore::Units::Angle;
+using namespace Boardcore::Units::Speed;
+using namespace Boardcore::Units::Length;
+using namespace Boardcore::Units::Acceleration;
+using namespace Common;
+namespace config = Parafoil::Config::Radio;
+
+namespace Parafoil
+{
+
+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_SERVO_TM_REQUEST_TC:
+ {
+ auto servo = static_cast<ServosList>(
+ mavlink_msg_servo_tm_request_tc_get_servo_id(&msg));
+
+ float position =
+ parent.getModule<Actuators>()->getServoPosition(servo);
+ if (position < 0)
+ return enqueueNack(msg);
+
+ mavlink_message_t tmMsg;
+ mavlink_servo_tm_t tm;
+
+ tm.servo_id = static_cast<uint8_t>(servo);
+ tm.servo_position = position;
+
+ mavlink_msg_servo_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID, &tmMsg,
+ &tm);
+ enqueueMessage(tmMsg);
+ return enqueueAck(msg);
+ }
+
+ case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
+ {
+ bool allowed = parent.getModule<FlightModeManager>()->isTestMode();
+ // Allow arbitrary servo movements in allowed states only
+ if (!allowed)
+ return enqueueNack(msg);
+
+ auto servo = static_cast<ServosList>(
+ mavlink_msg_set_servo_angle_tc_get_servo_id(&msg));
+ auto position = mavlink_msg_set_servo_angle_tc_get_angle(&msg);
+
+ if (parent.getModule<Actuators>()->setServoPosition(servo,
+ position))
+ return enqueueAck(msg);
+ else
+ return enqueueNack(msg);
+ }
+
+ case MAVLINK_MSG_ID_RESET_SERVO_TC:
+ {
+ bool allowed = parent.getModule<FlightModeManager>()->isTestMode();
+ // Reset servos in allowed states only
+ if (!allowed)
+ return enqueueNack(msg);
+
+ auto servo = static_cast<ServosList>(
+ mavlink_msg_reset_servo_tc_get_servo_id(&msg));
+
+ bool reset =
+ parent.getModule<Actuators>()->setServoPosition(servo, 0.0f);
+ if (reset)
+ {
+ // One of our servos was reset
+ return enqueueAck(msg);
+ }
+
+ return enqueueWack(msg);
+ }
+
+ case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
+ {
+ bool allowed = parent.getModule<FlightModeManager>()->isTestMode();
+ // Perform the wiggle in allowed states only
+ if (!allowed)
+ return enqueueNack(msg);
+
+ auto servo = static_cast<ServosList>(
+ mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
+
+ bool wiggled = parent.getModule<Actuators>()->wiggleServo(servo);
+ if (wiggled)
+ {
+ // One of our servos was wiggled
+ return enqueueAck(msg);
+ }
+
+ return enqueueWack(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_SET_DEPLOYMENT_ALTITUDE_TC:
+ {
+ float altitude =
+ mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg);
+
+ parent.getModule<AltitudeTrigger>()->setDeploymentAltitude(
+ Meter{altitude});
+
+ if (altitude < 200 || altitude > 450)
+ return enqueueWack(msg);
+ else
+ return enqueueAck(msg);
+ }
+
+ case MAVLINK_MSG_ID_SET_TARGET_COORDINATES_TC:
+ {
+ float latitude =
+ mavlink_msg_set_target_coordinates_tc_get_latitude(&msg);
+ float longitude =
+ mavlink_msg_set_target_coordinates_tc_get_longitude(&msg);
+
+ bool targetSet =
+ parent.getModule<WingController>()->setTargetCoordinates(
+ latitude, longitude);
+
+ if (targetSet)
+ return enqueueAck(msg);
+ else
+ return enqueueNack(msg);
+ }
+
+ case MAVLINK_MSG_ID_SET_ALGORITHM_TC:
+ {
+ uint8_t index =
+ mavlink_msg_set_algorithm_tc_get_algorithm_number(&msg);
+
+ bool algorithmSet =
+ parent.getModule<WingController>()->selectAlgorithm(
+ static_cast<Config::Wing::AlgorithmId>(index));
+
+ if (algorithmSet)
+ return enqueueAck(msg);
+ else
+ return enqueueNack(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 = parent.getModule<Actuators>()->isStarted();
+ tm.pin_handler = parent.getModule<PinHandler>()->isStarted();
+ tm.scheduler = parent.getModule<BoardScheduler>()->isStarted();
+ tm.can_handler = false; // Not present on parafoil board
+
+ 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 =
+ parent.getModule<Actuators>()
+ ->getServoAngle(ServosList::PARAFOIL_LEFT_SERVO)
+ .value<Degree>();
+ tm.right_servo_angle =
+ parent.getModule<Actuators>()
+ ->getServoAngle(ServosList::PARAFOIL_RIGHT_SERVO)
+ .value<Degree>();
+
+ // 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
+ auto wind = parent.getModule<WindEstimation>()->getPrediction();
+
+ tm.wes_n = wind.vn.value<MeterPerSecond>();
+ tm.wes_e = wind.ve.value<MeterPerSecond>();
+
+ 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* wnc = parent.getModule<WingController>();
+ auto* pinHandler = parent.getModule<PinHandler>();
+ auto* recorder = parent.getModule<FlightStatsRecorder>();
+ auto& logger = Logger::getInstance();
+
+ auto stats = recorder->getStats();
+ auto ref = nas->getReferenceValues();
+ auto wingTarget = wnc->getTargetCoordinates();
+ auto wingActiveTarget = wnc->getActiveTarget();
+ auto wingAlgorithm = wnc->getSelectedAlgorithm();
+ 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 =
+ stats.dropMaxAcc.value<MeterPerSecondSquared>();
+
+ // Max speed stats
+ tm.max_speed_ts = stats.maxSpeedTs;
+ tm.max_speed = stats.maxSpeed.value<MeterPerSecond>();
+ tm.max_speed_altitude = stats.maxSpeedAlt.value<Meter>();
+
+ // 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 = wingTarget.x();
+ tm.wing_target_lon = wingTarget.y();
+ tm.wing_active_target_n = wingActiveTarget.x();
+ tm.wing_active_target_e = wingActiveTarget.y();
+ tm.wing_algorithm = wingAlgorithm;
+
+ // Deployment stats
+ tm.dpl_ts = stats.dplTs;
+ tm.dpl_max_acc_ts = stats.dplMaxAccTs;
+ tm.dpl_alt = stats.dplAlt.value<Meter>();
+ tm.dpl_max_acc = stats.dplMaxAcc.value<MeterPerSecondSquared>();
+
+ // 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 = static_cast<uint8_t>(wnc->getState());
+
+ // 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 Parafoil
diff --git a/src/Parafoil/Radio/Radio.cpp b/src/Parafoil/Radio/Radio.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c49ae1f02308104e294291412b0fb524e0137d74
--- /dev/null
+++ b/src/Parafoil/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 <Parafoil/BoardScheduler.h>
+#include <Parafoil/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 = Parafoil::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 Parafoil
+{
+
+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,
+ Config::Radio::Xbee::TIMEOUT.value<Millisecond>());
+
+ // 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); },
+ config::MavlinkDriver::SLEEP_AFTER_SEND.value<Millisecond>(),
+ config::MavlinkDriver::MAX_PKT_AGE.value<Millisecond>());
+
+ 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 Parafoil
diff --git a/src/Parafoil/Radio/Radio.h b/src/Parafoil/Radio/Radio.h
new file mode 100644
index 0000000000000000000000000000000000000000..2b39b0ca749985465c11176a690beda70ffaa8b8
--- /dev/null
+++ b/src/Parafoil/Radio/Radio.h
@@ -0,0 +1,123 @@
+/* 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 <Parafoil/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 Parafoil
+{
+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 Actuators;
+class NASController;
+class WingController;
+class AltitudeTrigger;
+class PinHandler;
+class WindEstimation;
+class FlightStatsRecorder;
+
+class Radio : public Boardcore::InjectableWithDeps<
+ BoardScheduler, Sensors, Buses, FlightModeManager, Actuators,
+ NASController, WingController, AltitudeTrigger, PinHandler,
+ WindEstimation, 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 Parafoil
diff --git a/src/Parafoil/Sensors/Sensors.cpp b/src/Parafoil/Sensors/Sensors.cpp
index 2ccb85b154d12cd1928b6137967d6d9dfa5215a9..8866054608aaf179975bd8f02aac3acf154330d5 100644
--- a/src/Parafoil/Sensors/Sensors.cpp
+++ b/src/Parafoil/Sensors/Sensors.cpp
@@ -24,10 +24,12 @@
#include <Parafoil/BoardScheduler.h>
#include <Parafoil/Buses.h>
+#include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h>
#include <chrono>
using namespace Boardcore;
+using namespace Boardcore::Units::Frequency;
namespace config = Parafoil::Config::Sensors;
namespace hwmap = miosix::sensors;
@@ -341,9 +343,9 @@ 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,
- config::UBXGPS::SAMPLING_RATE.value());
+ ubxgps = std::make_unique<UBXGPSSpi>(
+ getModule<Buses>()->spi1, hwmap::ubxgps::cs::getPin(), spiConfig,
+ config::UBXGPS::SAMPLING_RATE.value<Kilohertz>());
LOG_INFO(logger, "UBXGPS initialized!");
}
@@ -390,6 +392,10 @@ void Sensors::bmx160Callback()
void Sensors::bmx160WithCorrectionCallback()
{
BMX160WithCorrectionData lastSample = bmx160WithCorrection->getLastSample();
+
+ // Update acceleration stats
+ getModule<FlightStatsRecorder>()->updateAcc(lastSample);
+
Logger::getInstance().log(lastSample);
}
diff --git a/src/Parafoil/Sensors/Sensors.h b/src/Parafoil/Sensors/Sensors.h
index 11f590a49c97c4f19931ae7d5e2ae60016b38cd4..f90040b0a08424325f71a6875da8b4a0ffa0fca4 100644
--- a/src/Parafoil/Sensors/Sensors.h
+++ b/src/Parafoil/Sensors/Sensors.h
@@ -42,11 +42,13 @@ namespace Parafoil
{
class BoardScheduler;
class Buses;
+class FlightStatsRecorder;
/**
* @brief Manages all the sensors of the parafoil board.
*/
-class Sensors : public Boardcore::InjectableWithDeps<BoardScheduler, Buses>
+class Sensors : public Boardcore::InjectableWithDeps<BoardScheduler, Buses,
+ FlightStatsRecorder>
{
public:
[[nodiscard]] bool start();
diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
index 94c15de3a31690dddb8bd37545efd3da453bd1b7..5415f579975d4406124b0e731c39a216420f2b33 100644
--- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -24,6 +24,7 @@
#include <Parafoil/BoardScheduler.h>
#include <Parafoil/Configs/FlightModeManagerConfig.h>
+#include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Parafoil/Sensors/Sensors.h>
#include <common/Events.h>
#include <drivers/timer/TimestampTimer.h>
@@ -448,7 +449,11 @@ State FlightModeManager::FlyingWingDescent(const Event& event)
// Send the event to the WingController
controlDelayId = EventBroker::getInstance().postDelayed(
FLIGHT_WING_DESCENT, TOPIC_FLIGHT,
- Millisecond{config::CONTROL_DELAY}.value());
+ config::CONTROL_DELAY.value<Millisecond>());
+
+ getModule<FlightStatsRecorder>()->dropDetected(
+ TimestampTimer::getTimestamp());
+
return HANDLED;
}
@@ -492,7 +497,7 @@ State FlightModeManager::Landed(const Event& event)
TOPIC_FLIGHT);
EventBroker::getInstance().postDelayed(
FMM_STOP_LOGGING, TOPIC_FMM,
- Millisecond{config::LOGGING_DELAY}.value());
+ config::LOGGING_DELAY.value<Millisecond>());
return HANDLED;
}
diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
index 549636b2d5bcca03c974fa177edce57823b621bf..c360f549284ce12b33d32a959e24c4d4b837c7b3 100644
--- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
@@ -31,6 +31,7 @@ namespace Parafoil
{
class Sensors;
class Actuators;
+class FlightStatsRecorder;
/**
* State machine that manages the flight modes of the Parafoil.
@@ -54,7 +55,8 @@ class Actuators;
*/
class FlightModeManager
: public Boardcore::HSM<FlightModeManager>,
- public Boardcore::InjectableWithDeps<Sensors, Actuators>
+ public Boardcore::InjectableWithDeps<Sensors, Actuators,
+ FlightStatsRecorder>
{
public:
FlightModeManager();
diff --git a/src/Parafoil/StateMachines/NASController/NASController.cpp b/src/Parafoil/StateMachines/NASController/NASController.cpp
index 3317629072692bf12b5f11f539934044f53eed75..b71d3a8cfe4e14fa4cdbd7b1f3c336d183882c56 100644
--- a/src/Parafoil/StateMachines/NASController/NASController.cpp
+++ b/src/Parafoil/StateMachines/NASController/NASController.cpp
@@ -22,6 +22,7 @@
#include <Parafoil/BoardScheduler.h>
#include <Parafoil/Configs/NASConfig.h>
+#include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Parafoil/Sensors/Sensors.h>
#include <Parafoil/StateMachines/NASController/NASController.h>
#include <algorithms/NAS/StateInitializer.h>
@@ -32,6 +33,7 @@
using namespace Boardcore;
using namespace Boardcore::Units::Time;
+using namespace Boardcore::Units::Length;
using namespace Boardcore::Units::Acceleration;
using namespace Eigen;
using namespace Common;
@@ -222,7 +224,7 @@ void NASController::calibrate()
baroSum += baro.pressure;
- Thread::sleep(Millisecond{config::CALIBRATION_SLEEP_TIME}.value());
+ Thread::sleep(config::CALIBRATION_SLEEP_TIME.value<Millisecond>());
}
Vector3f meanAcc = accSum / config::CALIBRATION_SAMPLES_COUNT;
@@ -281,6 +283,8 @@ void NASController::update()
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)
{
@@ -318,8 +322,9 @@ void NASController::update()
lastBaroTimestamp = baro.pressureTimestamp;
auto state = nas.getState();
- // auto ref = nas.getReferenceValues();
+ auto ref = nas.getReferenceValues();
+ getModule<FlightStatsRecorder>()->updateNas(state, ref.refTemperature);
Logger::getInstance().log(state);
}
@@ -334,4 +339,32 @@ void NASController::updateState(NASControllerState newState)
Logger::getInstance().log(status);
}
+void NASController::setReferenceAltitude(Meter altitude)
+{
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+ auto ref = nas.getReferenceValues();
+ ref.refAltitude = altitude.value<Meter>();
+ 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 Parafoil
diff --git a/src/Parafoil/StateMachines/NASController/NASController.h b/src/Parafoil/StateMachines/NASController/NASController.h
index 6c75c5830ea337fbc48d56011c0acba99d23269f..2998a5462eafb8a2abb8f992d09280a891709574 100644
--- a/src/Parafoil/StateMachines/NASController/NASController.h
+++ b/src/Parafoil/StateMachines/NASController/NASController.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
+/* Copyright (c) 2025 Skyward Experimental Rocketry
* Authors: Niccolò Betto, Davide Basso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -33,10 +33,12 @@ namespace Parafoil
{
class BoardScheduler;
class Sensors;
+class FlightStatsRecorder;
class NASController
: public Boardcore::FSM<NASController>,
- public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>
+ public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
+ FlightStatsRecorder>
{
public:
/**
@@ -60,6 +62,10 @@ public:
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();
diff --git a/src/Parafoil/StateMachines/WingController/WingController.cpp b/src/Parafoil/StateMachines/WingController/WingController.cpp
index 90ab7e46e88bf061afda0b42e8763c70bb91d377..3536fa810ea760c8d07997135ac2f462e23f3ebb 100644
--- a/src/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/Parafoil/StateMachines/WingController/WingController.cpp
@@ -25,10 +25,12 @@
#include <Parafoil/Configs/ActuatorsConfig.h>
#include <Parafoil/Configs/WESConfig.h>
#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Parafoil/StateMachines/NASController/NASController.h>
#include <Parafoil/WindEstimation/WindEstimation.h>
#include <Parafoil/Wing/AutomaticWingAlgorithm.h>
#include <Parafoil/Wing/FileWingAlgorithm.h>
+#include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
#include <Parafoil/Wing/WingAlgorithm.h>
#include <Parafoil/Wing/WingAlgorithmData.h>
#include <Parafoil/Wing/WingTargetPositionData.h>
@@ -107,7 +109,7 @@ State WingController::Flying(const Event& event)
}
case EV_INIT:
{
- return transition(&WingController::FlyingCalibration);
+ return transition(&WingController::FlyingDeployment);
}
case FLIGHT_LANDING_DETECTED:
{
@@ -120,15 +122,20 @@ State WingController::Flying(const Event& event)
}
}
-State WingController::FlyingCalibration(const Boardcore::Event& event)
+State WingController::FlyingDeployment(const Boardcore::Event& event)
{
static uint16_t calibrationTimeoutEventId;
switch (event)
{
- case EV_ENTRY: // starts twirling and calibration wes
+ case EV_ENTRY:
{
- updateState(WingControllerState::FLYING_CALIBRATION);
+ updateState(WingControllerState::FLYING_DEPLOYMENT);
+
+ auto nasState = getModule<NASController>()->getNasState();
+ auto altitude = Meter{-nasState.d};
+ getModule<FlightStatsRecorder>()->deploymentDetected(
+ TimestampTimer::getTimestamp(), altitude);
flareWing();
calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
@@ -169,7 +176,7 @@ State WingController::FlyingCalibration(const Boardcore::Event& event)
calibrationTimeoutEventId =
EventBroker::getInstance().postDelayed(
DPL_WES_CAL_DONE, TOPIC_DPL,
- Millisecond{WES::CALIBRATION_TIMEOUT}.value());
+ WES::CALIBRATION_TIMEOUT.value<Millisecond>());
getModule<WindEstimation>()->startAlgorithm();
getModule<Actuators>()->startTwirl();
@@ -391,6 +398,11 @@ Eigen::Vector2f WingController::getActiveTarget()
return emGuidance.getActiveTarget();
}
+uint8_t WingController::getSelectedAlgorithm()
+{
+ return static_cast<uint8_t>(selectedAlgorithm.load());
+}
+
void WingController::loadAlgorithms()
{
using namespace Wing;
@@ -419,12 +431,12 @@ void WingController::loadAlgorithms()
step.servo2Angle = 120_deg;
algorithm->addStep(step);
- step.timestamp += Microsecond{STRAIGHT_FLIGHT_TIMEOUT}.value();
+ step.timestamp += STRAIGHT_FLIGHT_TIMEOUT.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = 0_deg;
algorithm->addStep(step);
- step.timestamp += Microsecond{STRAIGHT_FLIGHT_TIMEOUT}.value();
+ step.timestamp += STRAIGHT_FLIGHT_TIMEOUT.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = 0_deg;
algorithm->addStep(step);
@@ -444,32 +456,32 @@ void WingController::loadAlgorithms()
step.servo2Angle = 0_deg;
algorithm->addStep(step);
- step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+ step.timestamp += WES::ROTATION_PERIOD.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = RightServo::ROTATION / 2;
algorithm->addStep(step);
- step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+ step.timestamp += WES::ROTATION_PERIOD.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = 0_deg;
algorithm->addStep(step);
- step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+ step.timestamp += WES::ROTATION_PERIOD.value<Microsecond>();
step.servo1Angle = LeftServo::ROTATION;
step.servo2Angle = RightServo::ROTATION;
algorithm->addStep(step);
- step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+ step.timestamp += WES::ROTATION_PERIOD.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = RightServo::ROTATION / 2;
algorithm->addStep(step);
- step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+ step.timestamp += WES::ROTATION_PERIOD.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = 0_deg;
algorithm->addStep(step);
- step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+ step.timestamp += WES::ROTATION_PERIOD.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = 0_deg;
algorithm->addStep(step);
@@ -484,19 +496,19 @@ void WingController::loadAlgorithms()
PARAFOIL_RIGHT_SERVO);
WingAlgorithmData step;
- step.timestamp = Microsecond(PROGRESSIVE_ROTATION_TIMEOUT).value();
+ step.timestamp = PROGRESSIVE_ROTATION_TIMEOUT.value<Microsecond>();
for (auto angle = 150_deg; angle >= 0_deg; angle -= WING_DECREMENT)
{
step.servo1Angle = angle;
step.servo2Angle = 0_deg;
algorithm->addStep(step);
- step.timestamp += Microsecond(COMMAND_PERIOD).value();
+ step.timestamp += COMMAND_PERIOD.value<Microsecond>();
step.servo1Angle = 0_deg;
step.servo2Angle = angle;
algorithm->addStep(step);
- step.timestamp += Microsecond(COMMAND_PERIOD).value();
+ step.timestamp += COMMAND_PERIOD.value<Microsecond>();
}
algorithms[static_cast<size_t>(AlgorithmId::PROGRESSIVE_ROTATION)] =
diff --git a/src/Parafoil/StateMachines/WingController/WingController.h b/src/Parafoil/StateMachines/WingController/WingController.h
index a38d95ece554b46e72d0acd7a15a377f90173ef2..5850784f0443f90a2e6f6b943cfda892d3b767b4 100644
--- a/src/Parafoil/StateMachines/WingController/WingController.h
+++ b/src/Parafoil/StateMachines/WingController/WingController.h
@@ -23,6 +23,7 @@
#pragma once
#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
#include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
#include <Parafoil/Wing/WingAlgorithm.h>
@@ -72,7 +73,6 @@ class WindEstimation;
* Idle
*
* Flying
- * ├── FlyingCalibration
* ├── FlyingDeployment
* └── FlyingControlledDescent
*
@@ -101,7 +101,6 @@ public:
// HSM states
Boardcore::State Idle(const Boardcore::Event& event);
Boardcore::State Flying(const Boardcore::Event& event);
- Boardcore::State FlyingCalibration(const Boardcore::Event& event);
Boardcore::State FlyingDeployment(const Boardcore::Event& event);
Boardcore::State FlyingControlledDescent(const Boardcore::Event& event);
Boardcore::State OnGround(const Boardcore::Event& event);
diff --git a/src/Parafoil/StateMachines/WingController/WingControllerData.h b/src/Parafoil/StateMachines/WingController/WingControllerData.h
index 1b239e58463db25f9d5692b5cbc326cbe493c8f0..5efcded47967ba7c14c08280e6e58223860855d3 100644
--- a/src/Parafoil/StateMachines/WingController/WingControllerData.h
+++ b/src/Parafoil/StateMachines/WingController/WingControllerData.h
@@ -32,7 +32,6 @@ namespace Parafoil
enum class WingControllerState : uint8_t
{
IDLE = 0,
- FLYING_CALIBRATION,
FLYING_DEPLOYMENT,
FLYING_CONTROLLED_DESCENT,
ON_GROUND,
diff --git a/src/Parafoil/WindEstimation/WindEstimation.cpp b/src/Parafoil/WindEstimation/WindEstimation.cpp
index 7704390aaf98693436c08d6496a59cdd3a3e6178..be909e97baac5bd7e5785755485319683c5a3fde 100644
--- a/src/Parafoil/WindEstimation/WindEstimation.cpp
+++ b/src/Parafoil/WindEstimation/WindEstimation.cpp
@@ -27,8 +27,8 @@
#include <Parafoil/Sensors/Sensors.h>
using namespace Boardcore;
+using namespace Boardcore::Units::Speed;
using namespace Parafoil::Config;
-using namespace Units::Speed;
namespace Parafoil
{
@@ -133,9 +133,9 @@ void WindEstimation::updateCalibration()
MeterPerSecond{gps.velocityEast}};
calibrationMatrix(nSampleCalibration, 0) =
- gpsVelocity.vn.value();
+ gpsVelocity.vn.value<MeterPerSecond>();
calibrationMatrix(nSampleCalibration, 1) =
- gpsVelocity.ve.value();
+ gpsVelocity.ve.value<MeterPerSecond>();
calibrationV2(nSampleCalibration) = gpsVelocity.normSquared();
velocity.vn += gpsVelocity.vn;
@@ -154,9 +154,11 @@ void WindEstimation::updateCalibration()
for (int i = 0; i < nSampleCalibration; i++)
{
calibrationMatrix(i, 0) =
- calibrationMatrix(i, 0) - velocity.vn.value();
+ calibrationMatrix(i, 0) -
+ velocity.vn.value<MeterPerSecond>();
calibrationMatrix(i, 1) =
- calibrationMatrix(i, 1) - velocity.ve.value();
+ calibrationMatrix(i, 1) -
+ velocity.ve.value<MeterPerSecond>();
calibrationV2(i) = 0.5f * (calibrationV2(i) - speedSquared);
}
@@ -230,9 +232,11 @@ void WindEstimation::updateAlgorithm()
speedSquared = (speedSquared * nSampleAlgorithm +
(gpsVelocity.normSquared())) /
(nSampleAlgorithm + 1);
- phi(0) = gpsVelocity.vn.value() - velocity.vn.value();
- phi(1) = gpsVelocity.ve.value() - velocity.ve.value();
- y = 0.5f * ((gpsVelocity.normSquared()) - speedSquared);
+ phi(0) = gpsVelocity.vn.value<MeterPerSecond>() -
+ velocity.vn.value<MeterPerSecond>();
+ phi(1) = gpsVelocity.ve.value<MeterPerSecond>() -
+ velocity.ve.value<MeterPerSecond>();
+ y = 0.5f * ((gpsVelocity.normSquared()) - speedSquared);
phiT = phi.transpose();
funv =
@@ -242,8 +246,8 @@ void WindEstimation::updateAlgorithm()
{
miosix::Lock<FastMutex> l(mutex);
- wind.vn = MeterPerSecond(wind.vn.value() + temp(0));
- wind.ve = MeterPerSecond(wind.ve.value() + temp(1));
+ wind.vn = wind.vn + MeterPerSecond(temp(0));
+ wind.ve = wind.ve + MeterPerSecond(temp(1));
}
logStatus();
diff --git a/src/Parafoil/WindEstimation/WindEstimationData.h b/src/Parafoil/WindEstimation/WindEstimationData.h
index 60e9668095b11b24d5cf3883a85eb4ea40c76e92..a5faaee6f4646908a4ae65b6aff80076210bacf0 100644
--- a/src/Parafoil/WindEstimation/WindEstimationData.h
+++ b/src/Parafoil/WindEstimation/WindEstimationData.h
@@ -64,10 +64,17 @@ struct GeoVelocity
*/
float normSquared() const
{
- return vn.value() * vn.value() + ve.value() * ve.value();
+ return vn.value<Boardcore::Units::Speed::MeterPerSecond>() *
+ vn.value<Boardcore::Units::Speed::MeterPerSecond>() +
+ ve.value<Boardcore::Units::Speed::MeterPerSecond>() *
+ ve.value<Boardcore::Units::Speed::MeterPerSecond>();
}
- Eigen::Vector2f asVector() const { return {vn.value(), ve.value()}; }
+ Eigen::Vector2f asVector() const
+ {
+ return {vn.value<Boardcore::Units::Speed::MeterPerSecond>(),
+ ve.value<Boardcore::Units::Speed::MeterPerSecond>()};
+ }
};
} // namespace Parafoil
diff --git a/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp
index 3c750418f2be5a0290e11b73ac3231ea6de2c7a4..ce9113b51c46cb00a127cb5052fa5b596bab5e2d 100644
--- a/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp
@@ -45,7 +45,7 @@ AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki,
: Super(servo1, servo2), guidance(guidance)
{
// PIController needs the sample period in floating point seconds
- auto samplePeriod = 1.0f / UPDATE_RATE.value();
+ auto samplePeriod = 1.0f / UPDATE_RATE.value<Hertz>();
controller = std::make_unique<PIController>(Kp, Ki, samplePeriod,
PI::SATURATION_MIN_LIMIT,
@@ -121,7 +121,7 @@ Degree AutomaticWingAlgorithm::algorithmStep(const NASState& state)
// Call the PI with the just calculated error. The result is in RADIANS,
// if positive we activate one servo, if negative the other
// We also need to convert the result from radians back to degrees
- auto result = Degree(Radian(controller->update(error.value())));
+ auto result = Degree(Radian(controller->update(error.value<Radian>())));
// Logs the outputs
{
@@ -139,7 +139,7 @@ Degree AutomaticWingAlgorithm::algorithmStep(const NASState& state)
Radian AutomaticWingAlgorithm::angleDiff(Radian a, Radian b)
{
- auto diff = (a - b).value();
+ auto diff = (a - b).value<Radian>();
// Angle difference
if (diff < -Constants::PI || Constants::PI < diff)
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index 00bb72f05d2f2f7d52af7fc174dc9bf08d226adc..a6321ac32bb5f38b9c4ebe72b97ee5174908e04b 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -24,7 +24,9 @@
#include <Parafoil/AltitudeTrigger/AltitudeTrigger.h>
#include <Parafoil/BoardScheduler.h>
#include <Parafoil/Buses.h>
+#include <Parafoil/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Parafoil/PinHandler/PinHandler.h>
+#include <Parafoil/Radio/Radio.h>
#include <Parafoil/Sensors/Sensors.h>
#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
#include <Parafoil/StateMachines/NASController/NASController.h>
@@ -114,7 +116,9 @@ int main()
auto pinHandler = new PinHandler();
initResult &= depman.insert(pinHandler);
- // TODO: Radio
+ // Radio
+ auto radio = new Radio();
+ initResult &= depman.insert(radio);
// Flight algorithms
auto altitudeTrigger = new AltitudeTrigger();
@@ -124,6 +128,10 @@ int main()
auto windEstimation = new WindEstimation();
initResult &= depman.insert(windEstimation);
+ // Stats recorder
+ auto flightStatsRecorder = new FlightStatsRecorder();
+ initResult &= depman.insert(flightStatsRecorder);
+
// Actuators
auto actuators = new Actuators();
initResult &= depman.insert(actuators);
@@ -138,7 +146,7 @@ int main()
START_MODULE(flightModeManager);
START_MODULE(pinHandler);
- // START_MODULE(radio) { miosix::led2On(); }
+ START_MODULE(radio);
START_MODULE(nasController);
START_MODULE(altitudeTrigger);
START_MODULE(windEstimation);