diff --git a/CMakeLists.txt b/CMakeLists.txt
index 239e46323be079d1091eab9766450382a3565995..65a2c2f39dcc2afed86092df03fcbe3658d809c1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,6 +52,16 @@ target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
target_compile_definitions(main-entry-euroc PRIVATE EUROC)
sbs_target(main-entry-euroc stm32f767zi_skyward_death_stack_v4)
+add_executable(payload-entry-roccaraso src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER})
+target_include_directories(payload-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(payload-entry-roccaraso PRIVATE ROCCARASO)
+sbs_target(payload-entry-roccaraso stm32f767zi_skyward_death_stack_v4)
+
+add_executable(payload-entry-euroc src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER})
+target_include_directories(payload-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(payload-entry-euroc PRIVATE EUROC)
+sbs_target(payload-entry-euroc stm32f767zi_skyward_death_stack_v4)
+
add_executable(motor-entry src/entrypoints/Motor/motor-entry.cpp ${MOTOR_SOURCES})
target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(motor-entry stm32f767zi_gemini_motor)
@@ -63,7 +73,3 @@ sbs_target(rig-entry stm32f429zi_skyward_rig)
add_executable(con_rig-entry src/entrypoints/con_RIG/con_rig-entry.cpp ${CON_RIG_COMPUTER})
target_include_directories(con_rig-entry PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(con_rig-entry stm32f429zi_stm32f4discovery)
-
-#-----------------------------------------------------------------------------#
-# Test entrypoints #
-#-----------------------------------------------------------------------------#
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index da4dc6d1f4720e9ad6e5cbb1ccb6dd009373b949..65860823f9cd53c58948c5d92b194c6c28dc06c5 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -25,7 +25,7 @@ set(OBSW_INCLUDE_DIRS
src/hardware_in_the_loop
)
-set(HIL
+set(HIL
src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
src/hardware_in_the_loop/HIL/HILTransceiver.cpp
)
@@ -73,3 +73,25 @@ set(CON_RIG_COMPUTER
src/boards/con_RIG/Radio/Radio.cpp
src/boards/con_RIG/BoardScheduler.cpp
)
+
+set(PAYLOAD_COMPUTER
+ src/boards/Payload/Actuators/Actuators.cpp
+ src/boards/Payload/CanHandler/CanHandler.cpp
+ src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
+ src/boards/Payload/Sensors/Sensors.cpp
+ src/boards/Payload/BoardScheduler.cpp
+ src/boards/Payload/PinHandler/PinHandler.cpp
+ src/boards/Payload/Radio/Radio.cpp
+ src/boards/Payload/TMRepository/TMRepository.cpp
+ src/boards/Payload/StateMachines/NASController/NASController.cpp
+ src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
+ src/boards/Payload/StateMachines/WingController/WingController.cpp
+ src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp
+ src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp
+ src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
+ src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+ src/boards/Payload/Wing/FileWingAlgorithm.cpp
+ src/boards/Payload/Wing/WingAlgorithm.cpp
+ src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp
+ src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
+)
diff --git a/src/boards/Payload/Actuators/Actuators.cpp b/src/boards/Payload/Actuators/Actuators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..759aef675a7d6b20a281ce92ed3cab0563e7e28a
--- /dev/null
+++ b/src/boards/Payload/Actuators/Actuators.cpp
@@ -0,0 +1,268 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli
+ *
+ * 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 "Actuators.h"
+
+#include <Payload/BoardScheduler.h>
+#include <Payload/Configs/ActuatorsConfigs.h>
+#include <interfaces-impl/hwmapping.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Payload::ActuatorsConfigs;
+
+namespace Payload
+{
+
+Actuators::Actuators(Boardcore::TaskScheduler* sched) : scheduler(sched)
+{
+ leftServo = new Servo(SERVO_1_TIMER, SERVO_1_PWM_CH, LEFT_SERVO_MIN_PULSE,
+ LEFT_SERVO_MAX_PULSE);
+ rightServo = new Servo(SERVO_2_TIMER, SERVO_2_PWM_CH, RIGHT_SERVO_MIN_PULSE,
+ RIGHT_SERVO_MAX_PULSE);
+ buzzer = new PWM(BUZZER_TIMER, BUZZER_FREQUENCY);
+ buzzer->setDutyCycle(BUZZER_CHANNEL, BUZZER_DUTY_CYCLE);
+ camOff();
+}
+
+bool Actuators::start()
+{
+ // Servos
+ enableServo(PARAFOIL_LEFT_SERVO);
+ setServo(PARAFOIL_LEFT_SERVO, 0);
+ enableServo(PARAFOIL_RIGHT_SERVO);
+ setServo(PARAFOIL_RIGHT_SERVO, 0);
+
+ // Signaling Devices configurations
+
+ return scheduler->addTask([&]() { updateBuzzer(); }, BUZZER_UPDATE_PERIOD,
+ TaskScheduler::Policy::RECOVER) != 0;
+}
+
+bool Actuators::setServo(ServosList servoId, float percentage)
+{
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
+ leftServo->setPosition(percentage);
+ Logger::getInstance().log(leftServo->getState());
+ break;
+ }
+ case PARAFOIL_RIGHT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
+ rightServo->setPosition(percentage);
+ Logger::getInstance().log(rightServo->getState());
+ break;
+ }
+ default:
+ {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool Actuators::setServoAngle(ServosList servoId, float angle)
+{
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
+ leftServo->setPosition(angle / LEFT_SERVO_ROTATION);
+ Logger::getInstance().log(leftServo->getState());
+ break;
+ }
+ case PARAFOIL_RIGHT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
+ rightServo->setPosition(angle / RIGHT_SERVO_ROTATION);
+ Logger::getInstance().log(rightServo->getState());
+ break;
+ }
+ default:
+ {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool Actuators::wiggleServo(ServosList servoId)
+{
+
+ if (!setServo(servoId, 1))
+ {
+ return false;
+ }
+ Thread::sleep(1000);
+ return setServo(servoId, 0);
+}
+
+bool Actuators::enableServo(ServosList servoId)
+{
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
+ leftServo->enable();
+ break;
+ }
+ case PARAFOIL_RIGHT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
+ rightServo->enable();
+ break;
+ }
+ default:
+ return false;
+ }
+
+ return true;
+}
+
+bool Actuators::disableServo(ServosList servoId)
+{
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
+ leftServo->disable();
+ break;
+ }
+ case PARAFOIL_RIGHT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
+ rightServo->disable();
+ break;
+ }
+ default:
+ return false;
+ }
+
+ return true;
+}
+
+float Actuators::getServoPosition(ServosList servoId)
+{
+
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
+ return leftServo->getPosition();
+ }
+ case PARAFOIL_RIGHT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
+ return rightServo->getPosition();
+ }
+ default:
+ return 0;
+ }
+
+ return 0;
+}
+
+float Actuators::getServoAngle(ServosList servoId)
+{
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
+ return leftServo->getPosition() * LEFT_SERVO_ROTATION;
+ }
+ case PARAFOIL_RIGHT_SERVO:
+ {
+ miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
+ return rightServo->getPosition() * RIGHT_SERVO_ROTATION;
+ }
+ default:
+ return 0;
+ }
+
+ return 0;
+}
+
+void Actuators::cuttersOn() { gpios::cut_trigger::high(); }
+
+void Actuators::cuttersOff() { gpios::cut_trigger::low(); }
+
+void Actuators::camOn() { gpios::camera_enable::high(); }
+
+void Actuators::camOff() { gpios::camera_enable::low(); }
+
+void Actuators::buzzerArmed()
+{
+ miosix::Lock<miosix::FastMutex> l(rocketSignalingStateMutex);
+ // Set the counter with respect to the update function period
+ buzzerCounterOverflow = ROCKET_SS_ARMED_PERIOD / BUZZER_UPDATE_PERIOD;
+}
+
+void Actuators::buzzerLanded()
+{
+ miosix::Lock<miosix::FastMutex> l(rocketSignalingStateMutex);
+ buzzerCounterOverflow = ROCKET_SS_LAND_PERIOD / BUZZER_UPDATE_PERIOD;
+}
+
+void Actuators::buzzerOff()
+{
+ miosix::Lock<miosix::FastMutex> l(rocketSignalingStateMutex);
+ buzzerCounterOverflow = 0;
+}
+
+void Actuators::updateBuzzer()
+{
+ miosix::Lock<miosix::FastMutex> l(rocketSignalingStateMutex);
+ if (buzzerCounterOverflow == 0)
+ {
+ // The buzzer is deactivated thus the channel is disabled
+ buzzer->disableChannel(BUZZER_CHANNEL);
+ }
+ else
+ {
+ if (buzzerCounter >= buzzerCounterOverflow)
+ {
+ // Enable the channel for this period
+ buzzer->enableChannel(BUZZER_CHANNEL);
+ buzzerCounter = 0;
+ }
+ else
+ {
+ buzzer->disableChannel(BUZZER_CHANNEL);
+ buzzerCounter++;
+ }
+ }
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/Actuators/Actuators.h b/src/boards/Payload/Actuators/Actuators.h
new file mode 100644
index 0000000000000000000000000000000000000000..db2169c17d79ed7648f0ae1b9150e690740ecd19
--- /dev/null
+++ b/src/boards/Payload/Actuators/Actuators.h
@@ -0,0 +1,138 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli
+ *
+ * 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 <actuators/Servo/Servo.h>
+#include <common/Mavlink.h>
+#include <interfaces/gpio.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+
+struct Actuators : public Boardcore::Module
+{
+ explicit Actuators(Boardcore::TaskScheduler* sched);
+
+ [[nodiscard]] bool start();
+
+ /**
+ * @brief Moves the specified servo to the given position.
+ *
+ * @param servoId Servo to move.
+ * @param percentage Angle to set [0-1].
+ * @return True if the the angle was set.
+ */
+ bool setServo(ServosList servoId, float percentage);
+
+ /**
+ * @brief Moves the specified servo to the given position.
+ *
+ * @param servoId Servo to move.
+ * @param angle Angle to set [degree].
+ * @return True if the the angle was set.
+ */
+ bool setServoAngle(ServosList servoId, float angle);
+
+ /**
+ * @brief Wiggles the servo for few seconds.
+ *
+ * @param servoId Servo to move.
+ * @return true
+ * @return false
+ */
+ bool wiggleServo(ServosList servoId);
+
+ /**
+ * @brief Enables the specified servo.
+ *
+ * @param servoId Servo to enable.
+ * @return True if the servo was enabled.
+ * @return False if the servoId is invalid.
+ */
+ bool enableServo(ServosList servoId);
+
+ /**
+ * @brief Disables the specified servo.
+ *
+ * @param servoId Servo to disable.
+ * @return True if the servo was disabled.
+ * @return False if the servoId is invalid.
+ */
+ bool disableServo(ServosList servoId);
+
+ /**
+ * @brief Returns the current position of the specified servo.
+ *
+ * @param servoId Servo to read.
+ * @return float current Servo position in range [0-1], 0 if the servoId is
+ * invalid.
+ */
+ float getServoPosition(ServosList servoId);
+
+ /**
+ * @brief Returns the current angle of the specified servo.
+ *
+ * @param servoId Servo to read.
+ * @return float current Servo angle in range [0-180], 0 if the servoId is
+ * invalid.
+ */
+ float getServoAngle(ServosList servoId);
+
+ void cuttersOn();
+ void cuttersOff();
+
+ void camOn();
+ void camOff();
+
+ void buzzerArmed();
+ void buzzerError();
+ void buzzerLanded();
+ void buzzerOff();
+
+private:
+ void toggleLed();
+ /**
+ * @brief Automatic called method to update the buzzer status
+ */
+ void updateBuzzer();
+
+ Boardcore::TaskScheduler* scheduler = nullptr;
+ Boardcore::Servo* leftServo = nullptr;
+ Boardcore::Servo* rightServo = nullptr;
+ Boardcore::PWM* buzzer = nullptr;
+
+ // mutexes
+ miosix::FastMutex leftServoMutex;
+ miosix::FastMutex rightServoMutex;
+ miosix::FastMutex rocketSignalingStateMutex;
+
+ // Counter that enables and disables the buzzer
+ uint32_t buzzerCounter = 0;
+ // Upper limit of the buzzer counter
+ uint32_t buzzerCounterOverflow = 0;
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6398f6e51ee398d55ca657035461a5f32a3cdddc
--- /dev/null
+++ b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp
@@ -0,0 +1,101 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 "AltitudeTrigger.h"
+
+#include <Payload/BoardScheduler.h>
+#include <Payload/Configs/WingConfig.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Common;
+
+namespace Payload
+{
+
+AltitudeTrigger::AltitudeTrigger(TaskScheduler *sched)
+ : running(false), confidence(0),
+ deploymentAltitude(WingConfig::ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE),
+ scheduler(sched)
+{
+}
+
+bool AltitudeTrigger::start()
+{
+ return (scheduler->addTask(std::bind(&AltitudeTrigger::update, this),
+ WingConfig::ALTITUDE_TRIGGER_PERIOD) != 0);
+}
+
+void AltitudeTrigger::enable()
+{
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ confidence = 0;
+ running = true;
+}
+
+void AltitudeTrigger::setDeploymentAltitude(float altitude)
+{
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ deploymentAltitude = altitude;
+}
+
+void AltitudeTrigger::disable()
+{
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ running = false;
+}
+
+bool AltitudeTrigger::isActive()
+{
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ return running;
+}
+
+void AltitudeTrigger::update()
+{
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ if (running)
+ {
+ // We multiply by -1 to have a positive height
+ float height =
+ -ModuleManager::getInstance().get<NASController>()->getNasState().d;
+
+ if (height < WingConfig::ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE)
+ {
+ confidence++;
+ }
+ else
+ {
+ confidence = 0;
+ }
+ if (confidence >= WingConfig ::ALTITUDE_TRIGGER_CONFIDENCE)
+ {
+ confidence = 0;
+ EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+ TOPIC_FLIGHT);
+ running = false;
+ }
+ }
+}
+} // namespace Payload
diff --git a/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.h b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.h
new file mode 100644
index 0000000000000000000000000000000000000000..b0d2731ac9ecbcdc19ca2cd5f0ae9edd156735e1
--- /dev/null
+++ b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.h
@@ -0,0 +1,81 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/BoardScheduler.h>
+
+#include <atomic>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+
+class AltitudeTrigger : public Boardcore::Module
+{
+public:
+ explicit AltitudeTrigger(Boardcore::TaskScheduler *sched);
+
+ /**
+ * @brief Adds the update() task to the task scheduler.
+ */
+ bool start();
+
+ /**
+ * @brief Enable the AltitudeTrigger.
+ */
+ void enable();
+
+ /**
+ * @brief Disable the AltitudeTrigger.
+ */
+ void disable();
+
+ /**
+ * @return The status of the AltitudeTrigger
+ */
+ bool isActive();
+
+ /**
+ * @return Set the altitude of the AltitudeTrigger
+ */
+ void setDeploymentAltitude(float altitude);
+
+private:
+ // Update method that posts a FLIGHT_WING_ALT_PASSED when the correct
+ // altitude is reached
+ void update();
+
+ bool running;
+
+ // Number of times that the algorithm detects to be below the fixed
+ // altitude
+ int confidence;
+
+ float deploymentAltitude;
+
+ miosix::FastMutex mutex;
+
+ Boardcore::TaskScheduler *scheduler = nullptr;
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/BoardScheduler.cpp b/src/boards/Payload/BoardScheduler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..10eafa5f52b0d11473470ae239165890a3795661
--- /dev/null
+++ b/src/boards/Payload/BoardScheduler.cpp
@@ -0,0 +1,66 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro
+ *
+ * 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 "BoardScheduler.h"
+
+using namespace Boardcore;
+
+namespace Payload
+{
+
+BoardScheduler::BoardScheduler()
+{
+ scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4);
+ scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3);
+ scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2);
+ scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1);
+}
+
+TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority)
+{
+ switch (priority.get())
+ {
+ case miosix::PRIORITY_MAX:
+ return scheduler4;
+ case miosix::PRIORITY_MAX - 1:
+ return scheduler3;
+ case miosix::PRIORITY_MAX - 2:
+ return scheduler2;
+ case miosix::MAIN_PRIORITY:
+ return scheduler1;
+ default:
+ return scheduler1;
+ }
+}
+
+bool BoardScheduler::start()
+{
+ return scheduler1->start() && scheduler2->start() && scheduler3->start() &&
+ scheduler4->start();
+}
+
+bool BoardScheduler::isStarted()
+{
+ return scheduler1->isRunning() && scheduler2->isRunning() &&
+ scheduler3->isRunning() && scheduler4->isRunning();
+}
+} // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/BoardScheduler.h b/src/boards/Payload/BoardScheduler.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a09bccc34b1508ff13df578ce9e8a9440badf43
--- /dev/null
+++ b/src/boards/Payload/BoardScheduler.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro
+ *
+ * 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 <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+/**
+ * @brief Class that wraps the 4 main task schedulers of the entire OBSW.
+ * There is a task scheduler for every miosix priority
+ */
+class BoardScheduler : public Boardcore::Module
+{
+public:
+ BoardScheduler();
+
+ /**
+ * @brief Get the Scheduler object relative to the requested priority
+ *
+ * @param priority The task scheduler priority
+ * @return Boardcore::TaskScheduler& Reference to the requested task
+ * scheduler.
+ * @note Min priority scheduler is returned in case of non valid priority.
+ */
+ Boardcore::TaskScheduler* getScheduler(miosix::Priority priority);
+
+ [[nodiscard]] bool start();
+
+ /**
+ * @brief Returns if all the schedulers are up and running
+ */
+ bool isStarted();
+
+private:
+ Boardcore::TaskScheduler* scheduler1;
+ Boardcore::TaskScheduler* scheduler2;
+ Boardcore::TaskScheduler* scheduler3;
+ Boardcore::TaskScheduler* scheduler4;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/Buses.h b/src/boards/Payload/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..d4f49813721bcd92a32e125a8547633c4bd8cf01
--- /dev/null
+++ b/src/boards/Payload/Buses.h
@@ -0,0 +1,55 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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/i2c/I2C.h>
+#include <drivers/spi/SPIBus.h>
+#include <drivers/usart/USART.h>
+#include <interfaces-impl/hwmapping.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+namespace Payload
+{
+class Buses : public Boardcore::Module
+{
+public:
+ Boardcore::SPIBus spi1;
+ Boardcore::SPIBus spi3;
+ Boardcore::SPIBus spi4;
+ Boardcore::SPIBus spi6;
+
+ Boardcore::I2C i2c1;
+
+ Boardcore::USART usart1;
+ Boardcore::USART usart2;
+ Boardcore::USART uart4;
+
+ Buses()
+ : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6),
+ i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(),
+ miosix::interfaces::i2c1::sda::getPin()),
+ usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200)
+ {
+ }
+};
+} // namespace Payload
diff --git a/src/boards/Payload/CanHandler/CanHandler.cpp b/src/boards/Payload/CanHandler/CanHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9ba14e44aba062f39f77f000624f9084b2ac35c
--- /dev/null
+++ b/src/boards/Payload/CanHandler/CanHandler.cpp
@@ -0,0 +1,149 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Alberto Nidasio
+ *
+ * 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 "CanHandler.h"
+
+#include <Payload/Configs/CanHandlerConfig.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+
+#include <functional>
+
+using namespace std;
+using namespace placeholders;
+using namespace Boardcore;
+using namespace Canbus;
+using namespace Common;
+using namespace CanConfig;
+using namespace Payload::CanHandlerConfig;
+
+namespace Payload
+{
+
+CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched)
+{
+ CanbusDriver::AutoBitTiming bitTiming;
+ bitTiming.baudRate = BAUD_RATE;
+ bitTiming.samplePoint = SAMPLE_POINT;
+ CanbusDriver::CanbusConfig config;
+ driver = new CanbusDriver(CAN2, config, bitTiming);
+
+ protocol =
+ new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1),
+ miosix::PRIORITY_MAX - 1);
+
+ // Accept messages only from the main and RIG board
+ protocol->addFilter(static_cast<uint8_t>(Board::MAIN),
+ static_cast<uint8_t>(Board::BROADCAST));
+ protocol->addFilter(static_cast<uint8_t>(Board::RIG),
+ static_cast<uint8_t>(Board::BROADCAST));
+ driver->init();
+}
+
+bool CanHandler::start()
+{
+ bool result;
+ // Add a task to periodically send the pitot data
+ result = scheduler->addTask( // sensor template
+ [&]()
+ {
+ protocol->enqueueData(
+ static_cast<uint8_t>(Priority::MEDIUM),
+ static_cast<uint8_t>(PrimaryType::SENSORS),
+ static_cast<uint8_t>(Board::PAYLOAD),
+ static_cast<uint8_t>(Board::BROADCAST),
+ static_cast<uint8_t>(SensorId::PITOT),
+ ModuleManager::getInstance()
+ .get<Sensors>()
+ ->getPitotLastSample());
+ },
+ PITOT_TRANSMISSION_PERIOD) != 0;
+
+ result =
+ result &&
+ scheduler->addTask( // status
+ [&]()
+ {
+ FlightModeManagerState state = ModuleManager::getInstance()
+ .get<FlightModeManager>()
+ ->getStatus()
+ .state;
+ protocol->enqueueSimplePacket(
+ static_cast<uint8_t>(Priority::MEDIUM),
+ static_cast<uint8_t>(PrimaryType::STATUS),
+ static_cast<uint8_t>(Board::PAYLOAD),
+ static_cast<uint8_t>(Board::BROADCAST),
+ static_cast<uint8_t>(state),
+ ((state == FlightModeManagerState::ARMED) ? 0x01 : 0x00));
+ },
+ STATUS_TRANSMISSION_PERIOD) != 0;
+ return protocol->start() && result;
+}
+
+bool CanHandler::isStarted() { return protocol->isStarted(); }
+
+void CanHandler::sendEvent(EventId event)
+{
+ protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL),
+ static_cast<uint8_t>(PrimaryType::EVENTS),
+ static_cast<uint8_t>(Board::PAYLOAD),
+ static_cast<uint8_t>(Board::BROADCAST),
+ static_cast<uint8_t>(event));
+}
+
+void CanHandler::handleCanMessage(const CanMessage &msg)
+{
+ PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType());
+
+ switch (msgType)
+ {
+ case PrimaryType::EVENTS:
+ {
+ handleCanEvent(msg);
+ break;
+ }
+ default:
+ {
+ LOG_WARN(logger, "Received unsupported message type: type={}",
+ msgType);
+ break;
+ }
+ }
+}
+
+void CanHandler::handleCanEvent(const Boardcore::Canbus::CanMessage &msg)
+{
+ EventId eventId = static_cast<EventId>(msg.getSecondaryType());
+
+ auto it = eventToEvent.find(eventId);
+
+ if (it != eventToEvent.end())
+ {
+ EventBroker::getInstance().post(it->second, TOPIC_CAN);
+ }
+ else
+ LOG_WARN(logger, "Received unsupported event: id={}", eventId);
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/CanHandler/CanHandler.h b/src/boards/Payload/CanHandler/CanHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f040b4e05bdaf1b7cf1793dc231c790e59d87f0
--- /dev/null
+++ b/src/boards/Payload/CanHandler/CanHandler.h
@@ -0,0 +1,90 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Alberto Nidasio
+ *
+ * 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 <Payload/BoardScheduler.h>
+#include <common/CanConfig.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+/**
+ * @class CanHandler
+ *
+ * @brief Payload implementation of CanProtocol with methods for ease of use
+ *
+ */
+class CanHandler : public Boardcore::Module
+{
+
+public:
+ /**
+ * @brief Creates the protocol and the driver and adds the filters.
+ *
+ * @warning With Payload motherboard CanDriver initialization could be
+ * blocking
+ */
+ explicit CanHandler(Boardcore::TaskScheduler *sched);
+
+ /**
+ * @brief Starts the protocol and adds the periodic messages to the task
+ * scheduler.
+ *
+ * @return true if the protocol started correctly and the tasks were
+ * successfully inserted
+ */
+ bool start();
+
+ /**
+ * @return true if the protocol is started
+ */
+ bool isStarted();
+
+ /**
+ * @brief Compile the the ID of the message and send the event trough
+ * CanProtocol
+ */
+ void sendEvent(Common::CanConfig::EventId event);
+
+private:
+ /**
+ * @brief Function called by CanProtocol when a new message is received
+ */
+ void handleCanMessage(const Boardcore::Canbus::CanMessage &msg);
+
+ /**
+ * @brief Converts the received CanEvent in Event and post it on TOPIC_CAN
+ */
+ void handleCanEvent(const Boardcore::Canbus::CanMessage &msg);
+
+ Boardcore::Canbus::CanbusDriver *driver;
+ Boardcore::Canbus::CanProtocol *protocol;
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
+
+ Boardcore::TaskScheduler *scheduler = nullptr;
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/ActuatorsConfigs.h b/src/boards/Payload/Configs/ActuatorsConfigs.h
new file mode 100644
index 0000000000000000000000000000000000000000..015793dc403fb1a6d6c98039f1fb171e0e0bb7cb
--- /dev/null
+++ b/src/boards/Payload/Configs/ActuatorsConfigs.h
@@ -0,0 +1,68 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli
+ *
+ * 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/timer/PWM.h>
+#include <drivers/timer/TimerUtils.h>
+
+namespace Payload
+{
+
+namespace ActuatorsConfigs
+{
+
+// Left servo
+static TIM_TypeDef* const SERVO_1_TIMER = TIM3;
+constexpr Boardcore::TimerUtils::Channel SERVO_1_PWM_CH =
+ Boardcore::TimerUtils::Channel::CHANNEL_1;
+
+constexpr float LEFT_SERVO_ROTATION = 120; // [deg]
+constexpr float LEFT_SERVO_MIN_PULSE = 900; // [us]
+constexpr float LEFT_SERVO_MAX_PULSE =
+ LEFT_SERVO_MIN_PULSE + 10 * LEFT_SERVO_ROTATION; // [us]
+
+// Right servo
+static TIM_TypeDef* const SERVO_2_TIMER = TIM3;
+constexpr Boardcore::TimerUtils::Channel SERVO_2_PWM_CH =
+ Boardcore::TimerUtils::Channel::CHANNEL_2;
+
+constexpr float RIGHT_SERVO_ROTATION = 120; // [deg]
+constexpr float RIGHT_SERVO_MIN_PULSE = 2100; // [us]
+constexpr float RIGHT_SERVO_MAX_PULSE =
+ RIGHT_SERVO_MIN_PULSE - 10 * RIGHT_SERVO_ROTATION; // [us]
+
+// Buzzer configs
+static TIM_TypeDef* const BUZZER_TIMER = TIM1;
+constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL =
+ Boardcore::TimerUtils::Channel::CHANNEL_1;
+constexpr uint32_t BUZZER_FREQUENCY = 1000;
+constexpr float BUZZER_DUTY_CYCLE = 0.5;
+
+constexpr uint32_t BUZZER_UPDATE_PERIOD = 100; // [ms]
+
+constexpr uint32_t ROCKET_SS_ARMED_PERIOD = 500;
+constexpr uint32_t ROCKET_SS_LAND_PERIOD = 1000; // [ms]
+
+} // namespace ActuatorsConfigs
+
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/CanHandlerConfig.h b/src/boards/Payload/Configs/CanHandlerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3ffa58ecf598a14a200ddb6caa846bd59d069fc
--- /dev/null
+++ b/src/boards/Payload/Configs/CanHandlerConfig.h
@@ -0,0 +1,36 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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
+
+namespace Payload
+{
+
+namespace CanHandlerConfig
+{
+
+constexpr unsigned int PITOT_TRANSMISSION_PERIOD = 50; // ms
+constexpr unsigned int STATUS_TRANSMISSION_PERIOD = 1000; // ms
+
+} // namespace CanHandlerConfig
+
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/FlightModeManagerConfig.h b/src/boards/Payload/Configs/FlightModeManagerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d09f70c850b4d443550ab7e79539a767e542a7f
--- /dev/null
+++ b/src/boards/Payload/Configs/FlightModeManagerConfig.h
@@ -0,0 +1,30 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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
+
+namespace Payload
+{
+constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000; // [ms]
+constexpr unsigned int APOGEE_TIMEOUT = 40 * 1000; // [ms]
+constexpr unsigned int LOGGING_DELAY = 30 * 1000; // [ms]
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/FlightStatsRecorderConfig.h b/src/boards/Payload/Configs/FlightStatsRecorderConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab003f917f89fa54dd28a893eec2ac57b5fc09a3
--- /dev/null
+++ b/src/boards/Payload/Configs/FlightStatsRecorderConfig.h
@@ -0,0 +1,33 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <stdint.h>
+
+namespace Payload
+{
+namespace FlightStatsRecorderConfig
+{
+constexpr uint32_t FSR_UPDATE_PERIOD = 100; // [ms]
+}
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/NASConfig.h b/src/boards/Payload/Configs/NASConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6b38e0856479c61358a139133e54234c59119ae
--- /dev/null
+++ b/src/boards/Payload/Configs/NASConfig.h
@@ -0,0 +1,67 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <algorithms/ReferenceValues.h>
+#include <common/ReferenceConfig.h>
+
+namespace Payload
+{
+
+namespace NASConfig
+{
+
+constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz
+
+constexpr int CALIBRATION_SAMPLES_COUNT = 20;
+constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms]
+
+constexpr int ACCELERATION_THRESHOLD_SAMPLE = 50;
+
+constexpr float ACCELERATION_THRESHOLD = 2; // [m/s^2]
+
+static const Boardcore::NASConfig config = {
+ UPDATE_PERIOD / 1000.0, // T
+ 0.0001f, // SIGMA_BETA
+ 0.3f, // SIGMA_W
+ 0.1f, // SIGMA_ACC
+ 0.1f, // SIGMA_MAG
+ 10.0f, // SIGMA_GPS
+ 4.3f, // SIGMA_BAR
+ 10.0f, // SIGMA_POS
+ 10.0f, // SIGMA_VEL
+ 10.0f, // SIGMA_PITOT
+ 1.0f, // P_POS
+ 10.0f, // P_POS_VERTICAL
+ 1.0f, // P_VEL
+ 10.0f, // P_VEL_VERTICAL
+ 0.01f, // P_ATT
+ 0.01f, // P_BIAS
+ 6.0f, // SATS_NUM
+ Common::ReferenceConfig::nedMag // NED_MAG
+};
+
+} // namespace NASConfig
+
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/PinHandlerConfig.h b/src/boards/Payload/Configs/PinHandlerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..d02c99e07c1334b573aa286c9aae5bf599f3d134
--- /dev/null
+++ b/src/boards/Payload/Configs/PinHandlerConfig.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
+ *
+ * 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>
+
+namespace Payload
+{
+
+constexpr unsigned int NC_DETACH_PIN_THRESHOLD = 20;
+constexpr Boardcore::PinTransition NC_DETACH_PIN_TRIGGER =
+ Boardcore::PinTransition::RISING_EDGE;
+
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/RadioConfig.h b/src/boards/Payload/Configs/RadioConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..ade3dd7f1d92fcbacd065d309dbe554bb7a4afcd
--- /dev/null
+++ b/src/boards/Payload/Configs/RadioConfig.h
@@ -0,0 +1,46 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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/Mavlink.h>
+
+namespace Payload
+{
+namespace RadioConfig
+{
+// Mavlink driver template parameters
+constexpr uint32_t RADIO_PKT_LENGTH = 255;
+constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20;
+constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+
+constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 200;
+
+constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50;
+constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10;
+
+constexpr uint8_t MAV_SYSTEM_ID = 171;
+constexpr uint8_t MAV_COMP_ID = 96;
+
+constexpr uint32_t MAVLINK_QUEUE_SIZE = 10;
+
+} // namespace RadioConfig
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/SensorsConfig.h b/src/boards/Payload/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..df790a830e160764b2b262a64da431bcab27f13d
--- /dev/null
+++ b/src/boards/Payload/Configs/SensorsConfig.h
@@ -0,0 +1,124 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/LPS28DFW/LPS28DFW.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+
+namespace Payload
+{
+namespace SensorsConfig
+{
+constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4;
+constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100;
+
+constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4;
+constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100;
+constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR =
+ Boardcore::LPS28DFW::FS_1260;
+
+constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR =
+ Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400;
+constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU =
+ Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
+constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR =
+ Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+
+constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE =
+ Boardcore::LIS2MDL::MD_CONTINUOUS;
+constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ;
+constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5;
+
+constexpr uint8_t UBXGPS_SAMPLE_RATE = 5;
+
+constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU =
+ Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
+
+constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS =
+ Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16;
+constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR =
+ Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416;
+constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE =
+ Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL;
+
+constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS =
+ Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000;
+constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR =
+ Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416;
+
+constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE =
+ Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
+constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION
+ LSM6DSRX_FIFO_TIMESTAMP_DECIMATION =
+ Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
+constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR
+ LSM6DSRX_FIFO_TEMPERATURE_BDR =
+ Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
+
+constexpr Boardcore::ADS131M08Defs::OversamplingRatio
+ ADS131M08_OVERSAMPLING_RATIO =
+ Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
+constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true;
+
+constexpr uint32_t LPS22DF_PERIOD = 20; // [ms] 50Hz
+constexpr uint32_t LPS28DFW_PERIOD = 20; // [ms] 50Hz
+constexpr uint32_t H3LIS331DL_PERIOD = 10; // [ms] 100Hz
+constexpr uint32_t LIS2MDL_PERIOD = 10; // [ms] 100Hz
+constexpr uint32_t UBXGPS_PERIOD = 1000 / UBXGPS_SAMPLE_RATE; // [ms]
+constexpr uint32_t LSM6DSRX_PERIOD = 20; // [ms] 50Hz
+constexpr uint32_t ADS131M08_PERIOD = 10; // [ms] 100Hz
+constexpr uint32_t IMU_PERIOD = 20; // [ms] Fake processed IMU 50Hz
+constexpr uint32_t MAG_CALIBRATION_PERIOD = 100; // [ms] 10Hz
+
+// ADC sensors configs
+constexpr float ADC_VOLTAGE_RANGE = 1.2f; // [V] Voltage reading range
+constexpr Boardcore::ADS131M08Defs::Channel BATTERY_VOLTAGE_CHANNEL =
+ Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
+constexpr Boardcore::ADS131M08Defs::Channel DYNAMIC_PRESSURE_CHANNEL =
+ Boardcore::ADS131M08Defs::Channel::CHANNEL_2;
+constexpr Boardcore::ADS131M08Defs::Channel STATIC_PRESSURE_CHANNEL =
+ Boardcore::ADS131M08Defs::Channel::CHANNEL_4;
+constexpr Boardcore::ADS131M08Defs::Channel CURRENT_CHANNEL =
+ Boardcore::ADS131M08Defs::Channel::CHANNEL_5;
+constexpr Boardcore::ADS131M08Defs::Channel CAM_BATTERY_VOLTAGE_CHANNEL =
+ Boardcore::ADS131M08Defs::Channel::CHANNEL_7;
+// ADC Voltage divider
+constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR =
+ (20.f / 2.4f) +
+ 1; // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider
+constexpr float CURRENT_CONVERSION_FACTOR =
+ (20.f / 4.f) / (12.f / (12.f + 33.f));
+constexpr float CURRENT_OFFSET = 0.133333333f; // V in ADC
+
+constexpr unsigned int NUMBER_OF_SENSORS = 8;
+
+// Calibration samples
+constexpr unsigned int CALIBRATION_SAMPLES = 20;
+constexpr unsigned int CALIBRATION_PERIOD = 100;
+
+} // namespace SensorsConfig
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/VerticalVelocityConfig.h b/src/boards/Payload/Configs/VerticalVelocityConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..28e36e0701311f5c037e7501747c1bbff4fe3b79
--- /dev/null
+++ b/src/boards/Payload/Configs/VerticalVelocityConfig.h
@@ -0,0 +1,37 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Raul Radu
+ *
+ * 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
+
+namespace Payload
+{
+
+namespace FailSafe
+{
+
+constexpr int FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD = 10; // [ms]
+constexpr float FAILSAFE_VERTICAL_VELOCITY_THRESHOLD = 12; // [m/s]
+constexpr int FAILSAFE_VERTICAL_VELOCITY_TRIGGER_CONFIDENCE = 30;
+
+} // namespace FailSafe
+
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/WESConfig.h b/src/boards/Payload/Configs/WESConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..36382803a9472568c8e9bfdf195847383cf561a5
--- /dev/null
+++ b/src/boards/Payload/Configs/WESConfig.h
@@ -0,0 +1,45 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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 <miosix.h>
+
+namespace Payload
+{
+
+namespace WESConfig
+{
+
+constexpr uint32_t WES_CALIBRATION_TIMEOUT =
+ 5 * 1000; // time needed for the first loop [ms]
+
+constexpr int WES_CALIBRATION_SAMPLE_NUMBER =
+ 20; // number to sample to take in the first loop
+constexpr uint32_t WES_CALIBRATION_UPDATE_PERIOD =
+ WES_CALIBRATION_TIMEOUT / WES_CALIBRATION_SAMPLE_NUMBER;
+constexpr uint32_t WES_PREDICTION_UPDATE_PERIOD =
+ 100; // update period of WES[ms]
+
+} // namespace WESConfig
+
+} // namespace Payload
diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..f8dbd72192a23106363a84d0a2061ac8db5176e9
--- /dev/null
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli
+ *
+ * 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
+
+namespace Payload
+{
+
+namespace WingConfig
+{
+// Algorithm configuration
+
+#if defined(EUROC)
+constexpr float DEFAULT_TARGET_LAT = 39.389733;
+constexpr float DEFAULT_TARGET_LON = -8.288992;
+#elif defined(ROCCARASO)
+constexpr float DEFAULT_TARGET_LAT = 41.809216;
+constexpr float DEFAULT_TARGET_LON = 14.055310;
+#else // Milan
+constexpr float DEFAULT_TARGET_LAT = 41.809216;
+constexpr float DEFAULT_TARGET_LON = 14.055310;
+#endif
+
+constexpr int WING_UPDATE_PERIOD = 1000; // [ms]
+
+constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = 2.09439;
+constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -2.09439;
+
+constexpr int GUIDANCE_CONFIDENCE = 15;
+constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD = 250; //[m]
+constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD = 150; //[m]
+constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50; //[m]
+
+// TODO check this parameter preflight
+constexpr float KP = 0.4; //[m]
+constexpr float KI = 0.08; //[m]
+constexpr float ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE = 300; // [meters]
+
+constexpr int ALTITUDE_TRIGGER_CONFIDENCE = 10; // [number of sample]
+constexpr int ALTITUDE_TRIGGER_PERIOD = 100; //[ms]
+} // namespace WingConfig
+
+} // namespace Payload
diff --git a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..faf62811c45892dd27aac1dcf5d63833f2db9f2b
--- /dev/null
+++ b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
@@ -0,0 +1,177 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 <Payload/Configs/FlightStatsRecorderConfig.h>
+#include <Payload/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+
+#include <Eigen/Core>
+
+using namespace Boardcore;
+using namespace Payload::FlightStatsRecorderConfig;
+using namespace Eigen;
+using namespace miosix;
+
+namespace Payload
+{
+FlightStatsRecorder::FlightStatsRecorder(TaskScheduler* sched)
+ : scheduler(sched)
+{
+ // Init the data structure to avoid UB
+ stats.max_airspeed_pitot = 0;
+ stats.apogee_alt = 0;
+ stats.apogee_lat = 0;
+ stats.apogee_lon = 0;
+ stats.apogee_ts = 0;
+ stats.cpu_load = 0;
+ stats.dpl_max_acc = 0;
+ stats.dpl_ts = 0;
+ stats.free_heap = 0;
+ stats.liftoff_max_acc = 0;
+ stats.liftoff_max_acc_ts = 0;
+ stats.max_z_speed = 0;
+ stats.max_z_speed_ts = 0;
+ stats.min_pressure = 0;
+ stats.max_speed_altitude = 0;
+}
+
+bool FlightStatsRecorder::start()
+{
+ return scheduler->addTask([&]() { update(); }, FSR_UPDATE_PERIOD,
+ TaskScheduler::Policy::RECOVER);
+}
+
+void FlightStatsRecorder::update()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // FMM state
+ FlightModeManagerState flightState =
+ modules.get<FlightModeManager>()->getStatus().state;
+
+ // Data gathering
+ Sensors* sensor = modules.get<Sensors>();
+ AccelerometerData accData = sensor->getIMULastSample();
+ PressureData baroData = sensor->getStaticPressureLastSample();
+ PitotData pitotData = sensor->getPitotLastSample();
+ GPSData gpsData = sensor->getGPSLastSample();
+ NASState nasData = modules.get<NASController>()->getNasState();
+
+ // Store the apogee ts
+ uint64_t previousApogee = stats.apogee_ts;
+
+ // Update the data
+ updateAcc(flightState, accData);
+ updateBaro(flightState, baroData);
+ updatePitot(flightState, pitotData);
+ updateNAS(flightState, nasData);
+
+ // If the apogee ts is different update the GPS apogee coordinates
+ if (previousApogee != stats.apogee_ts)
+ {
+ Lock<FastMutex> l(mutex);
+ // minus to get positive altitude
+ stats.apogee_alt = -nasData.d;
+ stats.apogee_lat = gpsData.latitude;
+ stats.apogee_lon = gpsData.longitude;
+ }
+}
+
+mavlink_payload_stats_tm_t FlightStatsRecorder::getStats()
+{
+ Lock<FastMutex> l(mutex);
+ return stats;
+}
+
+void FlightStatsRecorder::updateAcc(FlightModeManagerState flightState,
+ AccelerometerData data)
+{
+ Vector3f accelerations = static_cast<Vector3f>(data);
+ float norm = accelerations.norm();
+
+ if (flightState == FlightModeManagerState::ASCENDING)
+ {
+ Lock<FastMutex> l(mutex);
+
+ stats.liftoff_max_acc = std::max(stats.liftoff_max_acc, norm);
+ bool changed = stats.liftoff_max_acc == norm;
+
+ // In case of a change update the timestamp
+ stats.liftoff_max_acc_ts =
+ changed ? data.accelerationTimestamp : stats.liftoff_max_acc_ts;
+ }
+ else if (flightState >= FlightModeManagerState::DROGUE_DESCENT)
+ {
+ Lock<FastMutex> l(mutex);
+
+ stats.dpl_max_acc = std::max(stats.dpl_max_acc, norm);
+
+ // Change the timestamp of DPL if not already done
+ stats.dpl_ts =
+ stats.dpl_ts == 0 ? data.accelerationTimestamp : stats.dpl_ts;
+ }
+}
+
+void FlightStatsRecorder::updateBaro(FlightModeManagerState flightState,
+ PressureData data)
+{
+ Lock<FastMutex> l(mutex);
+
+ if (stats.min_pressure == 0)
+ {
+ stats.min_pressure = data.pressure;
+ }
+
+ // Update the min
+ stats.min_pressure = std::min(stats.min_pressure, data.pressure);
+}
+
+void FlightStatsRecorder::updatePitot(FlightModeManagerState flightState,
+ PitotData data)
+{
+ Lock<FastMutex> l(mutex);
+
+ stats.max_airspeed_pitot =
+ std::max(stats.max_airspeed_pitot, data.airspeed);
+}
+
+void FlightStatsRecorder::updateNAS(FlightModeManagerState flightState,
+ NASState state)
+{
+ if (flightState == FlightModeManagerState::ASCENDING)
+ {
+ Lock<FastMutex> l(mutex);
+
+ stats.max_z_speed = std::max(stats.max_z_speed, -state.vd);
+ bool changed = stats.max_z_speed == -state.vd;
+
+ // Change the max speed altitude and timestamp if the max speed changed
+ stats.max_speed_altitude =
+ changed ? -state.d : stats.max_speed_altitude;
+
+ stats.max_z_speed_ts = changed ? state.timestamp : stats.max_z_speed_ts;
+ }
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.h b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.h
new file mode 100644
index 0000000000000000000000000000000000000000..1470c416862cc738f3d757390203a669e77d5074
--- /dev/null
+++ b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.h
@@ -0,0 +1,80 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <algorithms/ADA/ADAData.h>
+#include <algorithms/NAS/NASState.h>
+#include <common/Mavlink.h>
+#include <scheduler/TaskScheduler.h>
+#include <sensors/SensorData.h>
+#include <sensors/analog/Pitot/PitotData.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+class FlightStatsRecorder : public Boardcore::Module
+{
+public:
+ FlightStatsRecorder(Boardcore::TaskScheduler* sched);
+
+ /**
+ * @brief Adds a task to the scheduler to update the stats
+ */
+ bool start();
+
+ /**
+ * @brief Gets the packet already populated
+ */
+ mavlink_payload_stats_tm_t getStats();
+
+private:
+ /**
+ * @brief Update method that gathers all the info to update the data
+ * structure
+ */
+ void update();
+
+ // Update methods
+ void updateAcc(FlightModeManagerState flightState,
+ Boardcore::AccelerometerData data);
+ void updateBaro(FlightModeManagerState flightState,
+ Boardcore::PressureData data);
+ void updateDPLPressure(FlightModeManagerState flightState,
+ Boardcore::PressureData data);
+ void updatePitot(FlightModeManagerState flightState,
+ Boardcore::PitotData data);
+ void updateNAS(FlightModeManagerState flightState,
+ Boardcore::NASState data);
+
+ // Update scheduler to update all the data
+ Boardcore::TaskScheduler* scheduler = nullptr;
+
+ // Data structure
+ mavlink_payload_stats_tm_t stats;
+
+ // Update mutex
+ miosix::FastMutex mutex;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/PinHandler/PinHandler.cpp b/src/boards/Payload/PinHandler/PinHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4005f105beded8ce6cb1a0cacda21029020bba26
--- /dev/null
+++ b/src/boards/Payload/PinHandler/PinHandler.cpp
@@ -0,0 +1,75 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Federico Mandelli
+ *
+ * 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 <Payload/Configs/PinHandlerConfig.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
+#include <miosix.h>
+
+#include <functional>
+
+using namespace std::placeholders;
+using namespace miosix;
+using namespace Boardcore;
+using namespace Common;
+
+// TODO change hwmapping for Euroc this is need to have the right name
+using nosecone_detach = gpios::liftoff_detach;
+
+namespace Payload
+{
+
+void PinHandler::onExpulsionPinTransition(PinTransition transition)
+{
+ if (transition == NC_DETACH_PIN_TRIGGER)
+ EventBroker::getInstance().post(FLIGHT_NC_DETACHED, TOPIC_FLIGHT);
+}
+
+bool PinHandler::start()
+{
+ running = PinObserver::getInstance().start();
+ return running;
+}
+
+bool PinHandler::isStarted() { return running; }
+
+std::map<PinsList, PinData> PinHandler::getPinsData()
+{
+ std::map<PinsList, PinData> data;
+
+ data[PinsList::NOSECONE_PIN] =
+ PinObserver::getInstance().getPinData(nosecone_detach::getPin());
+
+ return data;
+}
+
+PinHandler::PinHandler() : running(false)
+{
+ PinObserver::getInstance().registerPinCallback(
+ nosecone_detach::getPin(),
+ std::bind(&PinHandler::onExpulsionPinTransition, this, _1),
+ NC_DETACH_PIN_THRESHOLD);
+}
+} // namespace Payload
diff --git a/src/boards/Payload/PinHandler/PinHandler.h b/src/boards/Payload/PinHandler/PinHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..45dcff2c9fd1b27c766b43120161051ce5b06f0a
--- /dev/null
+++ b/src/boards/Payload/PinHandler/PinHandler.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Federico Mandelli
+ *
+ * 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/Mavlink.h>
+#include <diagnostic/PrintLogger.h>
+#include <utils/PinObserver/PinObserver.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+
+/**
+ * @brief This class contains the handlers for the detach pins on the rocket.
+ *
+ * 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::Module
+{
+public:
+ PinHandler();
+
+ bool start();
+
+ bool isStarted();
+
+ /**
+ * @brief Called when the deployment servo actuation is detected via the
+ * optical sensor.
+ */
+ void onExpulsionPinTransition(Boardcore::PinTransition transition);
+
+ /**
+ * @brief Returns a vector with all the pins data.
+ */
+ std::map<PinsList, Boardcore::PinData> getPinsData();
+
+private:
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("pinhandler");
+
+ std::atomic<bool> running;
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc84d7db797f658d561075c1f89e6967b8794f88
--- /dev/null
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -0,0 +1,517 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 <Payload/Actuators/Actuators.h>
+#include <Payload/AltitudeTrigger/AltitudeTrigger.h>
+#include <Payload/Buses.h>
+#include <Payload/Radio/Radio.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/StateMachines/WingController/WingController.h>
+#include <Payload/TMRepository/TMRepository.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/interrupt/external_interrupts.h>
+#include <events/EventBroker.h>
+#include <radio/SX1278/SX1278Frontends.h>
+
+#include <memory>
+
+using namespace Boardcore;
+using namespace Common;
+
+#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ if (modules.get<Payload::Radio>()->transceiver)
+ {
+ modules.get<Payload::Radio>()->transceiver->handleDioIRQ();
+ }
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ if (modules.get<Payload::Radio>()->transceiver)
+ {
+ modules.get<Payload::Radio>()->transceiver->handleDioIRQ();
+ }
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ if (modules.get<Payload::Radio>()->transceiver)
+ {
+ modules.get<Payload::Radio>()->transceiver->handleDioIRQ();
+ }
+}
+namespace Payload
+{
+
+Radio::Radio(TaskScheduler* sched) : scheduler(sched) {}
+
+bool Radio::start()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ // Config the transceiver
+ SX1278Fsk::Config config = {
+ .freq_rf = 868000000,
+ .freq_dev = 50000,
+ .bitrate = 48000,
+ .rx_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000,
+ .afc_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000,
+ .ocp = 120,
+ .power = 13,
+ .shaping = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0,
+ .dc_free = Boardcore::SX1278Fsk::Config::DcFree::WHITENING,
+ .enable_crc = false};
+
+ std::unique_ptr<SX1278::ISX1278Frontend> frontend =
+ std::make_unique<Skyward433Frontend>();
+
+ transceiver = new SX1278Fsk(
+ modules.get<Buses>()->spi6, miosix::radio::cs::getPin(),
+ miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(),
+ miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_128,
+ std::move(frontend));
+
+ // Config the radio
+ SX1278Fsk::Error error = transceiver->init(config);
+
+ // Add periodic telemetry send task
+ uint8_t result =
+ scheduler->addTask([=]() { this->sendPeriodicMessage(); },
+ RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD,
+ TaskScheduler::Policy::RECOVER);
+ result *= scheduler->addTask(
+ [&]()
+ {
+ this->enqueueMsg(
+ modules.get<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0));
+ },
+ RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2,
+ TaskScheduler::Policy::RECOVER);
+
+ // Config mavDriver
+ mavDriver = new MavDriver(
+ transceiver,
+ [=](MavDriver*, const mavlink_message_t& msg)
+ { this->handleMavlinkMessage(msg); },
+ RadioConfig::RADIO_SLEEP_AFTER_SEND,
+ RadioConfig::RADIO_OUT_BUFFER_MAX_AGE);
+
+ // Check radio failure
+ if (error != SX1278Fsk::Error::NONE)
+ {
+ return false;
+ }
+
+ // Start the mavlink driver thread
+ return mavDriver->start() && result != 0;
+}
+
+void Radio::sendAck(const mavlink_message_t& msg)
+{
+ mavlink_message_t ackMsg;
+ mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid,
+ msg.seq);
+ enqueueMsg(ackMsg);
+}
+
+void Radio::sendNack(const mavlink_message_t& msg)
+{
+ mavlink_message_t nackMsg;
+ mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid,
+ msg.seq);
+ enqueueMsg(nackMsg);
+}
+
+void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); }
+
+bool Radio::isStarted()
+{
+ return mavDriver->isStarted() && scheduler->isRunning();
+}
+
+void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ switch (msg.msgid)
+ {
+ case MAVLINK_MSG_ID_PING_TC:
+ {
+ // Do nothing, just add the ack to the queue
+ break;
+ }
+ case MAVLINK_MSG_ID_COMMAND_TC:
+ {
+ // Let the handle command reply to the message
+ return handleCommand(msg);
+ }
+ case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
+ {
+ SystemTMList tmId = static_cast<SystemTMList>(
+ mavlink_msg_system_tm_request_tc_get_tm_id(&msg));
+
+ if (tmId == SystemTMList::MAV_SENSORS_STATE_ID)
+ {
+ mavlink_message_t msg;
+ mavlink_sensor_state_tm_t tm;
+ auto sensorsState = ModuleManager::getInstance()
+ .get<Sensors>()
+ ->getSensorInfo();
+ for (SensorInfo i : sensorsState)
+ {
+ strcpy(tm.sensor_name, i.id.c_str());
+ tm.state = 0;
+ if (i.isEnabled)
+ {
+ tm.state += 1;
+ }
+ if (i.isInitialized)
+ {
+ tm.state += 2;
+ }
+ mavlink_msg_sensor_state_tm_encode(
+ RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID,
+ &msg, &tm);
+ enqueueMsg(msg);
+ }
+ }
+ else
+ {
+
+ // Add to the queue the respose
+ mavlink_message_t response =
+ modules.get<TMRepository>()->packSystemTm(tmId, msg.msgid,
+ msg.seq);
+
+ // Add the response to the queue
+ enqueueMsg(response);
+
+ // Check if the TM repo answered with a NACK. If so the function
+ // must return to avoid sending a default ack
+ if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+ {
+ return;
+ }
+ }
+ break;
+ }
+ case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
+ {
+ SensorsTMList tmId = static_cast<SensorsTMList>(
+ mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg));
+
+ // Add to the queue the respose
+ mavlink_message_t response =
+ modules.get<TMRepository>()->packSensorsTm(tmId, msg.msgid,
+ msg.seq);
+
+ // Add the response to the queue
+ enqueueMsg(response);
+
+ // Check if the TM repo answered with a NACK. If so the function
+ // must return to avoid sending a default ack
+ if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+ {
+ return;
+ }
+
+ break;
+ }
+ case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC:
+ {
+ ServosList servoId = static_cast<ServosList>(
+ mavlink_msg_servo_tm_request_tc_get_servo_id(&msg));
+
+ // Add to the queue the respose
+ mavlink_message_t response =
+ modules.get<TMRepository>()->packServoTm(servoId, msg.msgid,
+ msg.seq);
+
+ // Add the response to the queue
+ mavDriver->enqueueMsg(response);
+
+ // Check if the TM repo answered with a NACK. If so the function
+ // must return to avoid sending a default ack
+ if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+ {
+ return;
+ }
+
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
+ {
+ ServosList servoId = static_cast<ServosList>(
+ mavlink_msg_set_servo_angle_tc_get_servo_id(&msg));
+ float angle = mavlink_msg_set_servo_angle_tc_get_angle(&msg);
+
+ // Move the servo, if it fails send a nack
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE ||
+ !modules.get<Actuators>()->setServoAngle(servoId, angle))
+ {
+ return sendNack(msg);
+ }
+
+ break;
+ }
+ case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
+ {
+ ServosList servoId = static_cast<ServosList>(
+ mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
+
+ // Send nack if the FMM is not in test mode
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE ||
+ !modules.get<Actuators>()->wiggleServo(servoId))
+ {
+ return sendNack(msg);
+ }
+
+ break;
+ }
+ case MAVLINK_MSG_ID_RESET_SERVO_TC:
+ {
+ ServosList servoId = static_cast<ServosList>(
+ mavlink_msg_reset_servo_tc_get_servo_id(&msg));
+
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE ||
+ !modules.get<Actuators>()->setServo(servoId, 0))
+ {
+ return sendNack(msg);
+ }
+
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC:
+ {
+ float altitude =
+ mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE)
+ {
+ return sendNack(msg);
+ }
+ modules.get<NASController>()->setReferenceAltitude(altitude);
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
+ {
+ float temperature =
+ mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
+
+ temperature += 273.15;
+
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE)
+ {
+ return sendNack(msg);
+ }
+ modules.get<NASController>()->setReferenceAltitude(temperature);
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
+ {
+ float altitude =
+ mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg);
+
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE)
+ {
+ return sendNack(msg);
+ }
+ modules.get<AltitudeTrigger>()->setDeploymentAltitude(altitude);
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_ORIENTATION_TC:
+ {
+ float yaw = mavlink_msg_set_orientation_tc_get_yaw(&msg);
+ float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg);
+ float roll = mavlink_msg_set_orientation_tc_get_roll(&msg);
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE)
+ {
+ return sendNack(msg);
+ }
+ modules.get<NASController>()->setOrientation(yaw, pitch, roll);
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_COORDINATES_TC:
+ {
+ float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
+ float longitude =
+ mavlink_msg_set_coordinates_tc_get_longitude(&msg);
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE)
+ {
+ return sendNack(msg);
+ }
+ modules.get<NASController>()->setCoordinates(
+ Eigen::Vector2f(latitude, longitude));
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_TARGET_COORDINATES_TC:
+ {
+ float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
+ float longitude =
+ mavlink_msg_set_coordinates_tc_get_longitude(&msg);
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE)
+ {
+ return sendNack(msg);
+ }
+ modules.get<WingController>()->setTargetPosition(
+ Eigen::Vector2f(latitude, longitude));
+ break;
+ }
+ case MAVLINK_MSG_ID_SET_ALGORITHM_TC:
+ {
+ u_int8_t algoID =
+ mavlink_msg_set_algorithm_tc_get_algorithm_number(&msg);
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE ||
+ !modules.get<WingController>()->selectAlgorithm(algoID))
+ {
+ return sendNack(msg);
+ }
+
+ break;
+ }
+ 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);
+
+ // Send the event only if the flight mode manager is in test mode
+ if (modules.get<FlightModeManager>()->getStatus().state !=
+ FlightModeManagerState::TEST_MODE)
+ {
+ return sendNack(msg);
+ }
+
+ EventBroker::getInstance().post(topicId, eventId);
+ break;
+ }
+ }
+
+ // At the end send the ack message
+ sendAck(msg);
+}
+
+void Radio::handleCommand(const mavlink_message_t& msg)
+{
+ MavCommandList commandId = static_cast<MavCommandList>(
+ mavlink_msg_command_tc_get_command_id(&msg));
+
+ static const std::map<MavCommandList, Events> commandToEvent{
+ {MAV_CMD_ARM, TMTC_ARM},
+ {MAV_CMD_DISARM, TMTC_DISARM},
+ {MAV_CMD_CALIBRATE, TMTC_CALIBRATE},
+ {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT},
+ {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH},
+ {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING},
+ {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE},
+ {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION},
+ {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT},
+ {MAV_CMD_START_LOGGING, TMTC_START_LOGGING},
+ {MAV_CMD_STOP_LOGGING, TMTC_STOP_LOGGING},
+ {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD},
+ {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE},
+ {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE},
+ {MAV_CMD_START_RECORDING, TMTC_START_RECORDING},
+ {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING},
+ };
+ switch (commandId)
+ {
+ case MAV_CMD_SAVE_CALIBRATION:
+ {
+ // Save the sensor calibration and adopt it
+ ModuleManager::getInstance().get<Sensors>()->writeMagCalibration();
+ break;
+ }
+ default:
+ {
+ // If the command is not a particular one, look for it inside the
+ // map
+ auto it = commandToEvent.find(commandId);
+
+ if (it != commandToEvent.end())
+ {
+ EventBroker::getInstance().post(it->second, TOPIC_TMTC);
+ }
+ else
+ {
+ return sendNack(msg);
+ }
+ }
+ }
+ // Acknowledge the message
+ sendAck(msg);
+}
+
+void Radio::sendPeriodicMessage()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Send all the queue messages
+ {
+ Lock<FastMutex> lock(queueMutex);
+
+ for (uint8_t i = 0; i < messageQueueIndex; i++)
+ {
+ mavDriver->enqueueMsg(messageQueue[i]);
+ }
+
+ // Reset the index
+ messageQueueIndex = 0;
+ }
+
+ mavDriver->enqueueMsg(
+ modules.get<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0));
+}
+
+void Radio::enqueueMsg(const mavlink_message_t& msg)
+{
+ {
+ Lock<FastMutex> lock(queueMutex);
+
+ // Insert the message inside the queue only if there is enough space
+ if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE)
+ {
+ messageQueue[messageQueueIndex] = msg;
+ messageQueueIndex++;
+ }
+ }
+}
+} // namespace Payload
diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h
new file mode 100644
index 0000000000000000000000000000000000000000..4aa33b844b5eeffb2c7e2e1785f1c9c5532a2ddc
--- /dev/null
+++ b/src/boards/Payload/Radio/Radio.h
@@ -0,0 +1,102 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 <Payload/Configs/RadioConfig.h>
+#include <common/Mavlink.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/SX1278/SX1278Fsk.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
+ RadioConfig::RADIO_OUT_QUEUE_SIZE,
+ RadioConfig::RADIO_MAV_MSG_LENGTH>;
+
+class Radio : public Boardcore::Module
+{
+public:
+ Radio(Boardcore::TaskScheduler* sched);
+
+ /**
+ * @brief Starts the MavlinkDriver
+ */
+ [[nodiscard]] bool start();
+
+ /**
+ * @brief Sends via radio an acknowledge message about the parameter passed
+ * message
+ */
+ void sendAck(const mavlink_message_t& msg);
+
+ /**
+ * @brief Sends via radio an non-acknowledge message about the parameter
+ * passed message
+ */
+ void sendNack(const mavlink_message_t& msg);
+
+ /**
+ * @brief Saves the MavlinkDriver and transceiver status
+ */
+ void logStatus();
+
+ /**
+ * @brief Returns if the radio module is correctly started
+ */
+ bool isStarted();
+
+ Boardcore::SX1278Fsk* transceiver = nullptr;
+ MavDriver* mavDriver = nullptr;
+
+private:
+ /**
+ * @brief Called by the MavlinkDriver when a message is received
+ */
+ void handleMavlinkMessage(const mavlink_message_t& msg);
+
+ /**
+ * @brief Called by the handleMavlinkMessage to handle a command message
+ */
+ void handleCommand(const mavlink_message_t& msg);
+
+ /**
+ * @brief Sends the periodic telemetry
+ */
+ void sendPeriodicMessage();
+
+ /**
+ * @brief Inserts the mavlink message into the queue
+ */
+ void enqueueMsg(const mavlink_message_t& msg);
+
+ // Messages queue
+ mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE];
+ uint32_t messageQueueIndex = 0;
+ miosix::FastMutex queueMutex;
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
+ Boardcore::TaskScheduler* scheduler = nullptr;
+};
+} // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb1b56949d717d3b2a8c1c67ced98a799d70f150
--- /dev/null
+++ b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp
@@ -0,0 +1,103 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Sensors/RotatedIMU/RotatedIMU.h>
+
+using namespace Eigen;
+using namespace Boardcore;
+
+namespace Payload
+{
+
+RotatedIMU::RotatedIMU(std::function<AccelerometerData()> accSampleFunction,
+ std::function<MagnetometerData()> magSampleFunction,
+ std::function<GyroscopeData()> gyroSampleFunction)
+ : accSample(accSampleFunction), magSample(magSampleFunction),
+ gyroSample(gyroSampleFunction), accT(Matrix3f::Identity()),
+ magT(Matrix3f::Identity()), gyroT(Matrix3f::Identity())
+{
+}
+
+void RotatedIMU::addAccTransformation(const Matrix3f& t) { accT = t * accT; }
+void RotatedIMU::addMagTransformation(const Matrix3f& t) { magT = t * magT; }
+void RotatedIMU::addGyroTransformation(const Matrix3f& t) { gyroT = t * gyroT; }
+
+void RotatedIMU::resetTransformations()
+{
+ accT = Matrix3f::Identity();
+ magT = Matrix3f::Identity();
+ gyroT = Matrix3f::Identity();
+}
+
+RotatedIMUData RotatedIMU::sampleImpl()
+{
+ Vector3f accResult = accT * static_cast<Vector3f>(accSample());
+ Vector3f magResult = magT * static_cast<Vector3f>(magSample());
+ Vector3f gyroResult = gyroT * static_cast<Vector3f>(gyroSample());
+
+ return {AccelerometerData{accResult}, GyroscopeData{gyroResult},
+ MagnetometerData{magResult}};
+}
+
+// Static functions
+Matrix3f RotatedIMU::rotateAroundX(float angle)
+{
+ Matrix3f rotation;
+ angle = angle * EIGEN_PI / 180.f;
+
+ // clang-format off
+ rotation = Matrix3f{{1, 0, 0},
+ {0, cosf(angle), -sinf(angle)},
+ {0, sinf(angle), cosf(angle)}};
+ // clang-format on
+
+ return rotation;
+}
+
+Matrix3f RotatedIMU::rotateAroundY(float angle)
+{
+ Matrix3f rotation;
+ angle = angle * EIGEN_PI / 180.f;
+
+ // clang-format off
+ rotation = Matrix3f{{cosf(angle), 0, sinf(angle)},
+ {0, 1, 0},
+ {-sinf(angle), 0, cosf(angle)}};
+ // clang-format on
+
+ return rotation;
+}
+
+Matrix3f RotatedIMU::rotateAroundZ(float angle)
+{
+ Matrix3f rotation;
+ angle = angle * EIGEN_PI / 180.f;
+
+ // clang-format off
+ rotation = Matrix3f{{cosf(angle), -sinf(angle), 0},
+ {sinf(angle), cosf(angle), 0},
+ {0, 0, 1}};
+ // clang-format on
+
+ return rotation;
+}
+} // namespace Payload
diff --git a/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.h b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.h
new file mode 100644
index 0000000000000000000000000000000000000000..d939e8e54a8bb8c417dff47fe89f93a70f60304d
--- /dev/null
+++ b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.h
@@ -0,0 +1,120 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Sensors/RotatedIMU/RotatedIMUData.h>
+#include <sensors/Sensor.h>
+
+#include <Eigen/Eigen>
+#include <functional>
+
+namespace Payload
+{
+/**
+ * @brief Creates a "fake" sensor which allows the user
+ * to transform the values with transformation matrices.
+ * The default configuration (thus the reset one)
+ * comprehends identity matrices as transformation ones.
+ *
+ * @warning This is NOT a thread safe class
+ */
+class RotatedIMU : public Boardcore::Sensor<RotatedIMUData>
+{
+public:
+ /**
+ * @brief Construct a new Rotated IMU object
+ *
+ * @param accSampleFunction Function to call to retrieve an accelerometer
+ * sample
+ * @param magSampleFunction Function to call to retrieve a magnetometer
+ * sample
+ * @param gyroSampleFunction Function to call to retrieve a gyroscope sample
+ */
+ RotatedIMU(
+ std::function<Boardcore::AccelerometerData()> accSampleFunction = []()
+ { return Boardcore::AccelerometerData{}; },
+ std::function<Boardcore::MagnetometerData()> magSampleFunction = []()
+ { return Boardcore::MagnetometerData{}; },
+ std::function<Boardcore::GyroscopeData()> gyroSampleFunction = []()
+ { return Boardcore::GyroscopeData{}; });
+
+ /**
+ * @brief Multiplies the current accelerometer transformation
+ * @param transformation Transformation matrix to be multiplied to the
+ * current one
+ */
+ void addAccTransformation(const Eigen::Matrix3f& t);
+
+ /**
+ * @brief Multiplies the current magnetometer transformation
+ * @param transformation Transformation matrix to be multiplied to the
+ * current one
+ */
+ void addMagTransformation(const Eigen::Matrix3f& t);
+
+ /**
+ * @brief Multiplies the current gyroscope transformation
+ * @param transformation Transformation matrix to be multiplied to the
+ * current one
+ */
+ void addGyroTransformation(const Eigen::Matrix3f& t);
+
+ /**
+ * @brief Resets all the transformations to the original (Identity) ones
+ */
+ void resetTransformations();
+
+ bool init() override { return true; }
+ bool selfTest() override { return true; }
+
+ /**
+ * @brief Creates a rotation matrix around the X axis
+ * @param angle Angle in degrees
+ */
+ static Eigen::Matrix3f rotateAroundX(float angle);
+
+ /**
+ * @brief Creates a rotation matrix around the Y axis
+ * @param angle Angle in degrees
+ */
+ static Eigen::Matrix3f rotateAroundY(float angle);
+
+ /**
+ * @brief Creates a rotation matrix around the Z axis
+ * @param angle Angle in degrees
+ */
+ static Eigen::Matrix3f rotateAroundZ(float angle);
+
+private:
+ RotatedIMUData sampleImpl() override;
+
+ // Functions to sample the under neath sensors
+ std::function<Boardcore::AccelerometerData()> accSample;
+ std::function<Boardcore::MagnetometerData()> magSample;
+ std::function<Boardcore::GyroscopeData()> gyroSample;
+
+ // Transformation matrices
+ Eigen::Matrix3f accT;
+ Eigen::Matrix3f magT;
+ Eigen::Matrix3f gyroT;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/Sensors/RotatedIMU/RotatedIMUData.h b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMUData.h
new file mode 100644
index 0000000000000000000000000000000000000000..455d1ae206102886a1d8d222d9c75332d123b181
--- /dev/null
+++ b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMUData.h
@@ -0,0 +1,60 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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>
+
+struct RotatedIMUData : public Boardcore::AccelerometerData,
+ public Boardcore::MagnetometerData,
+ public Boardcore::GyroscopeData
+{
+ RotatedIMUData()
+ : AccelerometerData{0, 0.0, 0.0, 0.0},
+ MagnetometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0}
+
+ {
+ }
+
+ RotatedIMUData(AccelerometerData acc, GyroscopeData gyr,
+ MagnetometerData mag)
+ : AccelerometerData(acc), MagnetometerData(mag), GyroscopeData(gyr)
+ {
+ }
+
+ static std::string header()
+ {
+ return "accelerationTimestamp,accelerationX,accelerationY,"
+ "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+ "angularSpeedY,angularSpeedZ,magneticFieldTimestamp,"
+ "magneticFieldX,magneticFieldY,magneticFieldZ\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << accelerationTimestamp << "," << accelerationX << ","
+ << accelerationY << "," << accelerationZ << ","
+ << angularSpeedTimestamp << "," << angularSpeedX << ","
+ << angularSpeedY << "," << angularSpeedZ << ","
+ << magneticFieldTimestamp << "," << magneticFieldX << ","
+ << magneticFieldY << "," << magneticFieldZ << "\n";
+ }
+};
diff --git a/src/boards/Payload/Sensors/SensorData.h b/src/boards/Payload/Sensors/SensorData.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ea2399f7475cba2dc532c5298021dec61baa3fc
--- /dev/null
+++ b/src/boards/Payload/Sensors/SensorData.h
@@ -0,0 +1,97 @@
+
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Federico Mandelli
+ *
+ * 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/LPS28DFW/LPS28DFWData.h>
+namespace Payload
+{
+struct LPS28DFW_1Data : Boardcore::LPS28DFWData
+{
+ explicit LPS28DFW_1Data(const Boardcore::LPS28DFWData& data)
+ : Boardcore::LPS28DFWData(data)
+ {
+ }
+
+ LPS28DFW_1Data() {}
+
+ static std::string header()
+ {
+ return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n ";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << pressureTimestamp << "," << pressure << ","
+ << temperatureTimestamp << "," << temperature << "\n";
+ }
+};
+
+struct LPS28DFW_2Data : Boardcore::LPS28DFWData
+{
+ explicit LPS28DFW_2Data(const Boardcore::LPS28DFWData& data)
+ : Boardcore::LPS28DFWData(data)
+ {
+ }
+
+ LPS28DFW_2Data() {}
+
+ static std::string header()
+ {
+ return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n ";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << pressureTimestamp << "," << pressure << ","
+ << temperatureTimestamp << "," << temperature << "\n";
+ }
+};
+
+struct SensorsCalibrationParameter
+{
+ uint64_t timestamp;
+ float referencePressure;
+ float offsetStatic;
+ float offsetDynamic;
+
+ SensorsCalibrationParameter(uint64_t timestamp, float referencePressure,
+ float offsetStatic, float offsetDynamic)
+ : timestamp(timestamp), referencePressure(referencePressure),
+ offsetStatic(offsetStatic), offsetDynamic(offsetDynamic)
+ {
+ }
+
+ SensorsCalibrationParameter() : SensorsCalibrationParameter(0, 0, 0, 0) {}
+
+ static std::string header()
+ {
+ return "timestamp,referencePressure,offsetStatic,offsetDynamic";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << referencePressure << "," << offsetStatic
+ << "," << offsetDynamic << "\n";
+ }
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9adc0129ea5630df9a125cab42f63d989be24a33
--- /dev/null
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -0,0 +1,687 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Buses.h>
+#include <Payload/Configs/SensorsConfig.h>
+#include <common/ReferenceConfig.h>
+#include <interfaces-impl/hwmapping.h>
+
+using namespace Boardcore;
+using namespace std;
+using namespace Payload::SensorsConfig;
+namespace Payload
+{
+LPS22DFData Sensors::getLPS22DFLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{};
+}
+LPS28DFWData Sensors::getLPS28DFW_1LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{};
+}
+LPS28DFWData Sensors::getLPS28DFW_2LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{};
+}
+H3LIS331DLData Sensors::getH3LIS331DLLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return h3lis331dl != nullptr ? h3lis331dl->getLastSample()
+ : H3LIS331DLData{};
+}
+LIS2MDLData Sensors::getLIS2MDLLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{};
+}
+UBXGPSData Sensors::getGPSLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{};
+}
+LSM6DSRXData Sensors::getLSM6DSRXLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
+}
+ADS131M08Data Sensors::getADS131M08LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
+}
+
+HSCMRNN015PAData Sensors::getStaticPressureLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return staticPressure != nullptr ? staticPressure->getLastSample()
+ : HSCMRNN015PAData{};
+}
+
+SSCMRNN030PAData Sensors::getDynamicPressureLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return dynamicPressure != nullptr ? dynamicPressure->getLastSample()
+ : SSCMRNN030PAData{};
+}
+PitotData Sensors::getPitotLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return pitot != nullptr ? pitot->getLastSample() : PitotData{};
+}
+
+// Processed Getters
+BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
+{
+ // Do not need to pause the kernel, the last sample getter is already
+ // protected
+ ADS131M08Data sample = getADS131M08LastSample();
+ BatteryVoltageSensorData data;
+
+ // Populate the data
+ data.voltageTimestamp = sample.timestamp;
+ data.channelId = static_cast<uint8_t>(BATTERY_VOLTAGE_CHANNEL);
+ data.voltage = sample.getVoltage(BATTERY_VOLTAGE_CHANNEL).voltage;
+ data.batVoltage = sample.getVoltage(BATTERY_VOLTAGE_CHANNEL).voltage *
+ BATTERY_VOLTAGE_CONVERSION_FACTOR;
+ return data;
+}
+
+BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample()
+{
+ // Do not need to pause the kernel, the last sample getter is already
+ // protected
+ ADS131M08Data sample = getADS131M08LastSample();
+ BatteryVoltageSensorData data;
+
+ // Populate the data
+ data.voltageTimestamp = sample.timestamp;
+ data.channelId = static_cast<uint8_t>(CAM_BATTERY_VOLTAGE_CHANNEL);
+ data.voltage = sample.getVoltage(CAM_BATTERY_VOLTAGE_CHANNEL).voltage;
+ data.batVoltage = sample.getVoltage(CAM_BATTERY_VOLTAGE_CHANNEL).voltage *
+ BATTERY_VOLTAGE_CONVERSION_FACTOR;
+ return data;
+}
+
+CurrentData Sensors::getCurrentLastSample()
+{
+ // Do not need to pause the kernel, the last sample getter is already
+ // protected
+ ADS131M08Data sample = getADS131M08LastSample();
+ CurrentData data;
+
+ // Populate the data
+ data.currentTimestamp = sample.timestamp;
+ data.current =
+ (sample.getVoltage(CURRENT_CHANNEL).voltage - CURRENT_OFFSET) *
+ CURRENT_CONVERSION_FACTOR;
+ return data;
+}
+
+RotatedIMUData Sensors::getIMULastSample()
+{
+ miosix::PauseKernelLock lock;
+ return imu != nullptr ? imu->getLastSample() : RotatedIMUData{};
+}
+
+MagnetometerData Sensors::getCalibratedMagnetometerLastSample()
+{
+ // Do not need to pause the kernel, the last sample getter is already
+ // protected
+ MagnetometerData lastSample = getLIS2MDLLastSample();
+ MagnetometerData result;
+
+ // Correct the result and copy the timestamp
+ {
+ miosix::Lock<FastMutex> l(calibrationMutex);
+ result =
+ static_cast<MagnetometerData>(magCalibration.correct(lastSample));
+ }
+
+ result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp;
+ return result;
+}
+
+Sensors::Sensors(TaskScheduler* sched) : scheduler(sched), sensorsCounter(0) {}
+
+bool Sensors::start()
+{
+ // Read the magnetometer calibration from predefined file
+ magCalibration.fromFile("magCalibration.csv");
+ // Init all the sensors
+ lps22dfInit();
+ lps28dfw_1Init();
+ lps28dfw_2Init();
+ h3lis331dlInit();
+ lis2mdlInit();
+ ubxgpsInit();
+ lsm6dsrxInit();
+ ads131m08Init();
+ staticPressureInit();
+ dynamicPressureInit();
+ pitotInit();
+ imuInit();
+
+ // Add the magnetometer calibration to the scheduler
+ size_t result = scheduler->addTask(
+ [&]()
+ {
+ // Gather the last sample data
+ MagnetometerData lastSample = getLIS2MDLLastSample();
+
+ // Feed the data to the calibrator inside a protected area.
+ // Contention is not high and the use of a mutex is suitable to
+ // avoid pausing the kernel for this calibration operation
+ {
+ miosix::Lock<FastMutex> l(calibrationMutex);
+ magCalibrator.feed(lastSample);
+ }
+ },
+ MAG_CALIBRATION_PERIOD);
+
+ // Create sensor manager with populated map and configured scheduler
+ manager = new SensorManager(sensorMap, scheduler);
+ return manager->start() && result != 0;
+}
+
+void Sensors::stop() { manager->stop(); }
+
+bool Sensors::isStarted()
+{
+ return manager->areAllSensorsInitialized() && scheduler->isRunning();
+}
+
+void Sensors::calibrate()
+{
+ // Create the stats to calibrate the barometers
+ Stats lps28dfw1Stats;
+ Stats lps28dfw2Stats;
+ Stats staticPressureStats;
+ Stats dynamicPressureStats;
+
+ // Add N samples to the stats
+ for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++)
+ {
+ lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure);
+ lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure);
+ staticPressureStats.add(getStaticPressureLastSample().pressure);
+ dynamicPressureStats.add(getDynamicPressureLastSample().pressure);
+
+ // Delay for the expected period
+ miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD);
+ }
+
+ // Compute the difference between the mean value from LPS28DFW
+ float reference =
+ (lps28dfw1Stats.getStats().mean + lps28dfw2Stats.getStats().mean) / 2.f;
+
+ staticPressure->updateOffset(staticPressureStats.getStats().mean -
+ reference);
+ dynamicPressure->updateOffset(dynamicPressureStats.getStats().mean -
+ reference);
+
+ // Log the offsets
+ SensorsCalibrationParameter cal{};
+ cal.timestamp = TimestampTimer::getTimestamp();
+ cal.offsetStatic = staticPressureStats.getStats().mean - reference;
+ cal.offsetDynamic = dynamicPressureStats.getStats().mean - reference;
+ cal.referencePressure = reference;
+
+ Logger::getInstance().log(cal);
+}
+
+bool Sensors::writeMagCalibration()
+{
+ // Compute the calibration result in protected area
+ {
+ miosix::Lock<FastMutex> l(calibrationMutex);
+ SixParametersCorrector cal = magCalibrator.computeResult();
+
+ // Check result validity
+ if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) &&
+ !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) &&
+ !isnan(cal.getA()[1]) && !isnan(cal.getA()[2]))
+ {
+ magCalibration = cal;
+
+ // Save the calibration to the calibration file
+ return magCalibration.toFile("magCalibration.csv");
+ }
+ }
+ return false;
+}
+
+void Sensors::lps22dfInit()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Get the correct SPI configuration
+ SPIBusConfig config = LPS22DF::getDefaultSPIConfig();
+ config.clockDivider = SPI::ClockDivider::DIV_16;
+
+ // Configure the device
+ LPS22DF::Config sensorConfig;
+ sensorConfig.avg = LPS22DF_AVG;
+ sensorConfig.odr = LPS22DF_ODR;
+
+ // Create sensor instance with configured parameters
+ lps22df = new LPS22DF(modules.get<Buses>()->spi3,
+ miosix::sensors::LPS22DF::cs::getPin(), config,
+ sensorConfig);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LPS22DF", LPS22DF_PERIOD,
+ bind(&Sensors::lps22dfCallback, this));
+ sensorMap.emplace(make_pair(lps22df, info));
+
+ // used for the sensor state
+ auto lps22Status =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(lps22df); });
+ sensorsInit[sensorsCounter] = lps22Status;
+ sensorsCounter++;
+}
+void Sensors::lps28dfw_1Init()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Configure the sensor
+ LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG,
+ LPS28DFW_ODR, false};
+
+ // Create sensor instance with configured parameters
+ lps28dfw_1 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LPS28DFW_1", LPS28DFW_PERIOD,
+ bind(&Sensors::lps28dfw_1Callback, this));
+ sensorMap.emplace(make_pair(lps28dfw_1, info));
+
+ // used for the sensor state
+ auto lps28_1Status =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(lps28dfw_1); });
+ sensorsInit[sensorsCounter] = lps28_1Status;
+ sensorsCounter++;
+}
+void Sensors::lps28dfw_2Init()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Configure the sensor
+ LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG,
+ LPS28DFW_ODR, false};
+
+ // Create sensor instance with configured parameters
+ lps28dfw_2 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LPS28DFW_2", LPS28DFW_PERIOD,
+ bind(&Sensors::lps28dfw_2Callback, this));
+ sensorMap.emplace(make_pair(lps28dfw_2, info));
+
+ // used for the sensor state
+ auto lps28_2Status =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(lps28dfw_2); });
+ sensorsInit[sensorsCounter] = lps28_2Status;
+ sensorsCounter++;
+}
+void Sensors::h3lis331dlInit()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Get the correct SPI configuration
+ SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig();
+ config.clockDivider = SPI::ClockDivider::DIV_16;
+
+ // Create sensor instance with configured parameters
+ h3lis331dl = new H3LIS331DL(
+ modules.get<Buses>()->spi3, miosix::sensors::H3LIS331DL::cs::getPin(),
+ config, H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("H3LIS331DL", H3LIS331DL_PERIOD,
+ bind(&Sensors::h3lis331dlCallback, this));
+ sensorMap.emplace(make_pair(h3lis331dl, info));
+
+ // used for the sensor state
+ auto h3lis331Status =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(h3lis331dl); });
+ sensorsInit[sensorsCounter] = h3lis331Status;
+ sensorsCounter++;
+}
+void Sensors::lis2mdlInit()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Get the correct SPI configuration
+ SPIBusConfig config = LIS2MDL::getDefaultSPIConfig();
+ config.clockDivider = SPI::ClockDivider::DIV_16;
+
+ // Configure the sensor
+ LIS2MDL::Config sensorConfig;
+ sensorConfig.deviceMode = LIS2MDL_OPERATIVE_MODE;
+ sensorConfig.odr = LIS2MDL_ODR;
+ sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER;
+
+ // Create sensor instance with configured parameters
+ lis2mdl = new LIS2MDL(modules.get<Buses>()->spi3,
+ miosix::sensors::LIS2MDL::cs::getPin(), config,
+ sensorConfig);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LIS2MDL", LIS2MDL_PERIOD,
+ bind(&Sensors::lis2mdlCallback, this));
+ sensorMap.emplace(make_pair(lis2mdl, info));
+
+ // used for the sensor state
+ auto lis2mdlStatus =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(lis2mdl); });
+ sensorsInit[sensorsCounter] = lis2mdlStatus;
+ sensorsCounter++;
+}
+
+void Sensors::ubxgpsInit()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Get the correct SPI configuration
+ SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig();
+ config.clockDivider = SPI::ClockDivider::DIV_64;
+
+ // Create sensor instance with configured parameters
+ ubxgps = new UBXGPSSpi(modules.get<Buses>()->spi4,
+ miosix::sensors::GPS::cs::getPin(), config, 5);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("UBXGPS", UBXGPS_PERIOD,
+ bind(&Sensors::ubxgpsCallback, this));
+ sensorMap.emplace(make_pair(ubxgps, info));
+
+ // used for the sensor state
+ auto ubxStatus =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(ubxgps); });
+ sensorsInit[sensorsCounter] = ubxStatus;
+ sensorsCounter++;
+}
+
+void Sensors::lsm6dsrxInit()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Configure the SPI
+ SPIBusConfig config;
+ config.clockDivider = SPI::ClockDivider::DIV_32;
+ config.mode = SPI::Mode::MODE_0;
+
+ // Configure the sensor
+ LSM6DSRXConfig sensorConfig;
+ sensorConfig.bdu = LSM6DSRX_BDU;
+
+ // Accelerometer
+ sensorConfig.fsAcc = LSM6DSRX_ACC_FS;
+ sensorConfig.odrAcc = LSM6DSRX_ACC_ODR;
+ sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE;
+
+ // Gyroscope
+ sensorConfig.fsGyr = LSM6DSRX_GYR_FS;
+ sensorConfig.odrGyr = LSM6DSRX_GYR_ODR;
+ sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE;
+
+ // Fifo
+ sensorConfig.fifoMode = LSM6DSRX_FIFO_MODE;
+ sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION;
+ sensorConfig.fifoTemperatureBdr = LSM6DSRX_FIFO_TEMPERATURE_BDR;
+
+ // Create sensor instance with configured parameters
+ lsm6dsrx = new LSM6DSRX(modules.get<Buses>()->spi1,
+ miosix::sensors::LSM6DSRX::cs::getPin(), config,
+ sensorConfig);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD,
+ bind(&Sensors::lsm6dsrxCallback, this));
+ sensorMap.emplace(make_pair(lsm6dsrx, info));
+
+ // used for the sensor state
+ auto lsm6dsrxStatus =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(lsm6dsrx); });
+ sensorsInit[sensorsCounter] = lsm6dsrxStatus;
+ sensorsCounter++;
+}
+
+void Sensors::ads131m08Init()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Configure the SPI
+ SPIBusConfig config;
+ config.clockDivider = SPI::ClockDivider::DIV_32;
+
+ // Configure the device
+ ADS131M08::Config sensorConfig;
+ sensorConfig.oversamplingRatio = ADS131M08_OVERSAMPLING_RATIO;
+ sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE;
+
+ // Create the sensor instance with configured parameters
+ ads131m08 = new ADS131M08(modules.get<Buses>()->spi4,
+ miosix::sensors::ADS131::cs::getPin(), config,
+ sensorConfig);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("ADS131M08", ADS131M08_PERIOD,
+ bind(&Sensors::ads131m08Callback, this));
+ sensorMap.emplace(make_pair(ads131m08, info));
+
+ // used for the sensor state
+ auto ads131m08Status =
+ ([&]() -> SensorInfo { return manager->getSensorInfo(ads131m08); });
+ sensorsInit[sensorsCounter] = ads131m08Status;
+ sensorsCounter++;
+}
+
+void Sensors::staticPressureInit()
+{
+ // create lambda function to read the voltage
+ auto readVoltage = (
+ [&]() -> ADCData
+ {
+ ADS131M08Data sample = ads131m08->getLastSample();
+ return sample.getVoltage(
+ STATIC_PRESSURE_CHANNEL);
+ });
+
+ staticPressure = new HSCMRNN015PA(readVoltage, ADC_VOLTAGE_RANGE);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("StaticPressure", ADS131M08_PERIOD,
+ bind(&Sensors::staticPressureCallback, this));
+ sensorMap.emplace(make_pair(staticPressure, info));
+}
+
+void Sensors::dynamicPressureInit()
+{
+ // create lambda function to read the voltage
+ auto readVoltage = (
+ [&]() -> ADCData
+ {
+ auto sample = ads131m08->getLastSample();
+ return sample.getVoltage(
+ DYNAMIC_PRESSURE_CHANNEL);
+ });
+
+ dynamicPressure = new SSCMRNN030PA(readVoltage, ADC_VOLTAGE_RANGE);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("TotalPressure", ADS131M08_PERIOD,
+ bind(&Sensors::dynamicPressureCallback, this));
+ sensorMap.emplace(make_pair(dynamicPressure, info));
+}
+
+void Sensors::pitotInit()
+{
+ // create lambda function to read the pressure
+ function<float()> getDynamicPressure(
+ [&]() { return dynamicPressure->getLastSample().pressure; });
+ function<float()> getStaticPressure(
+ [&]() { return staticPressure->getLastSample().pressure; });
+
+ pitot = new Pitot(getDynamicPressure, getStaticPressure);
+ pitot->setReferenceValues(Common::ReferenceConfig::defaultReferenceValues);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("Pitot", ADS131M08_PERIOD,
+ bind(&Sensors::pitotCallback, this));
+ sensorMap.emplace(make_pair(pitot, info));
+}
+
+void Sensors::pitotSetReferenceAltitude(float altitude)
+{
+ // Need to pause the kernel because the only invocation comes from the radio
+ // which is a separate thread
+ miosix::PauseKernelLock l;
+
+ ReferenceValues reference = pitot->getReferenceValues();
+ reference.refAltitude = altitude;
+ pitot->setReferenceValues(reference);
+}
+
+void Sensors::pitotSetReferenceTemperature(float temperature)
+{
+ // Need to pause the kernel because the only invocation comes from the radio
+ // which is a separate thread
+ miosix::PauseKernelLock l;
+
+ ReferenceValues reference = pitot->getReferenceValues();
+ reference.refTemperature = temperature + 273.15f;
+ pitot->setReferenceValues(reference);
+}
+
+std::array<SensorInfo, 8> Sensors::getSensorInfo()
+{
+ std::array<SensorInfo, 8> sensorState;
+ for (size_t i = 0; i < sensorsInit.size(); i++)
+ {
+ sensorState[i] = sensorsInit[i]();
+ }
+ return sensorState;
+}
+
+void Sensors::imuInit()
+{
+ // Register the IMU as the fake sensor, passing as parameters the
+ // methods to retrieve real data. The sensor is not synchronized, but
+ // the sampling thread is always the same.
+ imu = new RotatedIMU(
+ bind(&LSM6DSRX::getLastSample, lsm6dsrx),
+ bind(&Sensors::getCalibratedMagnetometerLastSample, this),
+ bind(&LSM6DSRX::getLastSample, lsm6dsrx));
+
+ // Invert the Y axis on the magnetometer
+ Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}};
+ imu->addMagTransformation(m);
+
+ // Emplace the sensor inside the map (TODO CHANGE PERIOD INTO NON MAGIC)
+ SensorInfo info("RotatedIMU", IMU_PERIOD,
+ bind(&Sensors::imuCallback, this));
+ sensorMap.emplace(make_pair(imu, info));
+}
+
+void Sensors::lps22dfCallback()
+{
+ LPS22DFData lastSample = lps22df->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::lps28dfw_1Callback()
+{
+ LPS28DFW_1Data lastSample =
+ static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample());
+
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::lps28dfw_2Callback()
+{
+ LPS28DFW_2Data lastSample =
+ static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample());
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::h3lis331dlCallback()
+{
+ H3LIS331DLData lastSample = h3lis331dl->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::lis2mdlCallback()
+{
+ LIS2MDLData lastSample = lis2mdl->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::ubxgpsCallback()
+{
+ UBXGPSData lastSample = ubxgps->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::lsm6dsrxCallback()
+{
+ LSM6DSRXData lastSample = lsm6dsrx->getLastSample();
+
+ auto& fifo = lsm6dsrx->getLastFifo();
+ uint16_t fifoSize = lsm6dsrx->getLastFifoSize();
+
+ // For every instance inside the fifo log the sample
+ for (uint16_t i = 0; i < fifoSize; i++)
+ {
+ Logger::getInstance().log(fifo.at(i));
+ }
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::ads131m08Callback()
+{
+ ADS131M08Data lastSample = ads131m08->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::staticPressureCallback()
+{
+ HSCMRNN015PAData lastSample = staticPressure->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::dynamicPressureCallback()
+{
+ SSCMRNN030PAData lastSample = dynamicPressure->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+void Sensors::pitotCallback()
+{
+ PitotData lastSample = pitot->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+void Sensors::imuCallback()
+{
+ RotatedIMUData lastSample = imu->getLastSample();
+ Logger::getInstance().log(lastSample);
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..faeee0b846d2c6b90b85ad3772128a35ce119fe8
--- /dev/null
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -0,0 +1,180 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 <Payload/Configs/SensorsConfig.h>
+#include <Payload/Sensors/RotatedIMU/RotatedIMU.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/LPS28DFW/LPS28DFW.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/SensorData.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+#include <sensors/analog/BatteryVoltageSensorData.h>
+#include <sensors/analog/Pitot/Pitot.h>
+#include <sensors/analog/Pitot/PitotData.h>
+#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h>
+#include <sensors/analog/pressure/honeywell/SSCMRNN030PA.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "SensorData.h"
+
+namespace Payload
+{
+
+class Sensors : public Boardcore::Module
+{
+public:
+ explicit Sensors(Boardcore::TaskScheduler* sched);
+
+ [[nodiscard]] bool start();
+
+ /**
+ * @brief Stops the sensor manager
+ * @warning Stops the passed scheduler
+ */
+ void stop();
+
+ /**
+ * @brief Returns if all the sensors are started successfully
+ */
+ bool isStarted();
+
+ /**
+ * @brief Calibrates the sensors with an offset
+ */
+ void calibrate();
+
+ /**
+ * @brief Takes the result of the live magnetometer calibration and applies
+ * it to the current calibration + writes it in the csv file
+ *
+ * @return true if the write was successful
+ */
+ bool writeMagCalibration();
+
+ // Sensor getters
+ Boardcore::LPS22DFData getLPS22DFLastSample();
+ Boardcore::LPS28DFWData getLPS28DFW_1LastSample();
+ Boardcore::LPS28DFWData getLPS28DFW_2LastSample();
+ Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
+ Boardcore::LIS2MDLData getLIS2MDLLastSample();
+ Boardcore::UBXGPSData getGPSLastSample();
+ Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
+ Boardcore::ADS131M08Data getADS131M08LastSample();
+
+ // Processed getters
+ Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+ Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample();
+ Boardcore::CurrentData getCurrentLastSample();
+ RotatedIMUData getIMULastSample();
+ Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
+ Boardcore::HSCMRNN015PAData getStaticPressureLastSample();
+ Boardcore::SSCMRNN030PAData getDynamicPressureLastSample();
+ Boardcore::PitotData getPitotLastSample();
+
+ void pitotSetReferenceAltitude(float altitude);
+ void pitotSetReferenceTemperature(float temperature);
+
+ std::array<Boardcore::SensorInfo, 8> getSensorInfo();
+
+private:
+ // Init and callbacks methods
+ void lps22dfInit();
+ void lps22dfCallback();
+
+ void lps28dfw_1Init();
+ void lps28dfw_1Callback();
+
+ void lps28dfw_2Init();
+ void lps28dfw_2Callback();
+
+ void h3lis331dlInit();
+ void h3lis331dlCallback();
+
+ void lis2mdlInit();
+ void lis2mdlCallback();
+
+ void ubxgpsInit();
+ void ubxgpsCallback();
+
+ void lsm6dsrxInit();
+ void lsm6dsrxCallback();
+
+ void ads131m08Init();
+ void ads131m08Callback();
+
+ void staticPressureInit();
+ void staticPressureCallback();
+
+ void dynamicPressureInit();
+ void dynamicPressureCallback();
+
+ void pitotInit();
+ void pitotCallback();
+
+ void imuInit();
+ void imuCallback();
+
+ // Sensors instances
+ Boardcore::LPS22DF* lps22df = nullptr;
+ Boardcore::LPS28DFW* lps28dfw_1 = nullptr;
+ Boardcore::LPS28DFW* lps28dfw_2 = nullptr;
+ Boardcore::H3LIS331DL* h3lis331dl = nullptr;
+ Boardcore::LIS2MDL* lis2mdl = nullptr;
+ Boardcore::UBXGPSSpi* ubxgps = nullptr;
+ Boardcore::LSM6DSRX* lsm6dsrx = nullptr;
+ Boardcore::ADS131M08* ads131m08 = nullptr;
+
+ // Fake processed sensors
+ RotatedIMU* imu = nullptr;
+ Boardcore::HSCMRNN015PA* staticPressure = nullptr;
+ Boardcore::SSCMRNN030PA* dynamicPressure = nullptr;
+ Boardcore::Pitot* pitot = nullptr;
+
+ // Magnetometer live calibration
+ Boardcore::SoftAndHardIronCalibration magCalibrator;
+ Boardcore::SixParametersCorrector magCalibration;
+ miosix::FastMutex calibrationMutex;
+
+ // Sensor manager
+ Boardcore::SensorManager* manager = nullptr;
+ Boardcore::SensorManager::SensorMap_t sensorMap;
+ Boardcore::TaskScheduler* scheduler = nullptr;
+
+ uint8_t sensorsCounter;
+
+ // SD logger
+ Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance();
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
+
+ std::array<std::function<Boardcore::SensorInfo()>,
+ SensorsConfig::NUMBER_OF_SENSORS>
+ sensorsInit;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5d45e037da2874d25629acceae10a7d3b5a9190
--- /dev/null
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -0,0 +1,668 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli
+ *
+ * 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 <Payload/Actuators/Actuators.h>
+#include <Payload/AltitudeTrigger/AltitudeTrigger.h>
+#include <Payload/CanHandler/CanHandler.h>
+#include <Payload/Configs/FlightModeManagerConfig.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Common;
+
+namespace Payload
+{
+FlightModeManager::FlightModeManager()
+ : HSM(&FlightModeManager::state_on_ground)
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+ EventBroker::getInstance().subscribe(this, TOPIC_FMM);
+ EventBroker::getInstance().subscribe(this, TOPIC_TMTC);
+ EventBroker::getInstance().subscribe(this, TOPIC_CAN);
+ EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+}
+
+FlightModeManager::~FlightModeManager()
+{
+ EventBroker::getInstance().unsubscribe(this);
+}
+
+FlightModeManagerStatus FlightModeManager::getStatus()
+{
+ PauseKernelLock lock;
+ return status;
+}
+
+State FlightModeManager::state_on_ground(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::state_init);
+ }
+ case TMTC_START_LOGGING:
+ {
+ Logger::getInstance().start();
+ return HANDLED;
+ }
+ case TMTC_STOP_LOGGING:
+ {
+ Logger::getInstance().stop();
+ return HANDLED;
+ }
+ case TMTC_RESET_BOARD:
+ {
+ reboot();
+ return HANDLED;
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_init(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::INIT);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_on_ground);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case FMM_INIT_OK:
+ {
+ return transition(&FlightModeManager::state_init_done);
+ }
+ case FMM_INIT_ERROR:
+ {
+ return transition(&FlightModeManager::state_init_error);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_init_error(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::INIT_ERROR);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_on_ground);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case CAN_FORCE_INIT:
+ case TMTC_FORCE_INIT:
+ {
+ if (event != CAN_FORCE_INIT)
+ {
+ ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+ CanConfig::EventId::FORCE_INIT);
+ }
+ return transition(&FlightModeManager::state_init_done);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_init_done(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ // We turn off the led if we are coming from init_error
+ logStatus(FlightModeManagerState::INIT_DONE);
+ EventBroker::getInstance().post(FMM_CALIBRATE, TOPIC_FMM);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_on_ground);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case FMM_CALIBRATE:
+ {
+ return transition(&FlightModeManager::state_sensors_calibration);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_sensors_calibration(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::SENSORS_CALIBRATION);
+ ModuleManager::getInstance().get<Sensors>()->calibrate();
+ EventBroker::getInstance().post(FMM_ALGOS_CALIBRATE, TOPIC_FMM);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_on_ground);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case FMM_ALGOS_CALIBRATE:
+ {
+ return transition(&FlightModeManager::state_algos_calibration);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_algos_calibration(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::ALGOS_CALIBRATION);
+ // EventBroker::getInstance().post(FMM_ALGOS_CALIBRATE, TOPIC_FMM);
+ EventBroker::getInstance().post(NAS_CALIBRATE, TOPIC_NAS);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_on_ground);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case NAS_READY:
+ {
+ EventBroker::getInstance().post(FMM_READY, TOPIC_FMM);
+ return transition(&FlightModeManager::state_disarmed);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_disarmed(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::DISARMED);
+ // Stop eventual logging
+ Logger::getInstance().stop();
+ ModuleManager::getInstance().get<Actuators>()->buzzerOff();
+ ModuleManager::getInstance().get<Actuators>()->camOff();
+ EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_on_ground);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case CAN_ARM:
+ case TMTC_ARM:
+ {
+ if (event != CAN_ARM)
+ {
+ ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+ CanConfig::EventId::ARM);
+ }
+ return transition(&FlightModeManager::state_armed);
+ }
+ case CAN_ENTER_TEST_MODE:
+ case TMTC_ENTER_TEST_MODE:
+ {
+ if (event != CAN_ENTER_TEST_MODE)
+ {
+ ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+ CanConfig::EventId::ENTER_TEST_MODE);
+ }
+ return transition(&FlightModeManager::state_test_mode);
+ }
+ case CAN_CALIBRATE:
+ case TMTC_CALIBRATE:
+ {
+ if (event != CAN_CALIBRATE)
+ {
+ ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+ CanConfig::EventId::CALIBRATE);
+ }
+ return transition(&FlightModeManager::state_sensors_calibration);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_test_mode(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ Logger::getInstance().start();
+ logStatus(FlightModeManagerState::TEST_MODE);
+
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ ModuleManager::getInstance().get<Actuators>()->camOff();
+ Logger::getInstance().stop();
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_on_ground);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case TMTC_START_RECORDING:
+ {
+ ModuleManager::getInstance().get<Actuators>()->camOn();
+ return HANDLED;
+ }
+ case TMTC_STOP_RECORDING:
+ {
+ ModuleManager::getInstance().get<Actuators>()->camOff();
+ return HANDLED;
+ }
+ case TMTC_RESET_BOARD:
+ {
+ Logger::getInstance().stop();
+ reboot();
+ return HANDLED;
+ }
+ case CAN_EXIT_TEST_MODE:
+ case TMTC_EXIT_TEST_MODE:
+ {
+ Logger::getInstance().stop();
+ if (event != CAN_EXIT_TEST_MODE)
+ {
+ ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+ CanConfig::EventId::EXIT_TEST_MODE);
+ }
+ return transition(&FlightModeManager::state_disarmed);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_armed(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::ARMED);
+ Logger::getInstance().start();
+
+ // Starts signaling devices and camera
+ ModuleManager::getInstance().get<Actuators>()->buzzerArmed();
+ ModuleManager::getInstance().get<Actuators>()->camOn();
+ // Post event
+ EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ Logger::getInstance().stop();
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case CAN_DISARM:
+ case TMTC_DISARM:
+ {
+ EventBroker::getInstance().post(NAS_FORCE_STOP, TOPIC_NAS);
+ if (event != CAN_DISARM)
+ {
+ ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+ CanConfig::EventId::DISARM);
+ }
+ return transition(&FlightModeManager::state_disarmed);
+ }
+ case CAN_LIFTOFF:
+ case TMTC_FORCE_LAUNCH:
+ {
+ if (event != CAN_LIFTOFF)
+ {
+ ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+ CanConfig::EventId::LIFTOFF);
+ }
+ return transition(&FlightModeManager::state_flying);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_flying(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ EventBroker::getInstance().postDelayed(
+ FLIGHT_MISSION_TIMEOUT, TOPIC_FLIGHT, MISSION_TIMEOUT);
+ EventBroker::getInstance().postDelayed(
+ FLIGHT_NC_DETACHED, TOPIC_FLIGHT, APOGEE_TIMEOUT);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+ case EV_INIT:
+ {
+ return transition(&FlightModeManager::state_ascending);
+ }
+ case TMTC_FORCE_LANDING:
+ case FLIGHT_MISSION_TIMEOUT:
+ {
+ return transition(&FlightModeManager::state_landed);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_ascending(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::ASCENDING);
+ EventBroker::getInstance().post(FLIGHT_LIFTOFF, TOPIC_FLIGHT);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_flying);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case CAN_APOGEE_DETECTED:
+ case FLIGHT_NC_DETACHED:
+ case TMTC_FORCE_EXPULSION:
+ {
+ return transition(&FlightModeManager::state_drogue_descent);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_drogue_descent(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::DROGUE_DESCENT);
+
+ ModuleManager::getInstance().get<AltitudeTrigger>()->enable();
+
+ // ModuleManager::getInstance()
+ // .get<VerticalVelocityTrigger>()
+ // ->enable();
+
+ EventBroker::getInstance().post(FLIGHT_DROGUE_DESCENT,
+ TOPIC_FLIGHT);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_flying);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case ALTITUDE_TRIGGER_ALTITUDE_REACHED:
+ case TMTC_FORCE_DEPLOYMENT:
+ {
+ ModuleManager::getInstance().get<AltitudeTrigger>()->disable();
+ ModuleManager::getInstance()
+ .get<VerticalVelocityTrigger>()
+ ->disable();
+ return transition(&FlightModeManager::state_wing_descent);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_wing_descent(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ EventBroker::getInstance().post(FLIGHT_WING_DESCENT, TOPIC_FLIGHT);
+
+ logStatus(FlightModeManagerState::WING_DESCENT);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_flying);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State FlightModeManager::state_landed(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(FlightModeManagerState::LANDED);
+
+ // Turns off signaling devices
+ ModuleManager::getInstance().get<Actuators>()->buzzerLanded();
+ ModuleManager::getInstance().get<Actuators>()->camOff();
+ ModuleManager::getInstance().get<Actuators>()->disableServo(
+ PARAFOIL_LEFT_SERVO);
+ ModuleManager::getInstance().get<Actuators>()->disableServo(
+ PARAFOIL_RIGHT_SERVO);
+ // Sends events
+ EventBroker::getInstance().post(FLIGHT_LANDING_DETECTED,
+ TOPIC_FLIGHT);
+ EventBroker::getInstance().postDelayed(FMM_STOP_LOGGING, TOPIC_FMM,
+ LOGGING_DELAY);
+
+ 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();
+ reboot();
+ return HANDLED;
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+void FlightModeManager::logStatus(FlightModeManagerState state)
+{
+ status.timestamp = TimestampTimer::getTimestamp();
+ status.state = state;
+
+ Logger::getInstance().log(status);
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..a801add576b79bc001e95b3710e36163e25d0203
--- /dev/null
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
@@ -0,0 +1,126 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli
+ *
+ * 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 <events/HSM.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "FlightModeManagerData.h"
+
+namespace Payload
+{
+class FlightModeManager : public Boardcore::HSM<FlightModeManager>,
+ public Boardcore::Module
+{
+public:
+ FlightModeManager();
+ ~FlightModeManager();
+
+ bool startModule() { return start(); }
+ FlightModeManagerStatus getStatus();
+
+ /**
+ * @brief Super state for when the payload is on ground.
+ */
+ // Super state for when the payload is on ground.
+ Boardcore::State state_on_ground(const Boardcore::Event& event);
+
+ /**
+ * @brief Super state for when the payload is on ground.
+ */
+ Boardcore::State state_init(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the init has failed
+ */
+ Boardcore::State state_init_error(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the init is done and a calibration event is
+ * thrown
+ */
+ Boardcore::State state_init_done(const Boardcore::Event& event);
+
+ /**
+ * @brief Calibration of all sensors.
+ */
+ Boardcore::State state_sensors_calibration(const Boardcore::Event& event);
+
+ /**
+ * @brief Calibration of all algorithms.
+ */
+ Boardcore::State state_algos_calibration(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the electronics is ready to be armed
+ */
+ Boardcore::State state_disarmed(const Boardcore::Event& event);
+
+ /**
+ * @brief The rocket will accept specific telecommands otherwise considered
+ * risky.
+ */
+ Boardcore::State state_test_mode(const Boardcore::Event& event);
+
+ /**
+ * @brief Super state in which the algorithms start to run (NAS) and the
+ * electronics is ready to fly
+ */
+ Boardcore::State state_armed(const Boardcore::Event& event);
+
+ /**
+ * @brief Super state for when the payload is in the air.
+ */
+ Boardcore::State state_flying(const Boardcore::Event& event);
+
+ /**
+ * @brief Ascending phase of the trajectory.
+ */
+ Boardcore::State state_ascending(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the apogee/expulsion has occurred
+ */
+ Boardcore::State state_drogue_descent(const Boardcore::Event& event);
+
+ /**
+ * @brief State in which the parafoil wing is opened and starts guiding
+ * itself
+ */
+ Boardcore::State state_wing_descent(const Boardcore::Event& event);
+
+ /**
+ * @brief The rocket ended the flight and closes the log.
+ */
+ Boardcore::State state_landed(const Boardcore::Event& event);
+
+private:
+ void logStatus(FlightModeManagerState state);
+
+ FlightModeManagerStatus status;
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("fmm");
+};
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..6c40d9540195ab6ba158a9b6adc34a8d61864c6d
--- /dev/null
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h
@@ -0,0 +1,63 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Payload
+{
+
+enum class FlightModeManagerState : uint8_t
+{
+ INVALID = 0,
+ INIT,
+ INIT_ERROR,
+ INIT_DONE,
+ SENSORS_CALIBRATION,
+ ALGOS_CALIBRATION,
+ DISARMED,
+ TEST_MODE,
+ ARMED,
+ ASCENDING,
+ DROGUE_DESCENT,
+ WING_DESCENT,
+ LANDED,
+};
+
+struct FlightModeManagerStatus
+{
+ uint64_t timestamp = 0;
+ FlightModeManagerState state = FlightModeManagerState::INVALID;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)state << "\n";
+ }
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..41503d305065a1d9cc25ddae3b5507ecb69b2457
--- /dev/null
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -0,0 +1,370 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Configs/NASConfig.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/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 Eigen;
+using namespace std;
+using namespace Common;
+namespace Payload
+{
+NASController::NASController(TaskScheduler* sched)
+ : FSM(&NASController::state_idle), nas(NASConfig::config), scheduler(sched)
+{
+ // Subscribe the class to the topics
+ EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+
+ // 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(6) = q(0);
+ x(7) = q(1);
+ x(8) = q(2);
+ x(9) = q(3);
+
+ // Set the NAS x matrix
+ nas.setX(x);
+
+ // Set the referenced values for the correct place on earth
+ nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
+}
+
+bool NASController::start()
+{
+ // Add the task to the scheduler
+ size_t result = scheduler->addTask(bind(&NASController::update, this),
+ NASConfig::UPDATE_PERIOD,
+ TaskScheduler::Policy::RECOVER);
+
+ return ActiveObject::start() && result != 0;
+}
+
+void NASController::update()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Update the NAS state only if the FSM is active
+ if (this->testState(&NASController::state_active))
+ {
+ // Get the IMU data
+ RotatedIMUData imuData = modules.get<Sensors>()->getIMULastSample();
+ UBXGPSData gpsData = modules.get<Sensors>()->getGPSLastSample();
+
+ HSCMRNN015PAData baroData =
+ modules.get<Sensors>()->getStaticPressureLastSample();
+
+ // NAS prediction
+ nas.predictGyro(imuData);
+ nas.predictAcc(imuData);
+
+ // NAS correction
+ nas.correctMag(imuData);
+ nas.correctGPS(gpsData);
+ nas.correctBaro(baroData.pressure);
+ // Correct with accelerometer if the acceleration is in specs
+ Vector3f acceleration = static_cast<AccelerometerData>(imuData);
+ float accelerationNorm = acceleration.norm();
+ if (accelerationValid)
+ {
+ nas.correctAcc(imuData);
+ }
+ if ((accelerationNorm <
+ (9.8 + (NASConfig::ACCELERATION_THRESHOLD) / 2) &&
+ accelerationNorm >
+ (9.8 - (NASConfig::ACCELERATION_THRESHOLD) / 2)))
+ {
+ if (!accelerationValid)
+ {
+ accSampleAfterSpike++;
+ }
+ }
+ else
+ {
+ accelerationValid = false;
+ accSampleAfterSpike = 0;
+ }
+ if (accSampleAfterSpike > NASConfig::ACCELERATION_THRESHOLD_SAMPLE)
+ {
+ accSampleAfterSpike = 0;
+ accelerationValid = true;
+ }
+
+ Logger::getInstance().log(nas.getState());
+ }
+}
+
+void NASController::calibrate()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ Vector3f acceleration = Vector3f::Zero();
+ Vector3f magneticField = Vector3f::Zero();
+ Stats pressure;
+
+ for (int i = 0; i < NASConfig::CALIBRATION_SAMPLES_COUNT; i++)
+ {
+ // IMU
+ LSM6DSRXData imuData = modules.get<Sensors>()->getLSM6DSRXLastSample();
+ acceleration += Vector3f(imuData.accelerationX, imuData.accelerationY,
+ imuData.accelerationZ);
+
+ // Magnetometer
+ LIS2MDLData magData = modules.get<Sensors>()->getLIS2MDLLastSample();
+ magneticField +=
+ Vector3f(magData.magneticFieldX, magData.magneticFieldY,
+ magData.magneticFieldZ);
+
+ // Static pressure barometer
+ HSCMRNN015PAData barometerData =
+ modules.get<Sensors>()->getStaticPressureLastSample();
+ pressure.add(barometerData.pressure);
+
+ miosix::Thread::sleep(NASConfig::CALIBRATION_SLEEP_TIME);
+ }
+
+ // Normalize the data
+ acceleration /= NASConfig::CALIBRATION_SAMPLES_COUNT;
+ magneticField /= NASConfig::CALIBRATION_SAMPLES_COUNT;
+ acceleration.normalize();
+ magneticField.normalize();
+
+ // Use the triad algorithm to estimate the initial orientation
+ StateInitializer state;
+ state.triad(acceleration, magneticField, ReferenceConfig::nedMag);
+
+ // Set the pressure reference using an already existing reference values
+ ReferenceValues reference = nas.getReferenceValues();
+ reference.refPressure = pressure.getStats().mean;
+
+ // Set reference altitude using barometric measure
+ reference.refAltitude = Aeroutils::relAltitude(pressure.getStats().mean);
+
+ // If in this moment the GPS has fix i use that position as starting
+ UBXGPSData gps = modules.get<Sensors>()->getGPSLastSample();
+ if (gps.fix != 0)
+ {
+ // We don't set the altitude with the GPS because of not precise
+ // measurements
+ reference.refLatitude = gps.latitude;
+ reference.refLongitude = gps.longitude;
+ }
+
+ // Update the algorithm reference values
+ {
+ miosix::PauseKernelLock lock;
+ nas.setX(state.getInitX());
+ nas.setReferenceValues(reference);
+ }
+
+ EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
+}
+
+void NASController::setCoordinates(Vector2f position)
+{
+ // Need to pause the kernel because the only invocation comes from the radio
+ // which is a separate thread
+ miosix::PauseKernelLock l;
+
+ ReferenceValues reference = nas.getReferenceValues();
+ reference.refLatitude = position[0];
+ reference.refLongitude = position[1];
+ nas.setReferenceValues(reference);
+}
+
+void NASController::setOrientation(float yaw, float pitch, float roll)
+{
+ // Need to pause the kernel because the only invocation comes from the radio
+ // which is a separate thread
+ miosix::PauseKernelLock l;
+
+ Matrix<float, 13, 1> x = nas.getX();
+
+ // Set quaternions
+ Vector4f q = SkyQuaternion::eul2quat({yaw, pitch, roll});
+ x(6) = q(0);
+ x(7) = q(1);
+ x(8) = q(2);
+ x(9) = q(3);
+
+ nas.setX(x);
+}
+
+void NASController::setReferenceAltitude(float altitude)
+{
+ // Need to pause the kernel because the only invocation comes from the radio
+ // which is a separate thread
+ miosix::PauseKernelLock l;
+
+ ReferenceValues reference = nas.getReferenceValues();
+ reference.refAltitude = altitude;
+ nas.setReferenceValues(reference);
+}
+
+void NASController::setReferenceTemperature(float temperature)
+{
+ // Need to pause the kernel because the only invocation comes from the radio
+ // which is a separate thread
+ miosix::PauseKernelLock l;
+
+ ReferenceValues reference = nas.getReferenceValues();
+
+ // The temperature is in degrees, converted in kelvin
+ reference.refTemperature = temperature + 273.15f;
+ nas.setReferenceValues(reference);
+}
+
+void NASController::setReferenceValues(const ReferenceValues reference)
+{
+ // Need to pause the kernel
+ miosix::PauseKernelLock l;
+ nas.setReferenceValues(reference);
+}
+
+NASControllerStatus NASController::getStatus()
+{
+ // Need to pause the kernel
+ miosix::PauseKernelLock l;
+ return status;
+}
+
+NASState NASController::getNasState()
+{
+ // Need to pause the kernel
+ miosix::PauseKernelLock l;
+ return nas.getState();
+}
+
+ReferenceValues NASController::getReferenceValues()
+{
+ // Need to pause the kernel
+ miosix::PauseKernelLock l;
+ return nas.getReferenceValues();
+}
+
+void NASController::state_idle(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return logStatus(NASControllerState::IDLE);
+ }
+ case NAS_CALIBRATE:
+ {
+ return transition(&NASController::state_calibrating);
+ }
+ }
+}
+
+void NASController::state_calibrating(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+
+ // Calibrate the NAS
+ calibrate();
+ return logStatus(NASControllerState::CALIBRATING);
+ }
+ case NAS_READY:
+ {
+ return transition(&NASController::state_ready);
+ }
+ }
+}
+
+void NASController::state_ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return logStatus(NASControllerState::READY);
+ }
+ case NAS_CALIBRATE:
+ {
+ return transition(&NASController::state_calibrating);
+ }
+ case NAS_FORCE_START:
+ case FLIGHT_ARMED:
+ {
+ return transition(&NASController::state_active);
+ }
+ }
+}
+
+void NASController::state_active(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return logStatus(NASControllerState::ACTIVE);
+ }
+ case FLIGHT_LANDING_DETECTED:
+ case FLIGHT_MISSION_TIMEOUT:
+ {
+ return transition(&NASController::state_end);
+ }
+ case NAS_FORCE_STOP:
+ case FLIGHT_DISARMED:
+ {
+ return transition(&NASController::state_ready);
+ }
+ }
+}
+
+void NASController::state_end(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return logStatus(NASControllerState::END);
+ }
+ }
+}
+
+void NASController::logStatus(NASControllerState state)
+{
+ // Update the current FSM state
+ status.timestamp = TimestampTimer::getTimestamp();
+ status.state = state;
+
+ // Log the status
+ Logger::getInstance().log(status);
+}
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.h b/src/boards/Payload/StateMachines/NASController/NASController.h
new file mode 100644
index 0000000000000000000000000000000000000000..73a256714815349a9b293bb41457f5a98212efb9
--- /dev/null
+++ b/src/boards/Payload/StateMachines/NASController/NASController.h
@@ -0,0 +1,99 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "NASControllerData.h"
+
+namespace Payload
+{
+class NASController : public Boardcore::FSM<NASController>,
+ public Boardcore::Module
+{
+public:
+ NASController(Boardcore::TaskScheduler* sched);
+
+ /**
+ * @brief Starts the FSM thread and adds the update function into the
+ * scheduler
+ */
+ bool start() override;
+
+ // NAS FSM called methods
+ void calibrate();
+
+ // NAS setters
+ void setCoordinates(Eigen::Vector2f position);
+ void setOrientation(float yaw, float pitch, float roll);
+ void setReferenceAltitude(float altitude);
+ void setReferenceTemperature(float temperature);
+ void setReferenceValues(const Boardcore::ReferenceValues reference);
+
+ // NAS Getters
+ NASControllerStatus getStatus();
+ Boardcore::NASState getNasState();
+ Boardcore::ReferenceValues getReferenceValues();
+
+ // FSM states
+ void state_idle(const Boardcore::Event& event);
+ void state_calibrating(const Boardcore::Event& event);
+ void state_ready(const Boardcore::Event& event);
+ void state_active(const Boardcore::Event& event);
+ void state_end(const Boardcore::Event& event);
+
+private:
+ /**
+ * @brief Update the NAS estimation
+ */
+ void update();
+
+ /**
+ * @brief Logs the NAS status updating the FSM state
+ * @param state The current FSM state
+ */
+ void logStatus(NASControllerState state);
+
+ // Controller state machine status
+ NASControllerStatus status;
+ Boardcore::NAS nas;
+
+ // Scheduler to be used for update function
+ Boardcore::TaskScheduler* scheduler;
+
+ // User set (or triac set) initial orientation
+ Eigen::Vector3f initialOrientation;
+
+ // Parameters used to decide wether to correct with accelerometer after a
+ // spike is detected
+ bool accelerationValid = true;
+ u_int8_t accSampleAfterSpike = 0;
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("NAS");
+};
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/NASController/NASControllerData.h b/src/boards/Payload/StateMachines/NASController/NASControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..782c231b6660710de706b85b589d6d802d14bf33
--- /dev/null
+++ b/src/boards/Payload/StateMachines/NASController/NASControllerData.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Payload
+{
+
+enum class NASControllerState : uint8_t
+{
+ UNINIT = 0,
+ IDLE,
+ CALIBRATING,
+ READY,
+ ACTIVE,
+ END
+};
+
+struct NASControllerStatus
+{
+ long long timestamp = 0;
+ NASControllerState state = NASControllerState::UNINIT;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)state << "\n";
+ }
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..33a64f24aaf0617aa5b7566e903019e42a292449
--- /dev/null
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -0,0 +1,472 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul
+ *
+ * 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 "WingController.h"
+
+#include <Payload/Actuators/Actuators.h>
+#include <Payload/BoardScheduler.h>
+#include <Payload/Configs/WESConfig.h>
+#include <Payload/Configs/WingConfig.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/WindEstimationScheme/WindEstimation.h>
+#include <Payload/Wing/AutomaticWingAlgorithm.h>
+#include <Payload/Wing/FileWingAlgorithm.h>
+#include <Payload/Wing/WingAlgorithm.h>
+#include <Payload/Wing/WingAlgorithmData.h>
+#include <Payload/Wing/WingTargetPositionData.h>
+#include <common/Events.h>
+#include <diagnostic/PrintLogger.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Payload::WingConfig;
+using namespace Payload::WESConfig;
+using namespace Common;
+using namespace miosix;
+
+namespace Payload
+{
+
+WingController::WingController(TaskScheduler* sched)
+ : HSM(&WingController::state_idle), running(false), selectedAlgorithm(0),
+ scheduler(sched)
+{
+
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+ EventBroker::getInstance().subscribe(this, TOPIC_DPL);
+ this->targetPositionGEO = {DEFAULT_TARGET_LAT, DEFAULT_TARGET_LON};
+}
+
+bool WingController::start()
+{
+ return scheduler->addTask(std::bind(&WingController::update, this),
+ WING_UPDATE_PERIOD) &&
+ addAlgorithms() && HSM::start();
+}
+
+WingController::~WingController()
+{
+ EventBroker::getInstance().unsubscribe(this);
+}
+
+WingControllerStatus WingController::getStatus()
+{
+ miosix::Lock<miosix::FastMutex> s(statusMutex);
+ return status;
+}
+
+State WingController::state_idle(const Boardcore::Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(WingControllerState::IDLE);
+ return HANDLED;
+ }
+ case FLIGHT_WING_DESCENT:
+ {
+ ModuleManager::getInstance().get<Actuators>()->cuttersOn();
+ return transition(&WingController::state_flying);
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&WingController::state_top);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State WingController::state_flying(const Event& event)
+{
+
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&WingController::state_top);
+ }
+ case EV_INIT:
+ {
+ return transition(&WingController::state_calibration);
+ }
+ case FLIGHT_LANDING_DETECTED:
+ {
+ return transition(&WingController::state_on_ground);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State WingController::state_calibration(const Boardcore::Event& event)
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ switch (event)
+ {
+ case EV_ENTRY: // starts twirling and calibration wes
+ {
+ logStatus(WingControllerState::CALIBRATION);
+ modules.get<Actuators>()->setServo(ServosList::PARAFOIL_LEFT_SERVO,
+ 1);
+ modules.get<Actuators>()->setServo(ServosList::PARAFOIL_RIGHT_SERVO,
+ 0);
+ EventBroker::getInstance().postDelayed(DPL_WES_CAL_DONE, TOPIC_DPL,
+ WES_CALIBRATION_TIMEOUT);
+ modules.get<WindEstimation>()
+ ->startWindEstimationSchemeCalibration();
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&WingController::state_flying);
+ }
+ case DPL_WES_CAL_DONE:
+ {
+ reset();
+ // Turn off the cutters
+ ModuleManager::getInstance().get<Actuators>()->cuttersOff();
+ return transition(&WingController::state_controlled_descent);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+State WingController::state_controlled_descent(const Boardcore::Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY: // start automatic algorithm
+ {
+ logStatus(WingControllerState::ALGORITHM_CONTROLLED);
+ selectAlgorithm(0);
+ setEarlyManeuverPoints(
+ convertTargetPositionToNED(targetPositionGEO),
+ {ModuleManager::getInstance()
+ .get<NASController>()
+ ->getNasState()
+ .n,
+ ModuleManager::getInstance()
+ .get<NASController>()
+ ->getNasState()
+ .e});
+ startAlgorithm();
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&WingController::state_flying);
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State WingController::state_on_ground(const Boardcore::Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(WingControllerState::ON_GROUND);
+
+ ModuleManager::getInstance()
+ .get<WindEstimation>()
+ ->stopWindEstimationScheme();
+ ModuleManager::getInstance()
+ .get<WindEstimation>()
+ ->stopWindEstimationSchemeCalibration();
+ stopAlgorithm();
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&WingController::state_top);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+bool WingController::addAlgorithms()
+{
+ WingAlgorithm* algorithm;
+ WingAlgorithmData step;
+
+ bool result;
+ // We add an AutomaticWingAlgorithm and a FileWingAlgorithm
+
+ algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
+ PARAFOIL_RIGHT_SERVO, emGuidance);
+ // Ensure that the servos are correct
+ algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
+
+ // Init the algorithm
+ result = algorithm->init();
+
+ // Add the algorithm to the vector
+ algorithms.push_back(algorithm);
+
+ algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
+
+ for (int i = 0; i < 3; i++)
+ {
+ step.servo1Angle = 120;
+ step.servo2Angle = 0;
+ step.timestamp = 0 + i * 40000000;
+ algorithm->addStep(step);
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ step.timestamp += 10000000 + i * 40000000;
+ algorithm->addStep(step);
+ step.servo1Angle = 0;
+ step.servo2Angle = 120;
+ step.timestamp += 10000000 + i * 40000000;
+ algorithm->addStep(step);
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ step.timestamp += 10000000 + i * 40000000;
+ algorithm->addStep(step);
+ }
+
+ // Ensure that the servos are correct
+ algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
+
+ // Init the algorithm
+ result = result && algorithm->init();
+
+ // Add the algorithm to the vector
+ algorithms.push_back(algorithm);
+
+ selectAlgorithm(0);
+
+ return result;
+}
+
+bool WingController::selectAlgorithm(unsigned int index)
+{
+ bool success = false;
+ // We change the selected algorithm only if we are in IDLE
+ if (getStatus().state == WingControllerState::IDLE)
+ {
+ stopAlgorithm();
+ if (index < algorithms.size())
+ {
+ LOG_INFO(logger, "Algorithm {:1} selected", index);
+ selectedAlgorithm = index;
+ success = true;
+ }
+ else
+ {
+ // Select the 0 algorithm
+ selectedAlgorithm = 0;
+ success = false;
+ }
+ }
+ return success;
+}
+
+void WingController::startAlgorithm()
+{
+ // If the selected algorithm is valid --> also the
+ // algorithms array is not empty i start the whole thing
+ if (selectedAlgorithm < algorithms.size())
+ {
+ running = true;
+
+ // Begin the selected algorithm
+ algorithms[selectedAlgorithm]->begin();
+
+ LOG_INFO(logger, "Wing algorithm started");
+ }
+}
+
+void WingController::stopAlgorithm()
+{
+ if (running)
+ {
+ // Set running to false
+ running = false;
+ // Stop the algorithm if selected
+ if (selectedAlgorithm < algorithms.size())
+ {
+ algorithms[selectedAlgorithm]->end();
+ reset();
+ }
+ }
+}
+
+void WingController::update()
+{
+ if (running)
+ {
+ algorithms[selectedAlgorithm]->step();
+ }
+}
+
+void WingController::flare()
+{
+ // Set the servo position to flare (pull the two ropes as skydiving people
+ // do)
+
+ ModuleManager::getInstance().get<Actuators>()->setServo(PARAFOIL_LEFT_SERVO,
+ 1);
+ ModuleManager::getInstance().get<Actuators>()->setServo(
+ PARAFOIL_RIGHT_SERVO, 1);
+}
+
+void WingController::reset()
+{
+ // Set the servo position to reset
+ ModuleManager::getInstance().get<Actuators>()->setServo(PARAFOIL_LEFT_SERVO,
+ 0);
+ ModuleManager::getInstance().get<Actuators>()->setServo(
+ PARAFOIL_RIGHT_SERVO, 0);
+}
+
+void WingController::setTargetPosition(Eigen::Vector2f targetGEO)
+{
+ if (ModuleManager::getInstance().get<NASController>()->getStatus().state !=
+ NASControllerState::READY)
+ {
+ this->targetPositionGEO = targetGEO;
+ setEarlyManeuverPoints(
+ convertTargetPositionToNED(targetPositionGEO),
+ {ModuleManager::getInstance().get<NASController>()->getNasState().n,
+ ModuleManager::getInstance()
+ .get<NASController>()
+ ->getNasState()
+ .e});
+ }
+}
+
+void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED,
+ Eigen::Vector2f currentPosNED)
+{
+
+ Eigen::Vector2f targetOffsetNED = targetNED - currentPosNED;
+
+ Eigen::Vector2f norm_point = targetOffsetNED / targetOffsetNED.norm();
+
+ float psi0 = atan2(norm_point[1], norm_point[0]);
+
+ float distFromCenterline = 20; // the distance that the M1 and M2 points
+ // must have from the center line
+
+ // Calculate the angle between the lines <NED Origin, target> and <NED
+ // Origin, M1> This angle is the same for M2 since is symmetric to M1
+ // relatively to the center line
+ float psiMan = atan2(distFromCenterline, targetOffsetNED.norm());
+
+ float maneuverPointsMagnitude = distFromCenterline / sin(psiMan);
+ float m2Angle = psi0 + psiMan;
+ float m1Angle = psi0 - psiMan;
+
+ Eigen::Vector2f emcPosition =
+ targetOffsetNED * 1.2 +
+ currentPosNED; // EMC is calculated as target * 1.2
+
+ Eigen::Vector2f m1Position =
+ Eigen::Vector2f(cos(m1Angle), sin(m1Angle)) * maneuverPointsMagnitude +
+ currentPosNED;
+
+ Eigen::Vector2f m2Position =
+ Eigen::Vector2f(cos(m2Angle), sin(m2Angle)) * maneuverPointsMagnitude +
+ currentPosNED;
+
+ emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position);
+ WingTargetPositionData data;
+
+ data.receivedLat = targetPositionGEO[0];
+ data.receivedLon = targetPositionGEO[1];
+
+ data.targetN = targetNED[0];
+ data.targetE = targetNED[1];
+
+ data.emcN = emcPosition[0];
+ data.emcE = emcPosition[1];
+
+ data.m1N = m1Position[0];
+ data.m1E = m1Position[1];
+
+ data.m2N = m2Position[0];
+ data.m2E = m2Position[1];
+
+ // Log the received position
+ Logger::getInstance().log(data);
+}
+
+void WingController::logStatus(WingControllerState state)
+{
+ miosix::Lock<miosix::FastMutex> s(statusMutex);
+ status.timestamp = TimestampTimer::getTimestamp();
+ status.state = state;
+
+ Logger::getInstance().log(status);
+}
+
+Eigen::Vector2f WingController::convertTargetPositionToNED(
+ Eigen::Vector2f targetGEO)
+{
+ return Aeroutils::geodetic2NED(targetGEO, {ModuleManager::getInstance()
+ .get<NASController>()
+ ->getReferenceValues()
+ .refLatitude,
+ ModuleManager::getInstance()
+ .get<NASController>()
+ ->getReferenceValues()
+ .refLongitude});
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
new file mode 100644
index 0000000000000000000000000000000000000000..1fb84ee6638df0990638911734ca37c7d6f9b8c1
--- /dev/null
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -0,0 +1,182 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
+#include <Payload/Wing/WingAlgorithm.h>
+#include <events/HSM.h>
+
+#include <Eigen/Core>
+#include <atomic>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "WingControllerData.h"
+
+/**
+ * @brief This class allows the user to select the wing algorithm
+ * that has to be used during the tests. It also registers his
+ * dedicated function in the task scheduler in order to be
+ * executed every fixed period and to update the two servos position
+ * depending on the selected algorithm.
+ *
+ * Use case example:
+ * controller = new WingController(scheduler);
+ *
+ * controller.addAlgorithm("filename");
+ * OR
+ * controller.addAlgorithm(algorithm);
+ *
+ * controller.selectAlgorithm(index);
+ *
+ * controller.start();
+ * controller.stop(); //If you want to abort the execution
+ * controller.start(); //If you want to start again from the beginning
+ */
+
+namespace Payload
+{
+class WingController : public Boardcore::HSM<WingController>,
+ public Boardcore::Module
+
+{
+
+public:
+ Boardcore::State state_idle(const Boardcore::Event& event);
+ Boardcore::State state_flying(const Boardcore::Event& event);
+ Boardcore::State state_calibration(const Boardcore::Event& event);
+ Boardcore::State state_controlled_descent(const Boardcore::Event& event);
+ Boardcore::State state_on_ground(const Boardcore::Event& event);
+
+ /**
+ * @brief Destroy the Wing Controller object.
+ */
+ ~WingController();
+
+ /**
+ * @brief Method to set the target position.
+ */
+ void setTargetPosition(Eigen::Vector2f target);
+
+ /**
+ * @brief Selects the algorithm if present.
+ *
+ * @param index The algorithms vector index
+ *
+ * @return true if the selection was successful
+ */
+ bool selectAlgorithm(unsigned int index);
+
+ WingControllerStatus getStatus();
+
+ /**
+ * @brief Construct a new Wing Controller object
+ */
+ explicit WingController(Boardcore::TaskScheduler* sched);
+
+ bool start() override;
+
+private:
+ /**
+ * @brief Method to add the algorithm in the list
+ *
+ * @param algorithm The algorithm with
+ * all already done (e.g. steps already registered)
+ */
+ bool addAlgorithms();
+
+ /**
+ * @brief target position getter
+ */
+ Eigen::Vector2f convertTargetPositionToNED(Eigen::Vector2f targetGEO);
+
+ /**
+ * @brief set points needed by the Guidance
+ */
+ void setEarlyManeuverPoints(Eigen::Vector2f targetNED,
+ Eigen::Vector2f currentPosNED);
+
+ void logStatus(WingControllerState state);
+
+ WingControllerStatus status;
+
+ miosix::FastMutex statusMutex;
+
+ /**
+ * @brief Target position
+ */
+ Eigen::Vector2f targetPositionGEO;
+
+ /**
+ * @brief List of loaded algorithms (from SD or not)
+ */
+ std::vector<WingAlgorithm*> algorithms;
+
+ /**
+ * @brief PrintLogger
+ */
+ Boardcore::PrintLogger logger =
+ Boardcore::Logging::getLogger("PayloadTest");
+
+ /**
+ * @brief Internal running state
+ */
+ std::atomic<bool> running;
+ /**
+ * @brief This attribute is modified by the mavlink radio section.
+ * The user using the Ground Station can select the pre-enumerated algorithm
+ * to execute
+ */
+ std::atomic<size_t> selectedAlgorithm;
+
+ /**
+ * @brief Instance of the Early Maneuver Guidance Algorithm used by
+ * AutomaticWingAlgorithm
+ */
+ EarlyManeuversGuidanceAlgorithm emGuidance;
+
+ /**
+ * @brief starts the selected algorithm
+ */
+ void startAlgorithm();
+
+ /**
+ * @brief Sets the internal state to stop and
+ * stops the selected algorithm
+ */
+ void stopAlgorithm();
+
+ /**
+ * @brief Stops any on going algorithm and flares the wing
+ */
+ void flare();
+
+ /**
+ * @brief Resets the servos in their initial position
+ */
+ void reset();
+
+ void update();
+
+ Boardcore::TaskScheduler* scheduler = nullptr;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/WingController/WingControllerData.h b/src/boards/Payload/StateMachines/WingController/WingControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..82e444561b10465ed3c179b058b9198c5de7cf9a
--- /dev/null
+++ b/src/boards/Payload/StateMachines/WingController/WingControllerData.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Payload
+{
+
+enum class WingControllerState : uint8_t
+{
+ UNINIT = 0,
+ IDLE,
+ CALIBRATION,
+ ALGORITHM_CONTROLLED,
+ ON_GROUND,
+ END
+};
+
+struct WingControllerStatus
+{
+ long long timestamp = 0;
+ WingControllerState state = WingControllerState::UNINIT;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)state << "\n";
+ }
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/TMRepository/TMRepository.cpp b/src/boards/Payload/TMRepository/TMRepository.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..62eec8f4f034178cc1705e1d7e37989f05304eeb
--- /dev/null
+++ b/src/boards/Payload/TMRepository/TMRepository.cpp
@@ -0,0 +1,616 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Actuators/Actuators.h>
+#include <Payload/BoardScheduler.h>
+#include <Payload/Configs/RadioConfig.h>
+#include <Payload/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Payload/PinHandler/PinHandler.h>
+#include <Payload/Radio/Radio.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/StateMachines/WingController/WingController.h>
+#include <Payload/TMRepository/TMRepository.h>
+#include <Payload/WindEstimationScheme/WindEstimation.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace miosix;
+using namespace Boardcore;
+
+namespace Payload
+{
+mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
+ uint8_t seq)
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ mavlink_message_t msg;
+
+ switch (tmId)
+ {
+ case SystemTMList::MAV_SYS_ID:
+ {
+ mavlink_sys_tm_t tm;
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.logger = Logger::getInstance().isStarted();
+ tm.board_scheduler = modules.get<BoardScheduler>()->isStarted();
+ tm.event_broker = EventBroker::getInstance().isRunning();
+ tm.radio = modules.get<Radio>()->isStarted();
+ tm.sensors = modules.get<Sensors>()->isStarted();
+ tm.pin_observer = modules.get<PinHandler>()->isStarted();
+
+ mavlink_msg_sys_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+ case SystemTMList::MAV_LOGGER_ID:
+ {
+ mavlink_logger_tm_t tm;
+
+ // Get the logger stats
+ LoggerStats stats = Logger::getInstance().getStats();
+
+ tm.timestamp = stats.timestamp;
+ tm.average_write_time = stats.averageWriteTime;
+ tm.buffers_filled = stats.buffersFilled;
+ tm.buffers_written = stats.buffersWritten;
+ tm.dropped_samples = stats.droppedSamples;
+ tm.last_write_error = stats.lastWriteError;
+ tm.log_number = stats.logNumber;
+ tm.max_write_time = stats.maxWriteTime;
+ tm.queued_samples = stats.queuedSamples;
+ tm.too_large_samples = stats.tooLargeSamples;
+ tm.writes_failed = stats.writesFailed;
+
+ mavlink_msg_logger_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+ case SystemTMList::MAV_MAVLINK_STATS:
+ {
+ mavlink_mavlink_stats_tm_t tm;
+
+ // Get the mavlink stats
+ MavlinkStatus stats = modules.get<Radio>()->mavDriver->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(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg,
+ &tm);
+ break;
+ }
+ case SystemTMList::MAV_NAS_ID:
+ {
+ mavlink_nas_tm_t tm;
+
+ // Get the current NAS state
+ NASState state = modules.get<NASController>()->getNasState();
+ ReferenceValues ref =
+ modules.get<NASController>()->getReferenceValues();
+
+ tm.timestamp = state.timestamp;
+ tm.state = static_cast<uint8_t>(
+ modules.get<NASController>()->getStatus().state);
+ tm.nas_n = state.n;
+ tm.nas_e = state.e;
+ tm.nas_d = state.d;
+ tm.nas_vn = state.vn;
+ tm.nas_ve = state.ve;
+ tm.nas_vd = state.vd;
+ tm.nas_qx = state.qx;
+ tm.nas_qy = state.qy;
+ tm.nas_qz = state.qz;
+ tm.nas_qw = state.qw;
+ tm.nas_bias_x = state.bx;
+ tm.nas_bias_y = state.by;
+ tm.nas_bias_z = state.bz;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_temperature = ref.refTemperature;
+ tm.ref_latitude = ref.refLatitude;
+ tm.ref_longitude = ref.refLongitude;
+
+ mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+ case SystemTMList::MAV_FLIGHT_ID:
+ {
+ mavlink_payload_flight_tm_t tm;
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+
+ // Last samples
+ LPS28DFWData lps28dfw1 =
+ modules.get<Sensors>()->getLPS28DFW_1LastSample();
+ RotatedIMUData imu = modules.get<Sensors>()->getIMULastSample();
+ UBXGPSData gps = modules.get<Sensors>()->getGPSLastSample();
+ Eigen::Vector2f wind =
+ modules.get<WindEstimation>()->getWindEstimationScheme();
+
+ // NAS state
+ NASState nasState = modules.get<NASController>()->getNasState();
+
+ tm.nas_state = static_cast<uint8_t>(
+ modules.get<NASController>()->getStatus().state);
+ tm.fmm_state = static_cast<uint8_t>(
+ modules.get<FlightModeManager>()->getStatus().state);
+ tm.wes_state = static_cast<uint8_t>(
+ modules.get<WingController>()->getStatus().state);
+
+ tm.wes_n = wind[0];
+ tm.wes_e = wind[1];
+
+ tm.pressure_digi = lps28dfw1.pressure;
+ tm.pressure_static =
+ modules.get<Sensors>()->getStaticPressureLastSample().pressure;
+ // Pitot
+ tm.airspeed_pitot =
+ modules.get<Sensors>()->getPitotLastSample().airspeed;
+
+ // Altitude agl
+ tm.altitude_agl = -nasState.d;
+
+ // IMU
+ 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;
+
+ // Magnetometer
+ tm.mag_x = imu.magneticFieldX;
+ tm.mag_y = imu.magneticFieldY;
+ tm.mag_z = imu.magneticFieldZ;
+
+ // GPS
+ tm.gps_fix = gps.fix;
+ tm.gps_lat = gps.latitude;
+ tm.gps_lon = gps.longitude;
+ tm.gps_alt = gps.height;
+
+ // NAS
+ 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;
+ // Servos
+ tm.left_servo_angle = modules.get<Actuators>()->getServoAngle(
+ ServosList::PARAFOIL_LEFT_SERVO);
+ tm.right_servo_angle = modules.get<Actuators>()->getServoAngle(
+ ServosList::PARAFOIL_RIGHT_SERVO);
+
+ tm.pin_nosecone = modules.get<PinHandler>()
+ ->getPinsData()[PinsList::NOSECONE_PIN]
+ .lastState;
+
+ tm.cutter_presence =
+ static_cast<uint8_t>(miosix::gpios::cut_sense::value());
+
+ tm.battery_voltage = modules.get<Sensors>()
+ ->getBatteryVoltageLastSample()
+ .batVoltage;
+ tm.current_consumption =
+ modules.get<Sensors>()->getCurrentLastSample().current;
+ tm.temperature = lps28dfw1.temperature;
+ tm.logger_error = Logger::getInstance().getStats().lastWriteError;
+
+ tm.cam_battery_voltage = modules.get<Sensors>()
+ ->getCamBatteryVoltageLastSample()
+ .batVoltage;
+
+ mavlink_msg_payload_flight_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg,
+ &tm);
+
+ break;
+ }
+ case SystemTMList::MAV_STATS_ID:
+ {
+ mavlink_payload_stats_tm_t tm =
+ modules.get<FlightStatsRecorder>()->getStats();
+ tm.cpu_load = CpuMeter::getCpuStats().mean;
+ tm.free_heap = CpuMeter::getCpuStats().freeHeap;
+
+ mavlink_msg_payload_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg,
+ &tm);
+ break;
+ }
+ case SystemTMList::MAV_FSM_ID:
+ {
+ mavlink_fsm_tm_t tm;
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.abk_state = 0;
+ tm.ada_state = 0;
+ tm.dpl_state = static_cast<uint8_t>(
+ modules.get<WingController>()->getStatus().state);
+ tm.fmm_state = static_cast<uint8_t>(
+ modules.get<FlightModeManager>()->getStatus().state);
+ tm.nas_state = static_cast<uint8_t>(
+ modules.get<NASController>()->getStatus().state);
+ tm.wes_state = 0;
+
+ mavlink_msg_fsm_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+ case SystemTMList::MAV_PIN_OBS_ID:
+ {
+ mavlink_pin_tm_t tm;
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.last_change_timestamp =
+ modules.get<PinHandler>()
+ ->getPinsData()[PinsList::NOSECONE_PIN]
+ .lastStateTimestamp; /*< Last change timestamp of pin*/
+ tm.pin_id =
+ PinsList::NOSECONE_PIN; /*< A member of the PinsList enum*/
+ tm.changes_counter =
+ modules.get<PinHandler>()
+ ->getPinsData()[PinsList::NOSECONE_PIN]
+ .changesCount; /*< Number of changes of pin*/
+ tm.current_state = modules.get<PinHandler>()
+ ->getPinsData()[PinsList::NOSECONE_PIN]
+ .lastState; /*< Current state of pin*/
+
+ mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+ default:
+ {
+ // Send by default the nack message
+ mavlink_nack_tm_t nack;
+
+ nack.recv_msgid = msgId;
+ nack.seq_ack = seq;
+
+ LOG_ERR(logger, "Unknown message id: {}", tmId);
+ mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &nack);
+
+ break;
+ }
+ }
+ return msg;
+}
+mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId,
+ uint8_t msgId, uint8_t seq)
+{
+ mavlink_message_t msg;
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ switch (sensorId)
+ {
+ case SensorsTMList::MAV_GPS_ID:
+ {
+ mavlink_gps_tm_t tm;
+
+ UBXGPSData gpsData = modules.get<Sensors>()->getGPSLastSample();
+
+ tm.timestamp = gpsData.gpsTimestamp;
+ strcpy(tm.sensor_name, "UBXGPS");
+ tm.fix = gpsData.fix;
+ tm.height = gpsData.height;
+ tm.latitude = gpsData.latitude;
+ tm.longitude = gpsData.longitude;
+ tm.n_satellites = gpsData.satellites;
+ tm.speed = gpsData.speed;
+ tm.track = gpsData.track;
+ tm.vel_down = gpsData.velocityDown;
+ tm.vel_east = gpsData.velocityEast;
+ tm.vel_north = gpsData.velocityNorth;
+
+ mavlink_msg_gps_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+
+ case SensorsTMList::MAV_STATIC_PRESS_ID:
+ {
+ mavlink_pressure_tm_t tm;
+
+ HSCMRNN015PAData pressureData =
+ modules.get<Sensors>()->getStaticPressureLastSample();
+
+ tm.timestamp = pressureData.pressureTimestamp;
+ strcpy(tm.sensor_name, "STATIC_PRESSURE");
+ tm.pressure = pressureData.pressure;
+
+ mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+ case SensorsTMList::MAV_DPL_PRESS_ID:
+ {
+ mavlink_pressure_tm_t tm;
+
+ SSCMRNN030PAData pressureData =
+ modules.get<Sensors>()->getDynamicPressureLastSample();
+
+ tm.timestamp = pressureData.pressureTimestamp;
+ strcpy(tm.sensor_name, "DYNAMIC_PRESSURE");
+ tm.pressure = pressureData.pressure;
+
+ mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
+ case SensorsTMList::MAV_PITOT_PRESS_ID:
+ {
+ mavlink_pressure_tm_t tm;
+
+ SSCMRNN030PAData pitot =
+ modules.get<Sensors>()->getDynamicPressureLastSample();
+
+ tm.timestamp = pitot.pressureTimestamp;
+ tm.pressure = pitot.pressure;
+ strcpy(tm.sensor_name, "PITOT_PRESSURE");
+
+ mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_LIS2MDL_ID:
+ {
+ mavlink_imu_tm_t tm;
+
+ LIS2MDLData mag = modules.get<Sensors>()->getLIS2MDLLastSample();
+
+ 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 = mag.magneticFieldX;
+ tm.mag_y = mag.magneticFieldY;
+ tm.mag_z = mag.magneticFieldZ;
+ tm.timestamp = mag.magneticFieldTimestamp;
+ strcpy(tm.sensor_name, "LIS2MD");
+
+ mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_LSM6DSRX_ID:
+ {
+ mavlink_imu_tm_t tm;
+
+ LSM6DSRXData imu = modules.get<Sensors>()->getLSM6DSRXLastSample();
+
+ 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 = 0;
+ tm.mag_y = 0;
+ tm.mag_z = 0;
+
+ tm.timestamp = imu.accelerationTimestamp;
+ strcpy(tm.sensor_name, "LSM6DSRX");
+
+ mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_H3LIS331DL_ID:
+ {
+ mavlink_imu_tm_t tm;
+
+ H3LIS331DLData imu =
+ modules.get<Sensors>()->getH3LIS331DLLastSample();
+
+ tm.acc_x = imu.accelerationX;
+ tm.acc_y = imu.accelerationY;
+ tm.acc_z = imu.accelerationZ;
+ tm.gyro_x = 0;
+ tm.gyro_y = 0;
+ tm.gyro_z = 0;
+ tm.mag_x = 0;
+ tm.mag_y = 0;
+ tm.mag_z = 0;
+
+ tm.timestamp = imu.accelerationTimestamp;
+ strcpy(tm.sensor_name, "H3LIS331DL");
+
+ mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_LPS22DF_ID:
+ {
+ mavlink_pressure_tm_t tm;
+
+ LPS22DFData pressure =
+ modules.get<Sensors>()->getLPS22DFLastSample();
+
+ tm.timestamp = pressure.pressureTimestamp;
+ tm.pressure = pressure.pressure;
+ strcpy(tm.sensor_name, "LPS22DF");
+
+ mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_LPS28DFW_ID:
+ {
+ mavlink_pressure_tm_t tm;
+ LPS28DFWData pressure1 =
+ modules.get<Sensors>()->getLPS28DFW_1LastSample();
+
+ tm.timestamp = pressure1.pressureTimestamp;
+ tm.pressure = pressure1.pressure;
+ strcpy(tm.sensor_name, "LPS28DFW");
+
+ mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_CURRENT_SENSE_ID:
+ {
+ mavlink_current_tm_t tm;
+ CurrentData current =
+ modules.get<Sensors>()->getCurrentLastSample();
+ tm.current = current.current;
+ strcpy(tm.sensor_name, "CURRENT");
+ tm.timestamp = current.currentTimestamp;
+
+ mavlink_msg_current_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_BATTERY_VOLTAGE_ID:
+ {
+ mavlink_voltage_tm_t tm;
+ BatteryVoltageSensorData voltage =
+ modules.get<Sensors>()->getBatteryVoltageLastSample();
+ tm.voltage = voltage.batVoltage;
+ strcpy(tm.sensor_name, "BATTERY");
+ tm.timestamp = voltage.voltageTimestamp;
+ mavlink_msg_voltage_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ case SensorsTMList::MAV_ADS_ID:
+ {
+ mavlink_adc_tm_t tm;
+ ADS131M08Data voltage =
+ modules.get<Sensors>()->getADS131M08LastSample();
+ tm.channel_0 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
+
+ tm.channel_1 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
+
+ tm.channel_2 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
+
+ tm.channel_3 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
+
+ tm.channel_4 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
+
+ tm.channel_5 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
+
+ tm.channel_6 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
+
+ tm.channel_7 =
+ voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
+
+ strcpy(tm.sensor_name, "ADS131M08");
+ tm.timestamp = voltage.timestamp;
+ mavlink_msg_adc_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ break;
+ }
+ default:
+ {
+ mavlink_nack_tm_t nack;
+
+ nack.recv_msgid = msgId;
+ nack.seq_ack = seq;
+
+ LOG_DEBUG(logger, "Unknown telemetry id: {}", sensorId);
+ mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &nack);
+ break;
+ }
+ }
+
+ return msg;
+}
+
+mavlink_message_t TMRepository::packServoTm(ServosList servoId, uint8_t msgId,
+ uint8_t seq)
+{
+ mavlink_message_t msg;
+
+ if (servoId == PARAFOIL_LEFT_SERVO || servoId == PARAFOIL_RIGHT_SERVO)
+ {
+ mavlink_servo_tm_t tm;
+
+ tm.servo_id = servoId;
+ tm.servo_position =
+ ModuleManager::getInstance().get<Actuators>()->getServoAngle(
+ servoId);
+
+ mavlink_msg_servo_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+ }
+ else
+ {
+ mavlink_nack_tm_t nack;
+
+ nack.recv_msgid = msgId;
+ nack.seq_ack = seq;
+
+ mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &nack);
+ }
+
+ return msg;
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/TMRepository/TMRepository.h b/src/boards/Payload/TMRepository/TMRepository.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d2285c9c1c451511e03e593d4df7181811a1d29
--- /dev/null
+++ b/src/boards/Payload/TMRepository/TMRepository.h
@@ -0,0 +1,52 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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/Mavlink.h>
+#include <diagnostic/PrintLogger.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+class TMRepository : public Boardcore::Module
+{
+public:
+ inline TMRepository() {}
+
+ // Packs the telemetry at system level (E.g. logger telemetry..)
+ mavlink_message_t packSystemTm(SystemTMList tmId, uint8_t msgId,
+ uint8_t seq);
+
+ // Packs the telemetry at sensors level (E.g. pressure sensors..)
+ mavlink_message_t packSensorsTm(SensorsTMList sensorId, uint8_t msgId,
+ uint8_t seq);
+
+ // Packs the telemetry about servo positions states
+ mavlink_message_t packServoTm(ServosList servoId, uint8_t msgId,
+ uint8_t seq);
+
+private:
+ Boardcore::PrintLogger logger =
+ Boardcore::Logging::getLogger("TMRepository");
+};
+} // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..538d58a45b8441582492659d9f91faf28fe5217f
--- /dev/null
+++ b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp
@@ -0,0 +1,87 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Raul Radu
+ *
+ * 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 <Payload/BoardScheduler.h>
+#include <Payload/Configs/VerticalVelocityConfig.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+
+#include <functional>
+
+using namespace std;
+using namespace Boardcore;
+using namespace Common;
+
+namespace Payload
+{
+
+VerticalVelocityTrigger::VerticalVelocityTrigger(TaskScheduler* sched)
+ : running(false), confidence(0), scheduler(sched)
+{
+}
+
+bool VerticalVelocityTrigger::start()
+{
+ return scheduler->addTask(
+ bind(&VerticalVelocityTrigger::update, this),
+ FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD);
+}
+
+void VerticalVelocityTrigger::enable()
+{
+ confidence = 0;
+ running = true;
+}
+
+void VerticalVelocityTrigger::disable() { running = false; }
+
+bool VerticalVelocityTrigger::isActive() { return running; }
+
+void VerticalVelocityTrigger::update()
+{
+ if (running)
+ {
+ float verticalVelocity = -ModuleManager::getInstance()
+ .get<NASController>()
+ ->getNasState()
+ .vd;
+ if (verticalVelocity < FailSafe::FAILSAFE_VERTICAL_VELOCITY_THRESHOLD)
+ {
+ confidence++;
+ }
+ else
+ {
+ confidence = 0;
+ }
+ if (confidence >=
+ FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_CONFIDENCE)
+ {
+ confidence = 0;
+ EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+ TOPIC_FLIGHT);
+ running = false;
+ }
+ }
+}
+} // namespace Payload
diff --git a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h
new file mode 100644
index 0000000000000000000000000000000000000000..84f721f35a6e05efdff43de2da985f81ed8de9da
--- /dev/null
+++ b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h
@@ -0,0 +1,90 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Raul Radu
+ *
+ * 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 <Payload/BoardScheduler.h>
+
+#include <atomic>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Payload
+{
+
+/**
+ * This class is used by the FailSafe logic to react to
+ * the premature opening of the parafoil by the payload
+ * Required modules:
+ * - Payload::BoardScheduler
+ * - Payload::NASController
+ */
+class VerticalVelocityTrigger : public Boardcore::Module
+{
+public:
+ /**
+ * Default constructor for VerticalVelocityTrigger
+ * Sets the trigger to disabled by default
+ */
+ VerticalVelocityTrigger(Boardcore::TaskScheduler* sched);
+
+ /**
+ * Starts the module by inserting it in the BoardScheduler
+ */
+ bool start();
+
+ /**
+ * Enables the Vertical Velocity Trigger
+ */
+ void enable();
+
+ /**
+ * Disables the Vertical Velocity Trigger
+ */
+ void disable();
+
+ /**
+ * Checks if the trigger is enabled
+ * @return true if the trigger is enabled
+ * @return false if the trigger is not enabled
+ */
+ bool isActive();
+
+private:
+ /**
+ * This method is called every
+ * FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD
+ * and reads the state from the NAS and when the velocity is below
+ * the threshold for some confidence level it posts the event
+ * FLIGHT_FAILSAFE_TRIGGERED in topic TOPIC_FLIGHT
+ */
+ void update();
+
+ std::atomic<bool> running;
+
+ // Number of times that the algorithm detects to be slower than the velocity
+ // threshold
+ std::atomic<int> confidence;
+
+ Boardcore::TaskScheduler* scheduler;
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e6cc50cc7baec7997e6986e7119837eee200cde4
--- /dev/null
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
@@ -0,0 +1,215 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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 "WindEstimation.h"
+
+#include <Payload/BoardScheduler.h>
+#include <Payload/Configs/WESConfig.h>
+#include <Payload/Sensors/Sensors.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Payload::WESConfig;
+using namespace Boardcore;
+using namespace Common;
+using namespace miosix;
+
+namespace Payload
+{
+
+WindEstimation::WindEstimation(TaskScheduler* sched)
+ : running(false), calRunning(false), scheduler(sched)
+{
+ funv << 1.0f, 0.0f, 0.0f, 1.0f; // cppcheck-suppress constStatement
+}
+
+bool WindEstimation::start()
+{
+ scheduler->addTask(
+ std::bind(&WindEstimation::windEstimationSchemeCalibration, this),
+ WES_CALIBRATION_UPDATE_PERIOD);
+
+ // Register the WES task
+ scheduler->addTask(std::bind(&WindEstimation::windEstimationScheme, this),
+ WES_PREDICTION_UPDATE_PERIOD);
+
+ return true;
+}
+
+WindEstimation::~WindEstimation()
+{
+ stopWindEstimationSchemeCalibration();
+ stopWindEstimationScheme();
+}
+
+bool WindEstimation::getStatus() { return running; }
+
+void WindEstimation::startWindEstimationSchemeCalibration()
+{
+ if (!calRunning && !running)
+ {
+ calRunning = true;
+ nSampleCal = 0;
+ vx = 0;
+ vy = 0;
+ v2 = 0;
+ LOG_INFO(logger, "WindEstimationCalibration started");
+ }
+}
+
+void WindEstimation::stopWindEstimationSchemeCalibration()
+{
+ calRunning = false;
+ LOG_INFO(logger, "WindEstimationSchemeCalibration stopped");
+ // assign value to the array only if running != false;
+ if (!running)
+ {
+ miosix::Lock<FastMutex> l(mutex);
+ wind = windCalibration;
+ }
+ windLogger.vn = windCalibration[0];
+ windLogger.ve = windCalibration[1];
+ startWindEstimationScheme();
+ logStatus();
+}
+
+void WindEstimation::windEstimationSchemeCalibration()
+{
+
+ if (calRunning)
+ {
+ auto gpsData =
+ ModuleManager::getInstance().get<Sensors>()->getGPSLastSample();
+ if (gpsData.fix != 0)
+ {
+ if (nSampleCal < WES_CALIBRATION_SAMPLE_NUMBER)
+ {
+ float gpsN = 0, gpsE = 0;
+
+ gpsN = gpsData.velocityNorth;
+ gpsE = gpsData.velocityEast;
+ calibrationMatrix(nSampleCal, 0) = gpsN;
+ calibrationMatrix(nSampleCal, 1) = gpsE;
+ calibrationV2(nSampleCal) = gpsN * gpsN + gpsE * gpsE;
+
+ vx += gpsN;
+ vy += gpsE;
+ v2 += gpsN * gpsN + gpsE * gpsE;
+ nSampleCal++;
+ }
+ else
+ {
+ vx = vx / nSampleCal;
+ vy = vy / nSampleCal;
+ v2 = v2 / nSampleCal;
+ for (int i = 0; i < nSampleCal; i++)
+ {
+ calibrationMatrix(i, 0) = calibrationMatrix(i, 0) - vx;
+ calibrationMatrix(i, 1) = calibrationMatrix(i, 1) - vy;
+ calibrationV2(i) = 0.5f * (calibrationV2(i) - v2);
+ }
+ Eigen::MatrixXf calibrationMatrixT =
+ calibrationMatrix.transpose();
+ windCalibration =
+ (calibrationMatrixT * calibrationMatrix)
+ .ldlt()
+ .solve(calibrationMatrixT * calibrationV2);
+ stopWindEstimationSchemeCalibration();
+ }
+ }
+ }
+}
+
+void WindEstimation::startWindEstimationScheme()
+{
+ if (!running)
+ {
+ running = true;
+ // nSample has to be reset after the calibration
+ nSample = nSampleCal;
+ LOG_INFO(logger, "WindEstimationScheme started");
+ }
+}
+
+void WindEstimation::stopWindEstimationScheme()
+{
+ if (running)
+ {
+ running = false;
+ LOG_INFO(logger, "WindEstimationScheme ended");
+ }
+}
+
+void WindEstimation::windEstimationScheme()
+{
+ if (running)
+ {
+ auto gpsData =
+ ModuleManager::getInstance().get<Sensors>()->getGPSLastSample();
+ if (gpsData.fix != 0)
+ {
+ Eigen::Vector2f phi;
+ Eigen::Matrix<float, 1, 2> phiT;
+ Eigen::Vector2f temp;
+ float y, gpsN = 0, gpsE = 0;
+ gpsN = gpsData.velocityNorth;
+ gpsE = gpsData.velocityEast;
+ // update avg
+ nSample++;
+ vx = (vx * nSample + gpsN) / (nSample + 1);
+ vy = (vy * nSample + gpsE) / (nSample + 1);
+ v2 = (v2 * nSample + (gpsN * gpsN + gpsE * gpsE)) / (nSample + 1);
+ phi(0) = gpsN - vx;
+ phi(1) = gpsE - vy;
+ y = 0.5f * ((gpsN * gpsN + gpsE * gpsE) - v2);
+
+ phiT = phi.transpose();
+ funv =
+ (funv - (funv * phi * phiT * funv) / (1 + (phiT * funv * phi)));
+ temp = (0.5 * (funv + funv.transpose()) * phi) *
+ (y - phiT * getWindEstimationScheme());
+ {
+ miosix::Lock<FastMutex> l(mutex);
+ wind = wind + temp;
+ windLogger.vn = wind[0];
+ windLogger.ve = wind[1];
+ }
+ logStatus();
+ }
+ }
+}
+
+Eigen::Vector2f WindEstimation::getWindEstimationScheme()
+{
+ miosix::Lock<FastMutex> l(mutex);
+ return wind;
+}
+
+void WindEstimation::logStatus()
+{
+ windLogger.timestamp = TimestampTimer::getTimestamp();
+ Logger::getInstance().log(windLogger);
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.h b/src/boards/Payload/WindEstimationScheme/WindEstimation.h
new file mode 100644
index 0000000000000000000000000000000000000000..e3c5e0148ab938b66afcc26fda9ccd412842b31e
--- /dev/null
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.h
@@ -0,0 +1,130 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * 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 <Payload/BoardScheduler.h>
+#include <Payload/Configs/WESConfig.h>
+#include <diagnostic/PrintLogger.h>
+#include <logger/Logger.h>
+
+#include <Eigen/Core>
+#include <atomic>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "WindEstimationData.h"
+namespace Payload
+{
+
+/**
+ * @brief This class implements the wind prediction algorithm, the first part is
+ * the initial setup, and then the continuos algoritms runs;
+ */
+class WindEstimation : public Boardcore::Module
+{
+public:
+ /**
+ * @brief Construct a new Wing Controller object
+ */
+ explicit WindEstimation(Boardcore::TaskScheduler* sched);
+
+ /**
+ * @brief Destroy the Wing Controller object.
+ */
+ ~WindEstimation();
+
+ bool start();
+
+ void startWindEstimationSchemeCalibration();
+
+ void stopWindEstimationSchemeCalibration();
+
+ void startWindEstimationScheme();
+
+ void stopWindEstimationScheme();
+
+ bool getStatus();
+
+ Eigen::Vector2f getWindEstimationScheme();
+
+private:
+ /**
+ * @brief Creates the windCalibration matrix with the starting prediction
+ * value
+ */
+ void windEstimationSchemeCalibration();
+
+ /**
+ * @brief Updates the wind matrix with the updated wind prediction values
+ */
+ void windEstimationScheme();
+
+ /**
+ * @brief Logs the prediction
+ */
+ void logStatus();
+
+ /**
+ * @brief Parameters needed for calibration
+ */
+ Eigen::Vector2f windCalibration{0.0f, 0.0f};
+ uint8_t nSampleCal = 0;
+ Eigen::Matrix<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER, 2>
+ calibrationMatrix;
+ Eigen::Vector<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER>
+ calibrationV2;
+ float vx = 0;
+ float vy = 0;
+ float v2 = 0;
+
+ /**
+ * @brief Parameters needed for recursive
+ */
+ Eigen::Vector2f wind{0.0f, 0.0f};
+ uint8_t nSample = 0;
+ Eigen::Matrix2f funv;
+
+ /**
+ * @brief Mutex
+ */
+ miosix::FastMutex mutex;
+
+ /**
+ * @brief PrintLogger
+ */
+ Boardcore::PrintLogger logger =
+ Boardcore::Logging::getLogger("PayloadTest");
+
+ /**
+ * @brief Logging struct
+ */
+ WindLogging windLogger{0, 0, 0};
+
+ /**
+ * @brief Internal running state
+ */
+ std::atomic<bool> running;
+ std::atomic<bool> calRunning;
+
+ Boardcore::TaskScheduler* scheduler = nullptr;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimationData.h b/src/boards/Payload/WindEstimationScheme/WindEstimationData.h
new file mode 100644
index 0000000000000000000000000000000000000000..49d674fc96df107474c7b001e301530e1879770e
--- /dev/null
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimationData.h
@@ -0,0 +1,46 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Payload
+{
+
+struct WindLogging
+{
+ long long timestamp = 0;
+ float vn = 0, ve = 0;
+
+ static std::string header() { return "timestamp,vn,ve\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << vn << "," << ve << "\n ";
+ }
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eab89c3e4bea83a43ce9af65a76b91b5473272ef
--- /dev/null
+++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
@@ -0,0 +1,181 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Radu Raul
+ *
+ * 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 "AutomaticWingAlgorithm.h"
+
+#include <Payload/Configs/WingConfig.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/StateMachines/WingController/WingController.h>
+#include <Payload/WindEstimationScheme/WindEstimation.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <math.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/Constants.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Boardcore;
+using namespace Eigen;
+using namespace Payload::WingConfig;
+
+namespace Payload
+{
+AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki,
+ ServosList servo1,
+ ServosList servo2,
+ GuidanceAlgorithm& guidance)
+ : WingAlgorithm(servo1, servo2), guidance(guidance)
+{
+ controller = new PIController(Kp, Ki, WING_UPDATE_PERIOD / 1000.0f,
+ PI_CONTROLLER_SATURATION_MIN_LIMIT,
+ PI_CONTROLLER_SATURATION_MAX_LIMIT);
+}
+
+AutomaticWingAlgorithm::~AutomaticWingAlgorithm() { delete (controller); }
+
+void AutomaticWingAlgorithm::step()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ if (modules.get<Sensors>()->getGPSLastSample().fix != 0)
+ {
+ // The PI calculated result
+ float result = algorithmStep(
+ modules.get<NASController>()->getNasState(),
+ modules.get<WindEstimation>()->getWindEstimationScheme());
+
+ // Actuate the result
+ if (result > 0)
+ {
+ // Activate the servo1 and reset servo2
+ modules.get<Actuators>()->setServoAngle(servo1, result);
+ modules.get<Actuators>()->setServoAngle(servo2, 0);
+ }
+ else
+ {
+ // Activate the servo2 and reset servo1
+ modules.get<Actuators>()->setServoAngle(servo1, 0);
+ modules.get<Actuators>()->setServoAngle(servo2, result * -1);
+ }
+
+ // Log the servo positions
+ {
+ miosix::Lock<FastMutex> l(mutex);
+
+ data.timestamp = TimestampTimer::getTimestamp();
+ data.servo1Angle =
+ modules.get<Actuators>()->getServoPosition(servo1);
+ data.servo2Angle =
+ modules.get<Actuators>()->getServoPosition(servo2);
+ SDlogger->log(data);
+ }
+ }
+ else
+ {
+ // If we loose fix we set both servo at 0
+ modules.get<Actuators>()->setServoAngle(servo1, 0);
+ modules.get<Actuators>()->setServoAngle(servo2, 0);
+ }
+}
+
+float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
+{
+ float result;
+ // For some algorithms the third component is needed!
+ Vector3f currentPosition(state.n, state.e, state.d);
+
+ Vector2f heading; // used for logging purposes
+
+ float targetAngle = guidance.calculateTargetAngle(currentPosition, heading);
+
+ Vector2f relativeVelocity(state.vn - windNED[0], state.ve - windNED[1]);
+
+ // Compute the angle of the current velocity
+ float velocityAngle;
+
+ // In case of a 0 north velocity i force the angle to 90
+ if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0)
+ {
+ velocityAngle = 0;
+ }
+ else if (relativeVelocity[1] == 0)
+ {
+ velocityAngle = (relativeVelocity[0] > 0 ? 1 : -1) * Constants::PI / 2;
+ }
+ else
+ {
+ velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
+ }
+
+ // Compute the angle difference
+ float error = angleDiff(targetAngle, velocityAngle);
+
+ // Call the PI with the just calculated error. The result is in RADIANS,
+ // if positive we activate one servo, if negative the other
+ result = controller->update(error);
+
+ // Convert the result from radians back to degrees
+ result = result * (180.f / Constants::PI);
+
+ // Flip the servo orientation
+ result *= -1;
+
+ // Logs the outputs
+ {
+ miosix::Lock<FastMutex> l(mutex);
+ data.targetX = heading[0];
+ data.targetY = heading[1];
+ data.targetAngle = targetAngle;
+ data.velocityAngle = velocityAngle;
+ data.error = error;
+ data.pidOutput = result;
+ }
+
+ return result;
+}
+
+float AutomaticWingAlgorithm::angleDiff(float a, float b)
+{
+ float diff = a - b;
+
+ // Angle difference
+ if (diff < -Constants::PI || Constants::PI < diff)
+ {
+ diff += Constants::PI;
+ bool positiveInput = diff > 0;
+
+ diff = diff - floor(diff / (2 * Constants::PI)) * (2 * Constants::PI);
+
+ // diff = fmod(diff, 2 * Constants::PI);
+ if (diff == 0 && positiveInput)
+ {
+ diff = 2 * Constants::PI;
+ }
+
+ diff -= Constants::PI;
+ }
+
+ return diff;
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.h b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..ffc00ec599c1834da9f6259d9a809f3a54ebf8df
--- /dev/null
+++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h
@@ -0,0 +1,97 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Wing/Guidance/GuidanceAlgorithm.h>
+#include <Payload/Wing/WingAlgorithm.h>
+#include <algorithms/NAS/NASState.h>
+#include <algorithms/PIController.h>
+#include <algorithms/ReferenceValues.h>
+
+#include <Eigen/Core>
+
+namespace Payload
+{
+class AutomaticWingAlgorithm : public WingAlgorithm
+{
+
+public:
+ /**
+ * @brief Construct a new Automatic Wing Algorithm object
+ *
+ * @param Kp Proportional value for PI controller
+ * @param Ki Integral value for PI controller
+ * @param servo1 The first servo
+ * @param servo2 The second servo
+ * @param guidance The algorithm used to compute the target yaw and the
+ * heading
+ */
+ AutomaticWingAlgorithm(float Kp, float Ki, ServosList servo1,
+ ServosList servo2, GuidanceAlgorithm& guidance);
+
+ /**
+ * @brief Destroy the Automatic Wing Algorithm object and the PI
+ */
+ ~AutomaticWingAlgorithm();
+
+protected:
+ // Guidance algorithm that sets the yaw.
+ GuidanceAlgorithm& guidance;
+
+ // PI controller tuned on the Kp and Ki passed through constructor
+ Boardcore::PIController* controller;
+
+ /**
+ * @brief Actual algorithm implementation, all parameters should be in NED
+ *
+ * @param state NAS current state
+ * @param targetNED Target North & East
+ * @param windNED Wind velocity North & East
+ */
+ float algorithmStep(Boardcore::NASState state, Eigen::Vector2f windNED);
+
+ /**
+ * @brief This method implements the automatic algorithm that will steer the
+ * parafoil according to its position and velocity. IN THIS METHOD THE
+ * GUIDANCE IS TRANSLATED
+ */
+ void step() override;
+
+ /**
+ * @brief Computes the difference between two angles
+ *
+ * @param a The first angle
+ * @param b The second angle
+ *
+ * @returns angle(a) - angle(b)
+ */
+ float angleDiff(float a, float b);
+
+ // Logging structure
+ WingAlgorithmData data;
+
+ /**
+ * @brief Mutex
+ */
+ miosix::FastMutex mutex;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/FileWingAlgorithm.cpp b/src/boards/Payload/Wing/FileWingAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7d4f8e7a18e9191b5fd55fa962ebb9febd8e31ff
--- /dev/null
+++ b/src/boards/Payload/Wing/FileWingAlgorithm.cpp
@@ -0,0 +1,68 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Wing/FileWingAlgorithm.h>
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+
+namespace Payload
+{
+std::istream& operator>>(std::istream& input, WingAlgorithmData& data)
+{
+ input >> data.timestamp;
+ input.ignore(1, ',');
+ input >> data.servo1Angle;
+ input.ignore(1, ',');
+ input >> data.servo2Angle;
+ return input;
+}
+
+FileWingAlgorithm::FileWingAlgorithm(ServosList servo1, ServosList servo2,
+ const char* filename)
+ : WingAlgorithm(servo1, servo2), parser(filename)
+{
+ setServo(servo1, servo2);
+}
+
+bool FileWingAlgorithm::init()
+{
+ // Returns a std::vector which contains
+ // all the csv parsed with the data structure in mind
+ steps = parser.collect();
+
+ // Return if the size collected is greater than 0
+ fileValid = steps.size() > 0;
+
+ // Communicate it via serial
+ if (fileValid)
+ {
+ LOG_INFO(logger, "File valid");
+ }
+
+ // Close the file
+ parser.close();
+
+ return fileValid;
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/FileWingAlgorithm.h b/src/boards/Payload/Wing/FileWingAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..50884d5d1cb84d470189a7375ecff157a5ebada3
--- /dev/null
+++ b/src/boards/Payload/Wing/FileWingAlgorithm.h
@@ -0,0 +1,91 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Payload/Wing/WingAlgorithm.h>
+#include <utils/CSVReader/CSVReader.h>
+
+/**
+ * @brief This class abstracts the concept of wing timestamp based
+ * algorithm. These algorithms are stored in files (formatted in csv).
+ * We use a CSV parser to properly parse the procedure and every step
+ * we check if it is time to advance and in case actuate the step with
+ * the two servos.
+ *
+ * Brief use case:
+ *
+ * Actuators::getInstance().enableServo(PARAFOIL_LEFT_SERVO);
+ * Actuators::getInstance().enableServo(PARAFOIL_RIGHT_SERVO);
+ *
+ * WingAlgorithm algorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, "Optional
+ * File") algorithm.init();
+ *
+ * //In case of a non valid file/empty string you can add the steps
+ * algorithm.addStep(WingAlgorithmData{timestamp, angle1, angle2});
+ *
+ * algorithm.begin();
+ *
+ * algorithm.update()...
+ *
+ * //End of algorithm
+ *
+ * algorithm.begin();
+ *
+ * algorithm.update()...
+ */
+
+namespace Payload
+{
+
+class FileWingAlgorithm : public WingAlgorithm
+{
+public:
+ /**
+ * @brief Construct a new Wing Algorithm object
+ *
+ * @param servo1 The first servo
+ * @param servo2 The second servo
+ * @param filename The csv file where all the operations are stored
+ */
+ FileWingAlgorithm(ServosList servo1, ServosList servo2,
+ const char* filename);
+
+ /**
+ * @brief This method parses the file and stores it into a std::vector
+ *
+ * @return true If the initialization finished correctly
+ * @return false If something went wrong
+ */
+ bool init() override;
+
+protected:
+ /**
+ * @brief CSV format file parser
+ */
+ Boardcore::CSVParser<WingAlgorithmData> parser;
+
+ /**
+ * @brief Indicates whether the current file of the algorithm is readable
+ */
+ bool fileValid = false;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..48b42619cc7e4376119b4cede8ffec75e8b2ee7d
--- /dev/null
+++ b/src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
@@ -0,0 +1,42 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul
+ *
+ * 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 <Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
+#include <math.h>
+
+namespace Payload
+{
+
+float ClosedLoopGuidanceAlgorithm::calculateTargetAngle(
+ const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
+{
+ heading[0] = targetNED[0] - currentPositionNED[0];
+ heading[1] = targetNED[1] - currentPositionNED[1];
+ return atan2(heading[1], heading[0]);
+}
+
+void ClosedLoopGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED)
+{
+ this->targetNED = targetNED;
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..5bc73969369f3a0b72bb0241a3affd5ee4a15c05
--- /dev/null
+++ b/src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h
@@ -0,0 +1,57 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul
+ *
+ * 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 <Payload/Wing/Guidance/GuidanceAlgorithm.h>
+
+#include <Eigen/Core>
+
+namespace Payload
+{
+
+/**
+ * This class is the implementation of the Simple Closed Loop guidance.
+ * It calculates the yaw between the current position and the target position by
+ * calculating the difference between the two vectors and the angle formed by
+ * the diff vector
+ */
+class ClosedLoopGuidanceAlgorithm : public GuidanceAlgorithm
+{
+ /**
+ * @brief This method calculates the yaw angle of the parafoil knowing
+ * the current position and the target position.
+ *
+ * @param[in] position the current NED position of the parafoil in [m]
+ * @param[in] target NED position of the target in [m]
+ * @param[out] heading Saves the heading vector for logging purposes
+ *
+ * @returns the yaw angle of the parafoil in [rad]
+ */
+ float calculateTargetAngle(const Eigen::Vector3f& currentPositionNED,
+ Eigen::Vector2f& heading) override;
+
+public:
+ void setPoints(Eigen::Vector2f targetNED);
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..66ee2bcce0faeb2d8dd7cf40835d892d8153638b
--- /dev/null
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
@@ -0,0 +1,150 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul
+ *
+ * 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 <Payload/Configs/WingConfig.h>
+#include <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
+
+#include <Eigen/Core>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Payload::WingConfig;
+
+namespace Payload
+{
+
+EarlyManeuversGuidanceAlgorithm::EarlyManeuversGuidanceAlgorithm()
+ : activeTarget(Target::EMC), targetAltitudeConfidence(0),
+ m2AltitudeConfidence(0), m1AltitudeConfidence(0),
+ emcAltitudeConfidence(0){};
+
+EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){};
+
+float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
+ const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
+{
+ using namespace Boardcore;
+
+ float altitude = abs(currentPositionNED[2]);
+
+ computeActiveTarget(altitude);
+
+ switch (activeTarget)
+ {
+ case Target::EMC:
+ heading[0] = EMC[0] - currentPositionNED[0];
+ heading[1] = EMC[1] - currentPositionNED[1];
+ break;
+ case Target::M1:
+ heading[0] = M1[0] - currentPositionNED[0];
+ heading[1] = M1[1] - currentPositionNED[1];
+ break;
+ case Target::M2:
+ heading[0] = M2[0] - currentPositionNED[0];
+ heading[1] = M2[1] - currentPositionNED[1];
+ break;
+ case Target::FINAL:
+ heading[0] = targetNED[0] - currentPositionNED[0];
+ heading[1] = targetNED[1] - currentPositionNED[1];
+ break;
+ }
+
+ return atan2(heading[1], heading[0]);
+}
+
+void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
+{
+ if (altitude <=
+ GUIDANCE_TARGET_ALTITUDE_THRESHOLD) // Altitude is low, head directly
+ // to target
+ {
+ targetAltitudeConfidence++;
+ }
+ else if (altitude <= GUIDANCE_M2_ALTITUDE_THRESHOLD) // Altitude is almost
+ // okay, go to M2
+ {
+ m2AltitudeConfidence++;
+ }
+ else if (altitude <=
+ GUIDANCE_M1_ALTITUDE_THRESHOLD) // Altitude is high, go to M1
+ {
+ m1AltitudeConfidence++;
+ }
+ else
+ {
+ emcAltitudeConfidence++; // Altitude is too high, head to the emc
+ }
+
+ switch (activeTarget)
+ {
+ case Target::EMC:
+ if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ {
+ activeTarget = Target::FINAL;
+ emcAltitudeConfidence = 0;
+ }
+ else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ {
+ activeTarget = Target::M2;
+ emcAltitudeConfidence = 0;
+ }
+ else if (m1AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ {
+ activeTarget = Target::M1;
+ emcAltitudeConfidence = 0;
+ }
+ break;
+ case Target::M1:
+ if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ {
+ activeTarget = Target::FINAL;
+ m1AltitudeConfidence = 0;
+ }
+ else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ {
+ activeTarget = Target::M2;
+ m1AltitudeConfidence = 0;
+ }
+ break;
+ case Target::M2:
+ if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ {
+ activeTarget = Target::FINAL;
+ m2AltitudeConfidence = 0;
+ }
+ break;
+ case Target::FINAL:
+ break;
+ }
+}
+
+void EarlyManeuversGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED,
+ Eigen::Vector2f EMC,
+ Eigen::Vector2f M1,
+ Eigen::Vector2f M2)
+{
+ this->targetNED = targetNED;
+ this->EMC = EMC;
+ this->M1 = M1;
+ this->M2 = M2;
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..30a432504dbf6404c0219f62d88c4490c8d0f4b0
--- /dev/null
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
@@ -0,0 +1,112 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul
+ *
+ * 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 <Payload/Wing/Guidance/GuidanceAlgorithm.h>
+
+#include <Eigen/Core>
+
+namespace Payload
+{
+
+/**
+ * This class is the implementation of the Simple Closed Loop guidance.
+ * It calculates the yaw between the current position and the target position by
+ * calculating the difference between the two vectors and the angle formed by
+ * the diff vector
+ *
+ * requires: WingControllers
+ */
+class EarlyManeuversGuidanceAlgorithm : public GuidanceAlgorithm
+{
+
+public:
+ /**
+ * @brief Constructor for the EM Algorithm
+ *
+ */
+ EarlyManeuversGuidanceAlgorithm();
+
+ virtual ~EarlyManeuversGuidanceAlgorithm();
+
+ /**
+ * @brief This method calculates the yaw angle of the parafoil knowing
+ * the current position and the target position.
+ *
+ * @param[in] currentPositionNED the current NED position of the parafoil in
+ * m
+ * @param[in] targetPositionNED NED position of the target in m
+ * @param[out] heading Saves the heading vector for logging purposes
+ *
+ * @returns the yaw angle of the parafoil in rad
+ *
+ */
+ float calculateTargetAngle(const Eigen::Vector3f& currentPositionNED,
+ Eigen::Vector2f& heading) override;
+
+ /**
+ * @brief Set Early Maneuvers points
+ *
+ * @param[in] EMC NED
+ * @param[in] M1 NED
+ * @param[in] M2 NED
+ *
+ */
+ void setPoints(Eigen::Vector2f targetNED, Eigen::Vector2f EMC,
+ Eigen::Vector2f M1, Eigen::Vector2f M2);
+
+private:
+ /** @brief Updates the class target
+ *
+ */
+ void computeActiveTarget(float altitude);
+
+ /**
+ * @brief Enumerates all the possible targets of the EM algorithm
+ */
+ enum class Target
+ {
+ EMC = 0,
+ M1,
+ M2,
+ FINAL
+ };
+
+ // Point we are currently poinying to
+ Target activeTarget;
+
+ // Eigen::Vector2f targetNED; // NED
+
+ Eigen::Vector2f EMC; // NED
+
+ Eigen::Vector2f M1; // NED
+
+ Eigen::Vector2f M2; // NED
+
+ unsigned int targetAltitudeConfidence;
+ unsigned int m2AltitudeConfidence;
+ unsigned int m1AltitudeConfidence;
+ unsigned int emcAltitudeConfidence;
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/Guidance/GuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/GuidanceAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..4086116c32af40f2f6f7b858ed41dd963a9cde94
--- /dev/null
+++ b/src/boards/Payload/Wing/Guidance/GuidanceAlgorithm.h
@@ -0,0 +1,61 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul
+ *
+ * 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 <Eigen/Core>
+
+namespace Payload
+{
+
+/**
+ * This class acts as an interface for a generic guidance algorithm that is used
+ * by the Automatic Wing Algorithm.
+ */
+class GuidanceAlgorithm
+{
+public:
+ /**
+ * @brief This method must calculate the yaw angle of the parafoil knowing
+ * the current position and the target position without changing the vectors
+ * passed as arguments.
+ *
+ * @note the args are const references to reduce access time by avoiding
+ * copying objects that will be read-only.
+ *
+ * @param[in] position the current NED position of the parafoil in [m]
+ * @param[in] target NED position of the target in [m]
+ * @param[out] heading The current heading, it is used for logging purposes
+ *
+ * @returns the yaw angle of the parafoil in [rad]
+ */
+ virtual float calculateTargetAngle(
+ const Eigen::Vector3f& currentPositionNED,
+ Eigen::Vector2f& heading) = 0;
+
+ Eigen::Vector2f getTargetNED() { return targetNED; }
+
+protected:
+ Eigen::Vector2f targetNED{0, 0}; // NED
+};
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/WingAlgorithm.cpp b/src/boards/Payload/Wing/WingAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9bb3049916c59a6508ea7dcb0832b9ec3f51a2de
--- /dev/null
+++ b/src/boards/Payload/Wing/WingAlgorithm.cpp
@@ -0,0 +1,122 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 <Payload/Configs/WingConfig.h>
+#include <Payload/Wing/WingAlgorithm.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Boardcore;
+using namespace Payload::WingConfig;
+using namespace Common;
+namespace Payload
+{
+WingAlgorithm::WingAlgorithm(ServosList servo1, ServosList servo2)
+{
+ this->servo1 = servo1;
+ this->servo2 = servo2;
+ stepIndex = 0;
+ // Create the vector for algorithm data
+ steps = std::vector<WingAlgorithmData>();
+}
+
+bool WingAlgorithm::init()
+{
+ return true; // In this case the init is always true
+}
+
+void WingAlgorithm::setServo(ServosList servo1, ServosList servo2)
+{
+ this->servo1 = servo1;
+ this->servo2 = servo2;
+}
+
+void WingAlgorithm::addStep(WingAlgorithmData step) { steps.push_back(step); }
+
+void WingAlgorithm::begin()
+{
+ running = true;
+ shouldReset = true;
+
+ // Set the reference timestamp
+ timeStart = TimestampTimer::getTimestamp();
+}
+
+void WingAlgorithm::end()
+{
+ running = false;
+
+ // Set the reference timestamp to 0
+ timeStart = 0;
+}
+
+void WingAlgorithm::step()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ uint64_t currentTimestamp = TimestampTimer::getTimestamp();
+
+ if (shouldReset)
+ {
+ // If the algorithm has been stopped
+ // i want to start from the beginning
+ stepIndex = 0;
+ shouldReset = false;
+ }
+
+ if (stepIndex >= steps.size())
+ {
+ LOG_INFO(logger, "Algorithm end {:d} >= {:d}", stepIndex, steps.size());
+ // End the procedure so it won't be executed
+ // Set the index to 0 in case of another future execution
+ stepIndex = 0;
+ // Terminate here
+ // EventBroker::getInstance().post(ALGORITHM_ENDED, TOPIC_ALGOS);
+ return;
+ }
+
+ if (currentTimestamp - timeStart >= steps[stepIndex].timestamp)
+ {
+ // I need to execute the current step
+ modules.get<Actuators>()->setServoAngle(servo1,
+ steps[stepIndex].servo1Angle);
+ modules.get<Actuators>()->setServoAngle(servo2,
+ steps[stepIndex].servo2Angle);
+
+ // Log the data setting the timestamp to the absolute one
+ WingAlgorithmData data;
+ data.timestamp = TimestampTimer::getTimestamp();
+ data.servo1Angle = steps[stepIndex].servo1Angle;
+ data.servo2Angle = steps[stepIndex].servo2Angle;
+
+ // After copy i can log the actual step
+ SDlogger->log(data);
+
+ // finally increment the stepIndex
+ stepIndex++;
+
+ LOG_INFO(logger, "Step");
+ }
+}
+
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/WingAlgorithm.h b/src/boards/Payload/Wing/WingAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..4334300d8c0ee5f186ab91f2c8431c892dc691e4
--- /dev/null
+++ b/src/boards/Payload/Wing/WingAlgorithm.h
@@ -0,0 +1,107 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli
+ *
+ * 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 <Payload/Actuators/Actuators.h>
+#include <Payload/Wing/WingAlgorithmData.h>
+#include <algorithms/Algorithm.h>
+#include <diagnostic/PrintLogger.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+namespace Payload
+{
+class WingAlgorithm : public Boardcore::Algorithm
+{
+public:
+ /**
+ * @brief Construct a new Wing Algorithm object
+ *
+ * @param servo1 The first servo
+ * @param servo2 The second servo
+ */
+ WingAlgorithm(ServosList servo1, ServosList servo2);
+
+ /**
+ * @brief Method to initialize the algorithm
+ *
+ * @return true If the init process goes well
+ * @return false If the init process doesn't go well
+ */
+ bool init() override;
+
+ /**
+ * @brief Set the Servos object
+ *
+ * @param servo1 The first algorithm servo
+ * @param servo2 The second algorithm servo
+ */
+ void setServo(ServosList servo1, ServosList servo2);
+
+ /**
+ * @brief Add a step to the algorithm sequence
+ *
+ * @param step The step to add
+ */
+ void addStep(WingAlgorithmData step);
+
+ /**
+ * @brief This sets the reference timestamp for the algorithm
+ */
+ void begin();
+
+ /**
+ * @brief This method disables the algorithm
+ */
+ void end();
+ /**
+ * @brief This method implements the algorithm step based on the current
+ * timestamp
+ */
+ void step() override;
+
+protected:
+ // The actuators
+ ServosList servo1, servo2;
+
+ // Reference timestamp for relative algorithm timestamps
+ uint64_t timeStart;
+
+ // Procedure array to memorize all the steps needed to perform the algorithm
+ std::vector<WingAlgorithmData> steps;
+
+ // PrintLogger
+ Boardcore::PrintLogger logger =
+ Boardcore::Logging::getLogger("WingAlgorithm");
+
+ // SD logger
+ Boardcore::Logger* SDlogger = &Boardcore::Logger::getInstance();
+
+ // This boolean is used to understand when to reset
+ // the index where the algorithm has stopped.
+ // In case of end call, we want to be able to perform
+ // another time this algorithm starting from 0
+ std::atomic<bool> shouldReset;
+
+ // Variable to remember what step that has to be done
+ unsigned int stepIndex;
+};
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/WingAlgorithmData.h b/src/boards/Payload/Wing/WingAlgorithmData.h
new file mode 100644
index 0000000000000000000000000000000000000000..38e20628c332a5965e0a50e62381d6df61fc4ab8
--- /dev/null
+++ b/src/boards/Payload/Wing/WingAlgorithmData.h
@@ -0,0 +1,57 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 Payload
+{
+/**
+ * This class represents the algorithm data structure that needs to be logged
+ * into the onboard SD card. It has the timestamp(absolute) and the servo
+ * position set by the selected algorithm
+ */
+struct WingAlgorithmData
+{
+ uint64_t timestamp; // First timestamp is 0 (in microseconds)
+ float servo1Angle; // degrees
+ float servo2Angle; // degrees
+ float targetAngle; // radians (automatic only)
+ float velocityAngle; // radians
+ float targetX; // NED (only automatic algorithm)
+ float targetY; // NED (only automatic algorithm)
+ float error;
+ float pidOutput;
+
+ static std::string header()
+ {
+ return "WingAlgorithmTimestamp,servo1Angle,servo2Angle,targetAngle,"
+ "velocityAngle,targetX,targetY,error,pidOutput\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << servo1Angle << "," << servo2Angle << ","
+ << targetAngle << "," << velocityAngle << "," << targetX << ","
+ << targetY << "," << error << "," << pidOutput << "\n";
+ }
+};
+} // namespace Payload
diff --git a/src/boards/Payload/Wing/WingTargetPositionData.h b/src/boards/Payload/Wing/WingTargetPositionData.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1cab4dbf456c366d1a301487aacfd1b11a0099c
--- /dev/null
+++ b/src/boards/Payload/Wing/WingTargetPositionData.h
@@ -0,0 +1,58 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Radu Raul
+ *
+ * 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 <ostream>
+#include <string>
+
+namespace Payload
+{
+
+struct WingTargetPositionData
+{
+ float receivedLat;
+ float receivedLon;
+ float targetN;
+ float targetE;
+ float emcN;
+ float emcE;
+ float m1N;
+ float m1E;
+ float m2N;
+ float m2E;
+
+ static std::string header()
+ {
+ return "receivedLat, receivedLon, "
+ "targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << receivedLat << "," << receivedLon << "," << targetN << ","
+ << targetE << "," << emcN << "," << emcE << "," << m1N << "," << m1E
+ << "," << m2N << "," << m2E << "\n";
+ }
+};
+
+} // namespace Payload
diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b174e140bdca613a43e8da65d09215c41521cc5e
--- /dev/null
+++ b/src/entrypoints/Payload/payload-entry.cpp
@@ -0,0 +1,318 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+// #define DEFAULT_STDOUT_LOG_LEVEL LOGL_WARNING
+#include <Payload/Actuators/Actuators.h>
+#include <Payload/AltitudeTrigger/AltitudeTrigger.h>
+#include <Payload/BoardScheduler.h>
+#include <Payload/Buses.h>
+#include <Payload/CanHandler/CanHandler.h>
+#include <Payload/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Payload/PinHandler/PinHandler.h>
+#include <Payload/Radio/Radio.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/StateMachines/WingController/WingController.h>
+#include <Payload/TMRepository/TMRepository.h>
+#include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h>
+#include <Payload/WindEstimationScheme/WindEstimation.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <diagnostic/PrintLogger.h>
+#include <diagnostic/StackLogger.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <events/EventData.h>
+#include <events/utils/EventSniffer.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Boardcore;
+using namespace Payload;
+using namespace Common;
+
+int main()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+
+ // Overall status, if at some point it becomes false, there is a problem
+ // somewhere
+ bool initResult = true;
+ PrintLogger logger = Logging::getLogger("Payload");
+
+ // Scheduler
+ BoardScheduler* scheduler = new BoardScheduler();
+
+ // Nas priority (Max priority)
+ NASController* nas =
+ new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+
+ // Sensors priority (MAX - 1)
+ Sensors* sensors =
+ new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
+
+ // Other critical components (Max - 2)
+ Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+ AltitudeTrigger* altTrigger =
+ new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+ WingController* wingController =
+ new WingController(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+ VerticalVelocityTrigger* verticalVelocityTrigger =
+ new VerticalVelocityTrigger(
+ scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+ WindEstimation* windEstimation =
+ new WindEstimation(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+ CanHandler* canHandler =
+ new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+
+ // Non critical components (Max - 3)
+ // Actuators is considered non-critical since the scheduler is only used for
+ // the led and buzzer tasks
+ Actuators* actuators =
+ new Actuators(scheduler->getScheduler(miosix::PRIORITY_MAX - 3));
+ FlightStatsRecorder* statesRecorder = new FlightStatsRecorder(
+ scheduler->getScheduler(miosix::PRIORITY_MAX - 3));
+
+ // Components without a scheduler
+ TMRepository* tmRepo = new TMRepository();
+ FlightModeManager* fmm = new FlightModeManager();
+ Buses* buses = new Buses();
+ PinHandler* pinHandler = new PinHandler();
+
+ // Insert modules
+ if (!modules.insert<BoardScheduler>(scheduler))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Board Scheduler module");
+ }
+
+ if (!modules.insert<Buses>(buses))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Buses module");
+ }
+
+ if (!modules.insert<Sensors>(sensors))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Sensor module");
+ }
+
+ if (!modules.insert<NASController>(nas))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the NAS module");
+ }
+
+ if (!modules.insert<FlightModeManager>(fmm))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the FMM module");
+ }
+ if (!modules.insert<Radio>(radio))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Radio module");
+ }
+
+ if (!modules.insert<TMRepository>(tmRepo))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the TMRepository module");
+ }
+
+ if (!modules.insert<Actuators>(actuators))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Actuators module");
+ }
+
+ if (!modules.insert<AltitudeTrigger>(altTrigger))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Altitude Trigger module");
+ }
+
+ if (!modules.insert<WingController>(wingController))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the WingController module");
+ }
+
+ if (!modules.insert<WindEstimation>(windEstimation))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the WindEstimation module");
+ }
+
+ if (!modules.insert<PinHandler>(pinHandler))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the PinHandler module");
+ }
+
+ if (!modules.insert<VerticalVelocityTrigger>(verticalVelocityTrigger))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the VerticalVelocityTrigger module");
+ }
+
+ if (!modules.insert<CanHandler>(canHandler))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the CanHandler module");
+ }
+
+ if (!modules.insert<FlightStatsRecorder>(statesRecorder))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the FlightStatsRecorder module");
+ }
+
+ // Start modules
+ if (!Logger::getInstance().testSDCard())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error testing the Logger module");
+ }
+
+ if (!EventBroker::getInstance().start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the EventBroker module");
+ }
+
+ if (!modules.get<Sensors>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the Sensors module");
+ }
+
+ if (!modules.get<NASController>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the NAS module");
+ }
+
+ if (!modules.get<FlightModeManager>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the FMM module");
+ }
+
+ if (!modules.get<Radio>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the Radio module");
+ }
+
+ if (!modules.get<Actuators>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the Actuators module");
+ }
+
+ if (!modules.get<AltitudeTrigger>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the AltitudeTrigger module");
+ }
+
+ if (!modules.get<WingController>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the WingController module");
+ }
+
+ if (!modules.get<WindEstimation>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the WindEstimation module");
+ }
+
+ if (!modules.get<PinHandler>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the PinHandler module");
+ }
+
+ if (!modules.get<VerticalVelocityTrigger>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the VerticalVelocityTrigger module");
+ }
+
+ if (!modules.get<CanHandler>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the CanHandler module");
+ }
+
+ if (!modules.get<FlightStatsRecorder>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the FlightStatsRecorder module");
+ }
+
+ if (!modules.get<BoardScheduler>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the Board Scheduler module");
+ }
+
+ // Log all the events
+ EventSniffer sniffer(
+ EventBroker::getInstance(), TOPICS_LIST,
+ [](uint8_t event, uint8_t topic)
+ {
+ EventData ev{TimestampTimer::getTimestamp(), event, topic};
+ Logger::getInstance().log(ev);
+ });
+
+ // Check the init result and launch an event
+ if (initResult)
+ {
+ // Post OK
+ EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+
+ // Set the LED status
+ miosix::led1On();
+ }
+ else
+ {
+ EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
+ LOG_ERR(logger, "Failed to initialize");
+ }
+
+ // Periodic statistics
+ while (true)
+ {
+ Thread::sleep(1000);
+ Logger::getInstance().log(CpuMeter::getCpuStats());
+ CpuMeter::resetCpuStats();
+ StackLogger::getInstance().log();
+ }
+
+ return 0;
+}
diff --git a/src/tests/Payload/payload-test-VerticalVelocityTrigger.cpp b/src/tests/Payload/payload-test-VerticalVelocityTrigger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dc7ac5df34b09bf744f135f145db216e2183153
--- /dev/null
+++ b/src/tests/Payload/payload-test-VerticalVelocityTrigger.cpp
@@ -0,0 +1,166 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul
+ *
+ * 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 <Payload/BoardScheduler.h>
+#include <Payload/Configs/FailSafeConfig.h>
+#include <Payload/NASControllerMock/NASControllerMock.h>
+#include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h>
+#include <events/EventBroker.h>
+#include <miosix.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Boardcore;
+using namespace Payload;
+
+// Initial velocity for the Vertical Velocity Trigger
+constexpr float MOCK_INITIAL_VERTICAL_VELOCITY = -30;
+constexpr float MOCK_VELOCITY_DECELERATION = 0.5; // m/s^2
+
+/**
+ * This class mocks the NASController class since for this test we cannot start
+ * the NASController as it needs the sensor reading and we cannot test the
+ * velocity trigger unless we launch the rocket
+ */
+class NASMock : public NASController
+{
+public:
+ /**
+ * Default constructor for NASMock
+ * It initializes the mockedVerticalSpeed
+ */
+ NASMock() : mockedVerticalSpeed(MOCK_INITIAL_VERTICAL_VELOCITY) {}
+
+ /**
+ * Default destructor for NASMock
+ */
+ ~NASMock() {}
+
+ /**
+ * This method mocks the NASController::getNasState() and returns a
+ * NASState with a personalized vertical velocity that starts at
+ * NASMock::MOCK_INITIAL_VERTICAL_VELOCITY and decreases by 0.5 m/s^2
+ *
+ * @return NASState object that contains the vertical vector in NED
+ * orientation
+ */
+ NASState getNasState() override
+ {
+ mockedVerticalSpeed +=
+ MOCK_VELOCITY_DECELERATION *
+ ((float)FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD /
+ 1000.f);
+ NASState n = NASState();
+ n.vd = mockedVerticalSpeed;
+ return n;
+ }
+
+private:
+ // Keeps trace of the vertical velocity in NED orientation
+ float mockedVerticalSpeed;
+};
+
+/**
+ * This function halts the board
+ */
+void halt()
+{
+ while (1)
+ {
+ }
+}
+
+/**
+ * This function calculates the maximum wait time needed by the Vertical
+ * Velocity Trigger to trigger
+ */
+float calculateMaxWaitTime()
+{
+ float confidenceTime =
+ ((float)FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_CONFIDENCE *
+ (float)FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD) /
+ 1000.f;
+ float targetReachTime = (-MOCK_INITIAL_VERTICAL_VELOCITY -
+ FailSafe::FAILSAFE_VERTICAL_VELOCITY_THRESHOLD) /
+ MOCK_VELOCITY_DECELERATION;
+ TRACE("Target reach time: %f [s]\n", targetReachTime);
+ TRACE("Confidence time: %f [s]\n", confidenceTime);
+
+ return confidenceTime + targetReachTime + 1;
+}
+
+int main()
+{
+
+ // Insert the modules
+ ModuleManager::getInstance().insert<BoardScheduler>(new BoardScheduler());
+ ModuleManager::getInstance().insert<NASController>(new NASMock());
+ ModuleManager::getInstance().insert<VerticalVelocityTrigger>(
+ new VerticalVelocityTrigger());
+
+ // start the scheduler
+ TRACE("Starting the board scheduler\n");
+ if (!ModuleManager::getInstance().get<BoardScheduler>()->start())
+ {
+ TRACE("Error starting the General Purpose Scheduler\n");
+ halt();
+ }
+
+ // start the modules
+ if (!ModuleManager::getInstance().get<NASController>()->start())
+ {
+ TRACE("Error starting the NAS Controller\n");
+ halt();
+ }
+ if (!ModuleManager::getInstance()
+ .get<VerticalVelocityTrigger>()
+ ->startModule())
+ {
+ TRACE("Error starting the Vertical Velocity Trigger\n");
+ halt();
+ }
+
+ ModuleManager::getInstance().get<VerticalVelocityTrigger>()->enable();
+
+ TRACE("Starting... \n");
+ // wait for the trigger
+ int count = 0;
+ int maxWaitTime = calculateMaxWaitTime() * 100;
+ while (
+ ModuleManager::getInstance().get<VerticalVelocityTrigger>()->isActive())
+ {
+ Thread::sleep(100);
+ count++;
+
+ if (count > maxWaitTime)
+ {
+ TRACE("Vertical Velocity Trigger not working properly\n");
+ break;
+ }
+ }
+ if (count < maxWaitTime)
+ {
+ TRACE("Vertical Velocity Triggered \n");
+ }
+
+ halt();
+}