diff --git a/src/boards/Payload/BoardScheduler.h b/src/boards/Payload/BoardScheduler.h
index d10a4de252cfe467eccbda2f4b2b4669d78946e9..1340b8992aca633a0ebf11e35bb3c1aef7baa57a 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 3a28c460901e39e72683bd9bca9c1014b9e95db8..91c0b50b2237a569aa3d1070a7ebfe05a0d24baf 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 de80e13cd1d443ad51f98e5163c1c1c69b10e7e3..13d02817f39e78d06e9dbb718adb09688bad3064 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 2ff217581faafea179985931b2d07ee2ab6cb2c6..66874623bff274daff39d7381e80922d22bda4ff 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 82e444561b10465ed3c179b058b9198c5de7cf9a..0d175ea3530314d974a21c8a563781e7790fffa2 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 8639f619ae60d2ae6282256284f4ce8d488956b3..278894a11ef4a050f97b6890b01ed023beb36339 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 1b9b9fc23d6e27f178d099ffaaf13a898c45578d..7d3eb87c73634bae18b78a4a541928a21ab1c5af 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 d1d3e3a25667e7e2038c746a14b5bb339c2545d0..698f5f6cb04493188ee52a8b65b7ba5df712e028 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 38e20628c332a5965e0a50e62381d6df61fc4ab8..6bf746d62e4ea2270c3c4d629ab54f22cec77258 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 cb27979246a74c72cc61a22c67b13b6b2c89c634..1f875b5fa979be3726c58ca67dd92b7bd6c4c463 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 3a120781dd3919dca7baf5cc0b685b19dc2bdee4..1919157dd9cca085391ade4962bb3324dd84b721 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>();