From 3394c995898c12f116d0a0f1d0c6178c17aa435e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Sun, 8 Sep 2024 21:22:42 +0200
Subject: [PATCH] [Payload][WingController] General improvements for code
reliability and readibility
---
src/boards/Payload/BoardScheduler.h | 5 +
src/boards/Payload/Configs/WingConfig.h | 72 ++-
.../WingController/WingController.cpp | 513 ++++++++++--------
.../WingController/WingController.h | 147 +++--
.../WingController/WingControllerData.h | 26 +-
.../Payload/Wing/AutomaticWingAlgorithm.cpp | 21 +-
.../Payload/Wing/AutomaticWingAlgorithm.h | 7 +-
.../EarlyManeuverGuidanceAlgorithm.cpp | 25 +-
src/boards/Payload/Wing/WingAlgorithmData.h | 18 +-
.../Payload/Wing/WingTargetPositionData.h | 30 +-
src/scripts/logdecoder/Payload/logdecoder.cpp | 1 +
11 files changed, 470 insertions(+), 395 deletions(-)
diff --git a/src/boards/Payload/BoardScheduler.h b/src/boards/Payload/BoardScheduler.h
index d10a4de25..1340b8992 100644
--- a/src/boards/Payload/BoardScheduler.h
+++ b/src/boards/Payload/BoardScheduler.h
@@ -74,6 +74,11 @@ public:
return Priority::MEDIUM;
}
+ static Priority::PriorityLevel wingControllerPriority()
+ {
+ return Priority::MEDIUM;
+ }
+
/**
* @brief Starts all the schedulers
*/
diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index 3a28c4609..91c0b50b2 100644
--- a/src/boards/Payload/Configs/WingConfig.h
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -25,6 +25,8 @@
#include <units/Frequency.h>
#include <utils/Constants.h>
+#include <chrono>
+
namespace Payload
{
namespace Config
@@ -32,45 +34,57 @@ namespace Config
namespace Wing
{
-// Algorithm configuration
-#if defined(CLOSED_LOOP)
-constexpr int SELECTED_ALGORITHM = 0;
-#elif EARLY_MANEUVER
-constexpr int SELECTED_ALGORITHM = 1;
-#elif SEQUENCE
-constexpr int SELECTED_ALGORITHM = 2;
-#elif ROTATION
-constexpr int SELECTED_ALGORITHM = 3;
-#else
-constexpr int SELECTED_ALGORITHM = 0;
-#endif
+/* linter off */ using namespace std::chrono_literals;
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/**
+ * @brief The available algorithms for the wing controller.
+ */
+enum class AlgorithmId : size_t
+{
+ EARLY_MANEUVER = 0,
+ CLOSED_LOOP,
+ SEQUENCE, ///< A predefined sequence of maneuvers
+ ROTATION, ///< A sequence of maneuvers to rotate the wing
+ LAST, ///< Used to count the number of algorithms
+};
+
+namespace Default
+{
+// TODO: Verify the default target coordinates for all sites
#if defined(EUROC)
-constexpr float DEFAULT_TARGET_LAT = 39.389733;
-constexpr float DEFAULT_TARGET_LON = -8.288992;
+constexpr auto TARGET_LAT = 39.389733f;
+constexpr auto TARGET_LON = -8.288992f;
#elif defined(ROCCARASO)
-constexpr float DEFAULT_TARGET_LAT = 41.8089005;
-constexpr float DEFAULT_TARGET_LON = 14.0546716;
+constexpr auto TARGET_LAT = 41.809216;
+constexpr auto TARGET_LON = 14.055310;
#else // Milan
-constexpr float DEFAULT_TARGET_LAT = 45.5010679;
-constexpr float DEFAULT_TARGET_LON = 9.1563769;
+constexpr auto TARGET_LAT = 45.5010679f;
+constexpr auto TARGET_LON = 9.1563769f;
#endif
-constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms]
+constexpr auto ALGORITHM = AlgorithmId::EARLY_MANEUVER;
+} // namespace Default
-constexpr int WING_UPDATE_PERIOD = 1000; // [ms]
+constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15s;
+constexpr auto UPDATE_RATE = 1_hz;
-constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = Boardcore::Constants::PI;
-constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
+namespace PI
+{
+constexpr auto SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
+constexpr auto SATURATION_MAX_LIMIT = Boardcore::Constants::PI;
-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]
+constexpr auto KP = 0.9f;
+constexpr auto KI = 0.05f;
+} // namespace PI
-// TODO check this parameter preflight
-constexpr float KP = 0.9;
-constexpr float KI = 0.05;
+namespace Guidance
+{
+constexpr auto CONFIDENCE = 15; // [samples]
+constexpr auto M1_ALTITUDE_THRESHOLD = 250; // [m]
+constexpr auto M2_ALTITUDE_THRESHOLD = 150; // [m]
+constexpr auto TARGET_ALTITUDE_THRESHOLD = 50; // [m]
+} // namespace Guidance
constexpr float TWIRL_RADIUS = 0.5; // [%]
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index de80e13cd..13d02817f 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Federico Mandelli, Angelo Prete
+ * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -51,37 +51,15 @@ namespace Payload
{
WingController::WingController()
- : HSM(&WingController::state_idle), running(false), selectedAlgorithm(0)
+ : HSM(&WingController::Idle, miosix::STACK_DEFAULT_FOR_PTHREAD,
+ BoardScheduler::wingControllerPriority())
{
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
EventBroker::getInstance().subscribe(this, TOPIC_DPL);
EventBroker::getInstance().subscribe(this, TOPIC_WING);
- this->targetPositionGEO = {DEFAULT_TARGET_LAT, DEFAULT_TARGET_LON};
// Instantiate the algorithms
- addAlgorithms();
-}
-
-void WingController::inject(DependencyInjector& injector)
-{
- for (auto& algorithm : algorithms)
- {
- algorithm->inject(injector);
- }
- Super::inject(injector);
-}
-
-bool WingController::start()
-{
- auto& scheduler = getModule<BoardScheduler>()->wingController();
- bool success = true;
-
- success &= std::all_of(algorithms.begin(), algorithms.end(),
- [](auto& algorithm) { return algorithm->init(); });
-
- return success &&
- scheduler.addTask([this] { update(); }, WING_UPDATE_PERIOD) &&
- HSM::start();
+ loadAlgorithms();
}
WingController::~WingController()
@@ -89,21 +67,16 @@ 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)
+State WingController::Idle(const Boardcore::Event& event)
{
switch (event)
{
case EV_ENTRY:
{
- logStatus(WingControllerState::IDLE);
+ updateState(WingControllerState::IDLE);
return HANDLED;
}
+
case FLIGHT_WING_DESCENT:
{
auto nasState = getModule<NASController>()->getNasState();
@@ -113,12 +86,14 @@ State WingController::state_idle(const Boardcore::Event& event)
getModule<FlightStatsRecorder>()->deploymentDetected(
TimestampTimer::getTimestamp(), altitude);
- return transition(&WingController::state_flying);
+ return transition(&WingController::Flying);
}
+
case EV_EMPTY:
{
return tranSuper(&WingController::state_top);
}
+
default:
{
return UNHANDLED;
@@ -126,31 +101,35 @@ State WingController::state_idle(const Boardcore::Event& event)
}
}
-State WingController::state_flying(const Event& event)
+State WingController::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);
+ return transition(&WingController::FlyingCalibration);
}
+
case FLIGHT_LANDING_DETECTED:
{
- return transition(&WingController::state_on_ground);
+ return transition(&WingController::OnGround);
}
+
default:
{
return UNHANDLED;
@@ -158,7 +137,7 @@ State WingController::state_flying(const Event& event)
}
}
-State WingController::state_calibration(const Boardcore::Event& event)
+State WingController::FlyingCalibration(const Boardcore::Event& event)
{
static uint16_t calibrationTimeoutEventId;
@@ -166,89 +145,97 @@ State WingController::state_calibration(const Boardcore::Event& event)
{
case EV_ENTRY: // starts twirling and calibration wes
{
- logStatus(WingControllerState::CALIBRATION);
+ updateState(WingControllerState::FLYING_CALIBRATION);
- flare();
+ flareWing();
calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
DPL_SERVO_ACTUATION_DETECTED, TOPIC_DPL, 2000);
return HANDLED;
}
+
case EV_EXIT:
{
EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId);
return HANDLED;
}
+
case EV_EMPTY:
{
- return tranSuper(&WingController::state_flying);
+ return tranSuper(&WingController::Flying);
}
+
case DPL_SERVO_ACTUATION_DETECTED:
{
- reset();
+ resetWing();
calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
DPL_WIGGLE, TOPIC_DPL, 1000);
return HANDLED;
}
+
case DPL_WIGGLE:
{
- flare();
+ flareWing();
calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
DPL_NC_OPEN, TOPIC_DPL, 2000);
return HANDLED;
}
+
case DPL_NC_OPEN:
{
- reset();
+ resetWing();
calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
DPL_WES_CAL_DONE, TOPIC_DPL,
milliseconds{Config::WES::ROTATION_PERIOD}.count());
getModule<WindEstimation>()->startWindEstimationSchemeCalibration();
- twirl();
+ twirlWing();
return HANDLED;
}
+
case DPL_WES_CAL_DONE:
{
- reset();
+ resetWing();
- return transition(&WingController::state_controlled_descent);
+ return transition(&WingController::FlyingControlledDescent);
}
+
default:
{
return UNHANDLED;
}
}
}
-State WingController::state_controlled_descent(const Boardcore::Event& event)
+State WingController::FlyingControlledDescent(const Boardcore::Event& event)
{
switch (event)
{
- case EV_ENTRY: // start automatic algorithm
+ case EV_ENTRY:
{
- logStatus(WingControllerState::ALGORITHM_CONTROLLED);
- setEarlyManeuverPoints(
- convertTargetPositionToNED(targetPositionGEO),
- {getModule<NASController>()->getNasState().n,
- getModule<NASController>()->getNasState().e});
+ updateState(WingControllerState::FLYING_CONTROLLED_DESCENT);
+
startAlgorithm();
return HANDLED;
}
+
case EV_EMPTY:
{
- return tranSuper(&WingController::state_flying);
+ return tranSuper(&WingController::Flying);
}
+
case WING_ALGORITHM_ENDED:
{
- return transition(&WingController::state_on_ground);
+ return transition(&WingController::OnGround);
}
+
case EV_EXIT:
{
return HANDLED;
}
+
default:
{
return UNHANDLED;
@@ -256,13 +243,13 @@ State WingController::state_controlled_descent(const Boardcore::Event& event)
}
}
-State WingController::state_on_ground(const Boardcore::Event& event)
+State WingController::OnGround(const Boardcore::Event& event)
{
switch (event)
{
case EV_ENTRY:
{
- logStatus(WingControllerState::ON_GROUND);
+ updateState(WingControllerState::ON_GROUND);
getModule<WindEstimation>()->stopWindEstimationScheme();
getModule<WindEstimation>()->stopWindEstimationSchemeCalibration();
@@ -274,14 +261,17 @@ State WingController::state_on_ground(const Boardcore::Event& event)
return HANDLED;
}
+
case EV_EXIT:
{
return HANDLED;
}
+
case EV_EMPTY:
{
return tranSuper(&WingController::state_top);
}
+
default:
{
return UNHANDLED;
@@ -289,184 +279,227 @@ State WingController::state_on_ground(const Boardcore::Event& event)
}
}
-bool WingController::addAlgorithms()
+void WingController::inject(DependencyInjector& injector)
{
- WingAlgorithm* algorithm;
- WingAlgorithmData step;
-
- bool result = false;
-
- // Algorithm 0
- algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
- PARAFOIL_RIGHT_SERVO, clGuidance);
- result = algorithm->init();
- algorithms.push_back(algorithm);
- // Algorithm 1
- algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
- PARAFOIL_RIGHT_SERVO, emGuidance);
- result &= algorithm->init();
- algorithms.push_back(algorithm);
-
- // Algorithm 2
- algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
- step.servo1Angle = 0;
- step.servo2Angle = 120;
- step.timestamp = 0;
- algorithm->addStep(step);
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- step.timestamp += 1000 * WING_STRAIGHT_FLIGHT_TIMEOUT;
- algorithm->addStep(step);
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- step.timestamp += WING_STRAIGHT_FLIGHT_TIMEOUT;
- algorithm->addStep(step);
- result &= algorithm->init();
- // Add the algorithm to the vector
- algorithms.push_back(algorithm);
-
- // Algorithm 3 (rotation)
- algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
- step.timestamp = 0;
- step.servo1Angle = LeftServo::ROTATION / 2;
- step.servo2Angle = 0;
- algorithm->addStep(step);
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = 0;
- step.servo2Angle = RightServo::ROTATION / 2;
- algorithm->addStep(step);
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = LeftServo::ROTATION;
- step.servo2Angle = RightServo::ROTATION;
- algorithm->addStep(step);
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = 0;
- step.servo2Angle = RightServo::ROTATION;
- algorithm->addStep(step);
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
-
- result &= algorithm->init();
- algorithms.push_back(algorithm);
-
- selectAlgorithm(SELECTED_ALGORITHM);
-
- return result;
+ for (auto& algorithm : algorithms)
+ {
+ algorithm->inject(injector);
+ }
+ Super::inject(injector);
}
-bool WingController::selectAlgorithm(unsigned int index)
+bool WingController::start()
{
- bool success = false;
- // We change the selected algorithm only if we are in IDLE
- if (getStatus().state == WingControllerState::IDLE)
+ auto& scheduler = getModule<BoardScheduler>()->wingController();
+
+ bool algoStarted =
+ std::all_of(algorithms.begin(), algorithms.end(),
+ [](auto& algorithm) { return algorithm->init(); });
+
+ if (!algoStarted)
{
- 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;
- }
+ LOG_ERR(logger, "Failed to initialize wing algorithms");
+ return false;
+ }
+
+ auto updateTask = scheduler.addTask([this] { update(); }, UPDATE_RATE);
+
+ if (updateTask == 0)
+ {
+ LOG_ERR(logger, "Failed to add wing controller update task");
+ return false;
+ }
+
+ if (!HSM::start())
+ {
+ LOG_ERR(logger, "Failed to start WingController HSM active object");
+ return false;
}
- return success;
+
+ started = true;
+ return true;
}
-void WingController::startAlgorithm()
+bool WingController::isStarted() { return started; }
+
+WingControllerState WingController::getState() { return state; }
+
+bool WingController::setTargetPosition(Eigen::Vector2f targetPositionGEO)
{
- // If the selected algorithm is valid --> also the
- // algorithms array is not empty i start the whole thing
- if (selectedAlgorithm < algorithms.size())
+ // Allow changing the target position in the IDLE state only
+ if (state != WingControllerState::IDLE)
{
- running = true;
+ return false;
+ }
- // Begin the selected algorithm
- algorithms[selectedAlgorithm]->begin();
+ this->targetPositionGEO = targetPositionGEO;
- LOG_INFO(logger, "Wing algorithm started");
- }
+ auto data = WingTargetPositionData{
+ .targetLat = targetPositionGEO[0], .targetLon = targetPositionGEO[1],
+ // TODO: populate early maneuver points
+ };
+ Logger::getInstance().log(data);
+
+ return true;
}
-void WingController::stopAlgorithm()
+bool WingController::selectAlgorithm(uint8_t index)
{
- if (running)
+ // Allow changing the algorithm in the IDLE state only
+ if (state != WingControllerState::IDLE)
{
- // Set running to false
- running = false;
- // Stop the algorithm if selected
- if (selectedAlgorithm < algorithms.size())
+ return false;
+ }
+
+ switch (index)
+ {
+ case static_cast<uint8_t>(AlgorithmId::EARLY_MANEUVER):
+ case static_cast<uint8_t>(AlgorithmId::CLOSED_LOOP):
+ case static_cast<uint8_t>(AlgorithmId::SEQUENCE):
+ case static_cast<uint8_t>(AlgorithmId::ROTATION):
+ {
+ selectedAlgorithm = static_cast<AlgorithmId>(index);
+
+ auto data = WingControllerAlgorithmData{
+ .timestamp = TimestampTimer::getTimestamp(),
+ .algorithm = index};
+ Logger::getInstance().log(data);
+
+ return true;
+ }
+
+ default:
{
- algorithms[selectedAlgorithm]->end();
- reset();
+ return false;
}
}
}
-void WingController::update()
+void WingController::loadAlgorithms()
{
- if (running)
+ // Early Maneuver Guidance Automatic Algorithm
+ algorithms[static_cast<size_t>(AlgorithmId::EARLY_MANEUVER)] =
+ std::make_unique<AutomaticWingAlgorithm>(
+ PI::KP, PI::KI, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO,
+ emGuidance);
+
+ // Closed Loop Guidance Automatic Algorithm
+ algorithms[static_cast<size_t>(AlgorithmId::CLOSED_LOOP)] =
+ std::make_unique<AutomaticWingAlgorithm>(
+ PI::KP, PI::KI, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO,
+ clGuidance);
+
+ // Sequence
{
- algorithms[selectedAlgorithm]->step();
+ auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO,
+ PARAFOIL_RIGHT_SERVO);
+ WingAlgorithmData step;
+
+ step.timestamp = 0;
+ step.servo1Angle = 0;
+ step.servo2Angle = 120;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count();
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count();
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ algorithm->addStep(step);
+
+ algorithms[static_cast<size_t>(AlgorithmId::SEQUENCE)] =
+ std::move(algorithm);
}
-}
-void WingController::flare()
-{
- // Set the servo position to flare (pull the two ropes as skydiving people
- // do)
- getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f);
- getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f);
+ // Rotation
+ {
+ auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO,
+ PARAFOIL_RIGHT_SERVO);
+ WingAlgorithmData step;
+
+ step.timestamp = 0;
+ step.servo1Angle = LeftServo::ROTATION / 2;
+ step.servo2Angle = 0;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{ROTATION_PERIOD}.count();
+ step.servo1Angle = 0;
+ step.servo2Angle = RightServo::ROTATION / 2;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{ROTATION_PERIOD}.count();
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{ROTATION_PERIOD}.count();
+ step.servo1Angle = LeftServo::ROTATION;
+ step.servo2Angle = RightServo::ROTATION;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{ROTATION_PERIOD}.count();
+ step.servo1Angle = 0;
+ step.servo2Angle = RightServo::ROTATION;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{ROTATION_PERIOD}.count();
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ algorithm->addStep(step);
+
+ step.timestamp += microseconds{ROTATION_PERIOD}.count();
+ step.servo1Angle = 0;
+ step.servo2Angle = 0;
+ algorithm->addStep(step);
+
+ algorithms[static_cast<size_t>(AlgorithmId::ROTATION)] =
+ std::move(algorithm);
+ }
}
-void WingController::twirl()
+WingAlgorithm& WingController::getCurrentAlgorithm()
{
- // Set the servo position to twirl
- getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, TWIRL_RADIUS);
- getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
+ auto index = static_cast<size_t>(selectedAlgorithm.load());
+ return *algorithms[index].get();
}
-void WingController::reset()
+void WingController::startAlgorithm()
{
- // Set the servo position to reset
- getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f);
- getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
+ running = true;
+ updateEarlyManeuverPoints();
+
+ getCurrentAlgorithm().begin();
}
-void WingController::setTargetPosition(Eigen::Vector2f targetGEO)
+void WingController::stopAlgorithm()
{
- if (getModule<NASController>()->getState() == NASControllerState::ACTIVE)
+ if (running)
{
- this->targetPositionGEO = targetGEO;
- setEarlyManeuverPoints(convertTargetPositionToNED(targetPositionGEO),
- {getModule<NASController>()->getNasState().n,
- getModule<NASController>()->getNasState().e});
+ running = false;
+
+ getCurrentAlgorithm().end();
+ resetWing();
}
}
-void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED,
- Eigen::Vector2f currentPosNED)
+void WingController::updateEarlyManeuverPoints()
{
+ using namespace Eigen;
- Eigen::Vector2f targetOffsetNED = targetNED - currentPosNED;
+ auto nas = getModule<NASController>();
+ auto nasState = nas->getNasState();
+ auto nasRef = nas->getReferenceValues();
- Eigen::Vector2f norm_point = targetOffsetNED / targetOffsetNED.norm();
+ Vector2f currentPositionNED = {nasState.n, nasState.e};
+ Vector2f targetNED = Aeroutils::geodetic2NED(
+ targetPositionGEO, {nasRef.refLatitude, nasRef.refLongitude});
- float psi0 = atan2(norm_point[1], norm_point[0]);
+ Vector2f targetOffsetNED = targetNED - currentPositionNED;
+ Vector2f normPoint = targetOffsetNED / targetOffsetNED.norm();
+ float psi0 = atan2(normPoint[1], normPoint[0]);
float distFromCenterline = 20; // the distance that the M1 and M2 points
// must have from the center line
@@ -480,59 +513,71 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED,
float m2Angle = psi0 + psiMan;
float m1Angle = psi0 - psiMan;
- Eigen::Vector2f emcPosition =
- targetOffsetNED * 1.2 +
- currentPosNED; // EMC is calculated as target * 1.2
+ // EMC is calculated as target * 1.2
+ Vector2f emcPosition = targetOffsetNED * 1.2 + currentPositionNED;
- Eigen::Vector2f m1Position =
- Eigen::Vector2f(cos(m1Angle), sin(m1Angle)) * maneuverPointsMagnitude +
- currentPosNED;
+ Vector2f m1Position =
+ Vector2f{cos(m1Angle), sin(m1Angle)} * maneuverPointsMagnitude +
+ currentPositionNED;
- Eigen::Vector2f m2Position =
- Eigen::Vector2f(cos(m2Angle), sin(m2Angle)) * maneuverPointsMagnitude +
- currentPosNED;
+ Vector2f m2Position =
+ Vector2f{cos(m2Angle), sin(m2Angle)} * maneuverPointsMagnitude +
+ currentPositionNED;
emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position);
-
clGuidance.setPoints(targetNED);
- 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];
+ // Log the updated points
+ auto data = WingTargetPositionData{
+ .targetLat = targetPositionGEO[0],
+ .targetLon = targetPositionGEO[1],
+ .targetN = targetNED[0],
+ .targetE = targetNED[1],
+ .emcN = emcPosition[0],
+ .emcE = emcPosition[1],
+ .m1N = m1Position[0],
+ .m1E = m1Position[1],
+ .m2N = m2Position[0],
+ .m2E = m2Position[1],
+ };
+ Logger::getInstance().log(data);
+}
- data.m2N = m2Position[0];
- data.m2E = m2Position[1];
+void WingController::update()
+{
+ if (running)
+ {
+ getCurrentAlgorithm().step();
+ }
+}
- // Log the received position
- Logger::getInstance().log(data);
+void WingController::flareWing()
+{
+ getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f);
+ getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f);
}
-void WingController::logStatus(WingControllerState state)
+void WingController::twirlWing()
{
- miosix::Lock<miosix::FastMutex> s(statusMutex);
- status.timestamp = TimestampTimer::getTimestamp();
- status.state = state;
+ getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, TWIRL_RADIUS);
+ getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
+}
- Logger::getInstance().log(status);
+void WingController::resetWing()
+{
+ getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f);
+ getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
}
-Eigen::Vector2f WingController::convertTargetPositionToNED(
- Eigen::Vector2f targetGEO)
+void WingController::updateState(WingControllerState newState)
{
- return Aeroutils::geodetic2NED(
- targetGEO,
- {getModule<NASController>()->getReferenceValues().refLatitude,
- getModule<NASController>()->getReferenceValues().refLongitude});
+ state = newState;
+
+ auto status = WingControllerStatus{
+ .timestamp = TimestampTimer::getTimestamp(),
+ .state = newState,
+ };
+ Logger::getInstance().log(status);
}
} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
index 2ff217581..66874623b 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.h
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Federico Mandelli, Angelo Prete
+ * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,6 +22,7 @@
#pragma once
+#include <Payload/Configs/WingConfig.h>
#include <Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
#include <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
#include <Payload/Wing/WingAlgorithm.h>
@@ -83,40 +84,41 @@ public:
*/
~WingController();
- 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);
+ // HSM states
+ Boardcore::State Idle(const Boardcore::Event& event);
+ Boardcore::State Flying(const Boardcore::Event& event);
+ Boardcore::State FlyingCalibration(const Boardcore::Event& event);
+ Boardcore::State FlyingControlledDescent(const Boardcore::Event& event);
+ Boardcore::State OnGround(const Boardcore::Event& event);
/**
- * @brief Method to set the target position.
+ * @brief Override the inject method to inject dependencies into the
+ * algorithms, which are insantiated later than top-level modules.
*/
- void setTargetPosition(Eigen::Vector2f target);
+ void inject(Boardcore::DependencyInjector& injector) override;
- /**
- * @brief Selects the algorithm if present.
- *
- * @param index The algorithms vector index
- *
- * @return true if the selection was successful
- */
- bool selectAlgorithm(unsigned int index);
+ bool start() override;
- WingControllerStatus getStatus();
+ bool isStarted();
- bool start() override;
+ WingControllerState getState();
/**
- * @brief Override the inject method to inject dependencies into the
- * algorithms, which are insantiated later than top-level modules.
+ * @brief Sets the target position.
+ * @note The provided position must be using geodetic coordinates.
*/
- void inject(Boardcore::DependencyInjector& injector) override;
+ bool setTargetPosition(Eigen::Vector2f targetPositionGEO);
/**
- * @brief This method is a forward method from early maneuvers guidance
- * algorithm to calculate the yaw angle of the parafoil knowing the current
- * position and the target position.
+ * @brief Changes the selected algorithm.
+ * @return Whether the provided index selected a valid algorithm.
+ */
+ bool selectAlgorithm(uint8_t index);
+
+ /**
+ * @brief This is a forward method to the early maneuvers guidance algorithm
+ * to calculate 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
@@ -124,7 +126,6 @@ public:
* @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)
@@ -134,58 +135,75 @@ public:
private:
/**
- * @brief Instantiates the algorithms and adds them to the algorithms
- * vector.
+ * @brief Loads all algorithms.
*
* @note Algorithms should be instantiated before dependency injection so
* that they can be injected with dependencies when the WingController is
* injected.
*/
- bool addAlgorithms();
+ void loadAlgorithms();
/**
- * @brief target position getter
+ * @brief Returns the currently selected algorithm.
*/
- Eigen::Vector2f convertTargetPositionToNED(Eigen::Vector2f targetGEO);
+ WingAlgorithm& getCurrentAlgorithm();
/**
- * @brief set points needed by the Guidance
+ * @brief Starts the currently selected algorithm. If the algorithm is
+ * already running, it resets the algorithm.
*/
- void setEarlyManeuverPoints(Eigen::Vector2f targetNED,
- Eigen::Vector2f currentPosNED);
-
- void logStatus(WingControllerState state);
-
- WingControllerStatus status;
+ void startAlgorithm();
- miosix::FastMutex statusMutex;
+ /**
+ * @brief Stops the currently selected algorithm.
+ */
+ void stopAlgorithm();
/**
- * @brief Target position
+ * @brief Update early maneuver guidance points (EMC, M1, M2) based on the
+ * current position and the target position.
*/
- Eigen::Vector2f targetPositionGEO;
+ void updateEarlyManeuverPoints();
/**
- * @brief List of loaded algorithms (from SD or not)
+ * @brief Periodic update method that steps the currently selected
+ * algorithm.
*/
- std::vector<WingAlgorithm*> algorithms;
+ void update();
/**
- * @brief PrintLogger
+ * @brief Flare the wing.
+ * Pulls the two ropes as skydiving people do.
*/
- Boardcore::PrintLogger logger =
- Boardcore::Logging::getLogger("WingController");
+ void flareWing();
/**
- * @brief Internal running state
+ * @brief Twirl the wing to the left.
*/
- std::atomic<bool> running;
+ void twirlWing();
+
/**
- * @brief This attribute is modified by the mavlink radio section.
- * The user using the Ground Station can select the pre-enumerated algorithm
- * to execute
+ * @brief Reset the wing to the initial position.
*/
- std::atomic<size_t> selectedAlgorithm;
+ void resetWing();
+
+ void updateState(WingControllerState newState);
+
+ Eigen::Vector2f targetPositionGEO{Config::Wing::Default::TARGET_LAT,
+ Config::Wing::Default::TARGET_LON};
+
+ std::atomic<bool> targetPositionDirty{
+ false}; ///< Whether the target position was changed and EM points need
+ ///< to be updated
+
+ std::atomic<Config::Wing::AlgorithmId> selectedAlgorithm{
+ Config::Wing::Default::ALGORITHM};
+
+ std::atomic<WingControllerState> state{WingControllerState::UNINIT};
+
+ std::array<std::unique_ptr<WingAlgorithm>,
+ static_cast<size_t>(Config::Wing::AlgorithmId::LAST)>
+ algorithms; ///< The available algorithms
/**
* @brief Instance of the Early Maneuver Guidance Algorithm used by
@@ -199,30 +217,11 @@ private:
*/
ClosedLoopGuidanceAlgorithm clGuidance;
- /**
- * @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();
-
- void twirl();
+ std::atomic<bool> started{false};
+ std::atomic<bool> running{false}; ///< Whether the algorithm is running
- /**
- * @brief Resets the servos in their initial position
- */
- void reset();
-
- void update();
+ Boardcore::PrintLogger logger =
+ Boardcore::Logging::getLogger("WingController");
};
} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/WingController/WingControllerData.h b/src/boards/Payload/StateMachines/WingController/WingControllerData.h
index 82e444561..0d175ea35 100644
--- a/src/boards/Payload/StateMachines/WingController/WingControllerData.h
+++ b/src/boards/Payload/StateMachines/WingController/WingControllerData.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Federico Mandelli, Niccolò Betto
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,8 +22,7 @@
#pragma once
-#include <stdint.h>
-
+#include <cstdint>
#include <iostream>
#include <string>
@@ -34,15 +33,15 @@ enum class WingControllerState : uint8_t
{
UNINIT = 0,
IDLE,
- CALIBRATION,
- ALGORITHM_CONTROLLED,
+ FLYING_CALIBRATION,
+ FLYING_CONTROLLED_DESCENT,
ON_GROUND,
END
};
struct WingControllerStatus
{
- long long timestamp = 0;
+ uint64_t timestamp = 0;
WingControllerState state = WingControllerState::UNINIT;
static std::string header() { return "timestamp,state\n"; }
@@ -53,4 +52,17 @@ struct WingControllerStatus
}
};
+struct WingControllerAlgorithmData
+{
+ uint64_t timestamp = 0;
+ uint8_t algorithm = 0;
+
+ static std::string header() { return "timestamp,algorithm\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)algorithm << "\n";
+ }
+};
+
} // namespace Payload
diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
index 8639f619a..278894a11 100644
--- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
@@ -44,12 +44,13 @@ AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki,
GuidanceAlgorithm& guidance)
: Super(servo1, servo2), guidance(guidance)
{
- controller = new PIController(Kp, Ki, WING_UPDATE_PERIOD / 1000.0f,
- PI_CONTROLLER_SATURATION_MIN_LIMIT,
- PI_CONTROLLER_SATURATION_MAX_LIMIT);
-}
+ // PIController needs the sample period in floating point seconds
+ auto samplePeriod = 1.0f / Hertz{UPDATE_RATE}.value();
-AutomaticWingAlgorithm::~AutomaticWingAlgorithm() { delete (controller); }
+ controller = std::make_unique<PIController>(Kp, Ki, samplePeriod,
+ PI::SATURATION_MIN_LIMIT,
+ PI::SATURATION_MAX_LIMIT);
+}
void AutomaticWingAlgorithm::step()
{
@@ -72,14 +73,14 @@ void AutomaticWingAlgorithm::step()
if (result > 0)
{
// Activate the servo2 and reset servo1
- getModule<Actuators>()->setServoAngle(servo1, 0);
+ getModule<Actuators>()->setServoAngle(servo1, 0.0f);
getModule<Actuators>()->setServoAngle(servo2, result);
}
else
{
// Activate the servo1 and reset servo2
- getModule<Actuators>()->setServoAngle(servo1, result * -1);
- getModule<Actuators>()->setServoAngle(servo2, 0);
+ getModule<Actuators>()->setServoAngle(servo1, result * -1.0f);
+ getModule<Actuators>()->setServoAngle(servo2, 0.0f);
}
// Log the servo positions
@@ -95,8 +96,8 @@ void AutomaticWingAlgorithm::step()
else
{
// If we loose fix we set both servo at 0
- getModule<Actuators>()->setServoAngle(servo1, 0);
- getModule<Actuators>()->setServoAngle(servo2, 0);
+ getModule<Actuators>()->setServoAngle(servo1, 0.0f);
+ getModule<Actuators>()->setServoAngle(servo2, 0.0f);
}
}
diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.h b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h
index 1b9b9fc23..7d3eb87c7 100644
--- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.h
+++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h
@@ -56,17 +56,12 @@ public:
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;
+ std::unique_ptr<Boardcore::PIController> controller;
/**
* @brief Actual algorithm implementation, all parameters should be in NED
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
index d1d3e3a25..698f5f6cb 100644
--- a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
@@ -70,18 +70,18 @@ float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
{
if (altitude <=
- GUIDANCE_TARGET_ALTITUDE_THRESHOLD) // Altitude is low, head directly
- // to target
+ 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
+ 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
+ Guidance::M1_ALTITUDE_THRESHOLD) // Altitude is high, go to M1
{
m1AltitudeConfidence++;
}
@@ -93,41 +93,44 @@ void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
switch (activeTarget)
{
case Target::EMC:
- if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
{
activeTarget = Target::FINAL;
emcAltitudeConfidence = 0;
}
- else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
{
activeTarget = Target::M2;
emcAltitudeConfidence = 0;
}
- else if (m1AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ else if (m1AltitudeConfidence >= Guidance::CONFIDENCE)
{
activeTarget = Target::M1;
emcAltitudeConfidence = 0;
}
break;
+
case Target::M1:
- if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
{
activeTarget = Target::FINAL;
m1AltitudeConfidence = 0;
}
- else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
{
activeTarget = Target::M2;
m1AltitudeConfidence = 0;
}
break;
+
case Target::M2:
- if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+ if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
{
activeTarget = Target::FINAL;
m2AltitudeConfidence = 0;
}
break;
+
case Target::FINAL:
break;
}
diff --git a/src/boards/Payload/Wing/WingAlgorithmData.h b/src/boards/Payload/Wing/WingAlgorithmData.h
index 38e20628c..6bf746d62 100644
--- a/src/boards/Payload/Wing/WingAlgorithmData.h
+++ b/src/boards/Payload/Wing/WingAlgorithmData.h
@@ -31,15 +31,15 @@ namespace Payload
*/
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;
+ uint64_t timestamp = 0; // First timestamp is 0 (in microseconds)
+ float servo1Angle = 0; // degrees
+ float servo2Angle = 0; // degrees
+ float targetAngle = 0; // radians (automatic only)
+ float velocityAngle = 0; // radians
+ float targetX = 0; // NED (only automatic algorithm)
+ float targetY = 0; // NED (only automatic algorithm)
+ float error = 0;
+ float pidOutput = 0;
static std::string header()
{
diff --git a/src/boards/Payload/Wing/WingTargetPositionData.h b/src/boards/Payload/Wing/WingTargetPositionData.h
index cb2797924..1f875b5fa 100644
--- a/src/boards/Payload/Wing/WingTargetPositionData.h
+++ b/src/boards/Payload/Wing/WingTargetPositionData.h
@@ -30,28 +30,28 @@ namespace Payload
struct WingTargetPositionData
{
- float receivedLat;
- float receivedLon;
- float targetN;
- float targetE;
- float emcN;
- float emcE;
- float m1N;
- float m1E;
- float m2N;
- float m2E;
+ float targetLat = 0;
+ float targetLon = 0;
+ float targetN = 0;
+ float targetE = 0;
+ float emcN = 0;
+ float emcE = 0;
+ float m1N = 0;
+ float m1E = 0;
+ float m2N = 0;
+ float m2E = 0;
static std::string header()
{
- return "receivedLat, receivedLon, "
- "targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n";
+ return "targetLat, "
+ "targetLon,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";
+ os << targetLat << "," << targetLon << "," << targetN << "," << targetE
+ << "," << emcN << "," << emcE << "," << m1N << "," << m1E << ","
+ << m2N << "," << m2E << "\n";
}
};
diff --git a/src/scripts/logdecoder/Payload/logdecoder.cpp b/src/scripts/logdecoder/Payload/logdecoder.cpp
index 3a120781d..1919157dd 100644
--- a/src/scripts/logdecoder/Payload/logdecoder.cpp
+++ b/src/scripts/logdecoder/Payload/logdecoder.cpp
@@ -68,6 +68,7 @@ void registerTypes(Deserializer& ds)
ds.registerType<DynamicPressureData>();
ds.registerType<SensorsCalibrationParameter>();
ds.registerType<PinChangeData>();
+ ds.registerType<WingControllerAlgorithmData>();
ds.registerType<WingAlgorithmData>();
ds.registerType<WingTargetPositionData>();
ds.registerType<WindLogging>();
--
GitLab