diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 6508dc5c7e23f0ff9b7fa50ac09146dabe2e0053..240c2798bd18756083de7e6b51864aea50e9fff2 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -92,6 +92,7 @@ set(PAYLOAD_COMPUTER
     src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp
     src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
     src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+    src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
     src/boards/Payload/Wing/FileWingAlgorithm.cpp
     src/boards/Payload/Wing/WingAlgorithm.cpp
     src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
diff --git a/src/boards/Payload/Configs/WESConfig.h b/src/boards/Payload/Configs/WESConfig.h
index 36382803a9472568c8e9bfdf195847383cf561a5..da2357dc4f7a800a76030c32425cc8b8bdb52fb3 100644
--- a/src/boards/Payload/Configs/WESConfig.h
+++ b/src/boards/Payload/Configs/WESConfig.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
+/* Copyright (c) 2024 Skyward Experimental Rocketry
  * Author: Federico Mandelli
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -22,24 +22,33 @@
 
 #pragma once
 
-#include <miosix.h>
+#include <units/Frequency.h>
+
+#include <chrono>
+#include <cstdint>
 
 namespace Payload
 {
-
-namespace WESConfig
+namespace Config
+{
+namespace WES
 {
+/* linter off */ using namespace std::chrono_literals;
+/* linter off */ using namespace Boardcore::Units::Frequency;
 
-constexpr uint32_t WES_CALIBRATION_TIMEOUT =
-    5 * 1000;  // time needed for the first loop [ms]
+constexpr auto ROTATION_PERIOD = 10s;
 
-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 Calibration
+{
+constexpr auto SAMPLE_RATE  = 4_hz;
+constexpr auto SAMPLE_COUNT = 20;  ///< Samples to collect during calibration
+}  // namespace Calibration
 
-}  // namespace WESConfig
+namespace Prediction
+{
+constexpr auto UPDATE_RATE = 10_hz;
+}
 
+}  // namespace WES
+}  // namespace Config
 }  // namespace Payload
diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index ee07030cb31622a223cec7864f4e3c91198bc996..3a28c460901e39e72683bd9bca9c1014b9e95db8 100644
--- a/src/boards/Payload/Configs/WingConfig.h
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022-2024 Skyward Experimental Rocketry
- * Authors: Federico Mandelli, Nicclò Betto
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * 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
@@ -23,6 +23,7 @@
 #pragma once
 
 #include <units/Frequency.h>
+#include <utils/Constants.h>
 
 namespace Payload
 {
@@ -30,24 +31,37 @@ 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
+
 #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;
-#elif defined(EUROC)
-constexpr float DEFAULT_TARGET_LAT = 39.383;
-constexpr float DEFAULT_TARGET_LON = -8.27963;
+constexpr float DEFAULT_TARGET_LAT = 41.8089005;
+constexpr float DEFAULT_TARGET_LON = 14.0546716;
 #else  // Milan
-constexpr float DEFAULT_TARGET_LAT = 39.383;
-constexpr float DEFAULT_TARGET_LON = -8.27963;
+constexpr float DEFAULT_TARGET_LAT = 45.5010679;
+constexpr float DEFAULT_TARGET_LON = 9.1563769;
 #endif
 
+constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000;  // [ms]
+
 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 float PI_CONTROLLER_SATURATION_MAX_LIMIT = Boardcore::Constants::PI;
+constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
 
 constexpr int GUIDANCE_CONFIDENCE                = 15;
 constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD     = 250;  //[m]
@@ -55,15 +69,18 @@ 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 KP = 0.9;
+constexpr float KI = 0.05;
+
+constexpr float TWIRL_RADIUS = 0.5;  // [%]
+
 }  // namespace Wing
 
 namespace AltitudeTrigger
 {
 /* linter off */ using namespace Boardcore::Units::Frequency;
 
-constexpr auto DEPLOYMENT_ALTITUDE = 400;  // [meters]
+constexpr auto DEPLOYMENT_ALTITUDE = 300;  // [meters]
 constexpr auto CONFIDENCE          = 10;   // [samples]
 constexpr auto UPDATE_RATE         = 10_hz;
 }  // namespace AltitudeTrigger
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index 62e64bcb31ad75fa55047db63022c4b1f9d2ddbb..de80e13cd1d443ad51f98e5163c1c1c69b10e7e3 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -23,6 +23,7 @@
 
 #include <Payload/Actuators/Actuators.h>
 #include <Payload/BoardScheduler.h>
+#include <Payload/Configs/ActuatorsConfig.h>
 #include <Payload/Configs/WESConfig.h>
 #include <Payload/Configs/WingConfig.h>
 #include <Payload/FlightStatsRecorder/FlightStatsRecorder.h>
@@ -39,10 +40,12 @@
 #include <drivers/timer/TimestampTimer.h>
 #include <events/EventBroker.h>
 
+using namespace std::chrono;
 using namespace Boardcore;
-using namespace Payload::Config::Wing;
-using namespace Payload::WESConfig;
 using namespace Common;
+using namespace Payload::Config::Wing;
+using namespace Payload::Config::WES;
+using namespace Payload::Config::Actuators;
 
 namespace Payload
 {
@@ -52,6 +55,7 @@ WingController::WingController()
 {
     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
@@ -156,33 +160,61 @@ State WingController::state_flying(const Event& event)
 
 State WingController::state_calibration(const Boardcore::Event& event)
 {
+    static uint16_t calibrationTimeoutEventId;
+
     switch (event)
     {
         case EV_ENTRY:  // starts twirling and calibration wes
         {
             logStatus(WingControllerState::CALIBRATION);
-            getModule<Actuators>()->setServoPosition(
-                ServosList::PARAFOIL_LEFT_SERVO, 1);
-            getModule<Actuators>()->setServoPosition(
-                ServosList::PARAFOIL_RIGHT_SERVO, 0);
-            EventBroker::getInstance().postDelayed(DPL_WES_CAL_DONE, TOPIC_DPL,
-                                                   WES_CALIBRATION_TIMEOUT);
-            getModule<WindEstimation>()->startWindEstimationSchemeCalibration();
+
+            flare();
+            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);
         }
+        case DPL_SERVO_ACTUATION_DETECTED:
+        {
+            reset();
+            calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+                DPL_WIGGLE, TOPIC_DPL, 1000);
+
+            return HANDLED;
+        }
+        case DPL_WIGGLE:
+        {
+            flare();
+            calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+                DPL_NC_OPEN, TOPIC_DPL, 2000);
+
+            return HANDLED;
+        }
+        case DPL_NC_OPEN:
+        {
+            reset();
+            calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+                DPL_WES_CAL_DONE, TOPIC_DPL,
+                milliseconds{Config::WES::ROTATION_PERIOD}.count());
+            getModule<WindEstimation>()->startWindEstimationSchemeCalibration();
+
+            twirl();
+
+            return HANDLED;
+        }
         case DPL_WES_CAL_DONE:
         {
             reset();
-            // Turn off the cutters
-            getModule<Actuators>()->cuttersOff();
+
             return transition(&WingController::state_controlled_descent);
         }
         default:
@@ -198,7 +230,6 @@ State WingController::state_controlled_descent(const Boardcore::Event& event)
         case EV_ENTRY:  // start automatic algorithm
         {
             logStatus(WingControllerState::ALGORITHM_CONTROLLED);
-            selectAlgorithm(0);
             setEarlyManeuverPoints(
                 convertTargetPositionToNED(targetPositionGEO),
                 {getModule<NASController>()->getNasState().n,
@@ -210,6 +241,10 @@ State WingController::state_controlled_descent(const Boardcore::Event& event)
         {
             return tranSuper(&WingController::state_flying);
         }
+        case WING_ALGORITHM_ENDED:
+        {
+            return transition(&WingController::state_on_ground);
+        }
         case EV_EXIT:
         {
             return HANDLED;
@@ -232,6 +267,11 @@ State WingController::state_on_ground(const Boardcore::Event& event)
             getModule<WindEstimation>()->stopWindEstimationScheme();
             getModule<WindEstimation>()->stopWindEstimationSchemeCalibration();
             stopAlgorithm();
+
+            // disable servos
+            getModule<Actuators>()->disableServo(PARAFOIL_LEFT_SERVO);
+            getModule<Actuators>()->disableServo(PARAFOIL_RIGHT_SERVO);
+
             return HANDLED;
         }
         case EV_EXIT:
@@ -249,48 +289,79 @@ State WingController::state_on_ground(const Boardcore::Event& event)
     }
 }
 
-void WingController::addAlgorithms()
+bool WingController::addAlgorithms()
 {
     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);
-    // Ensure that the servos are correct
-    algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
+    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);
-
-    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);
-
-    // Add the algorithm to the vector
+    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(0);
+    selectAlgorithm(SELECTED_ALGORITHM);
+
+    return result;
 }
 
 bool WingController::selectAlgorithm(unsigned int index)
@@ -358,20 +429,27 @@ void WingController::flare()
 {
     // Set the servo position to flare (pull the two ropes as skydiving people
     // do)
-    getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1);
-    getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1);
+    getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f);
+    getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f);
+}
+
+void WingController::twirl()
+{
+    // Set the servo position to twirl
+    getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, TWIRL_RADIUS);
+    getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
 }
 
 void WingController::reset()
 {
     // Set the servo position to reset
-    getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0);
-    getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0);
+    getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f);
+    getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
 }
 
 void WingController::setTargetPosition(Eigen::Vector2f targetGEO)
 {
-    if (getModule<NASController>()->getState() != NASControllerState::READY)
+    if (getModule<NASController>()->getState() == NASControllerState::ACTIVE)
     {
         this->targetPositionGEO = targetGEO;
         setEarlyManeuverPoints(convertTargetPositionToNED(targetPositionGEO),
@@ -415,6 +493,9 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED,
         currentPosNED;
 
     emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position);
+
+    clGuidance.setPoints(targetNED);
+
     WingTargetPositionData data;
 
     data.receivedLat = targetPositionGEO[0];
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
index 3bdda1a71f600a78a89c8642e984ff5a5a7c64c7..7fb6f482f7f7935349ca7f1ad97801d4be88c0d1 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.h
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Angelo Prete
  *
  * 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/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
 #include <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
 #include <Payload/Wing/WingAlgorithm.h>
 #include <diagnostic/PrintLogger.h>
@@ -121,7 +122,7 @@ private:
      * that they can be injected with dependencies when the WingController is
      * injected.
      */
-    void addAlgorithms();
+    bool addAlgorithms();
 
     /**
      * @brief target position getter
@@ -154,7 +155,7 @@ private:
      * @brief PrintLogger
      */
     Boardcore::PrintLogger logger =
-        Boardcore::Logging::getLogger("PayloadTest");
+        Boardcore::Logging::getLogger("WingController");
 
     /**
      * @brief Internal running state
@@ -173,6 +174,12 @@ private:
      */
     EarlyManeuversGuidanceAlgorithm emGuidance;
 
+    /**
+     * @brief Instance of the Closed Loop Guidance Algorithm used by
+     * AutomaticWingAlgorithm
+     */
+    ClosedLoopGuidanceAlgorithm clGuidance;
+
     /**
      * @brief  starts the selected algorithm
      */
@@ -189,6 +196,8 @@ private:
      */
     void flare();
 
+    void twirl();
+
     /**
      * @brief Resets the servos in their initial position
      */
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
index 8bf0d32f2ca8b0e1f2ea0aa76f9ba98851146e38..4c5df6049aa68a1195f2377d97ae249d90ade402 100644
--- a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
+/* Copyright (c) 2024 Skyward Experimental Rocketry
  * Author: Federico Mandelli
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -28,9 +28,9 @@
 #include <common/Events.h>
 #include <events/EventBroker.h>
 
-using namespace Payload::WESConfig;
 using namespace Boardcore;
 using namespace Common;
+namespace config = Payload::Config::WES;
 
 namespace Payload
 {
@@ -42,14 +42,16 @@ WindEstimation::WindEstimation() : running(false), calRunning(false)
 
 bool WindEstimation::start()
 {
+    using namespace std::chrono;
+
     auto& scheduler = getModule<BoardScheduler>()->windEstimation();
 
     scheduler.addTask([this] { windEstimationSchemeCalibration(); },
-                      WES_CALIBRATION_UPDATE_PERIOD);
+                      config::Calibration::SAMPLE_RATE);
 
     // Register the WES task
     scheduler.addTask([this] { windEstimationScheme(); },
-                      WES_PREDICTION_UPDATE_PERIOD);
+                      config::Prediction::UPDATE_RATE);
 
     return true;
 }
@@ -68,8 +70,8 @@ void WindEstimation::startWindEstimationSchemeCalibration()
     {
         calRunning = true;
         nSampleCal = 0;
-        vx         = 0;
-        vy         = 0;
+        vn         = 0;
+        ve         = 0;
         v2         = 0;
         LOG_INFO(logger, "WindEstimationCalibration started");
     }
@@ -85,8 +87,9 @@ void WindEstimation::stopWindEstimationSchemeCalibration()
         miosix::Lock<FastMutex> l(mutex);
         wind = windCalibration;
     }
-    windLogger.vn = windCalibration[0];
-    windLogger.ve = windCalibration[1];
+    windLogger.cal = true;
+    windLogger.vn  = windCalibration[0];
+    windLogger.ve  = windCalibration[1];
     startWindEstimationScheme();
     logStatus();
 }
@@ -99,7 +102,7 @@ void WindEstimation::windEstimationSchemeCalibration()
         auto gpsData = getModule<Sensors>()->getUBXGPSLastSample();
         if (gpsData.fix != 0)
         {
-            if (nSampleCal < WES_CALIBRATION_SAMPLE_NUMBER)
+            if (nSampleCal < config::Calibration::SAMPLE_COUNT)
             {
                 float gpsN = 0, gpsE = 0;
 
@@ -109,20 +112,20 @@ void WindEstimation::windEstimationSchemeCalibration()
                 calibrationMatrix(nSampleCal, 1) = gpsE;
                 calibrationV2(nSampleCal)        = gpsN * gpsN + gpsE * gpsE;
 
-                vx += gpsN;
-                vy += gpsE;
+                vn += gpsN;
+                ve += gpsE;
                 v2 += gpsN * gpsN + gpsE * gpsE;
                 nSampleCal++;
             }
             else
             {
-                vx = vx / nSampleCal;
-                vy = vy / nSampleCal;
+                vn = vn / nSampleCal;
+                ve = ve / 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;
+                    calibrationMatrix(i, 0) = calibrationMatrix(i, 0) - vn;
+                    calibrationMatrix(i, 1) = calibrationMatrix(i, 1) - ve;
                     calibrationV2(i)        = 0.5f * (calibrationV2(i) - v2);
                 }
                 Eigen::MatrixXf calibrationMatrixT =
@@ -172,11 +175,11 @@ void WindEstimation::windEstimationScheme()
             gpsE = gpsData.velocityEast;
             // update avg
             nSample++;
-            vx = (vx * nSample + gpsN) / (nSample + 1);
-            vy = (vy * nSample + gpsE) / (nSample + 1);
+            vn = (vn * nSample + gpsN) / (nSample + 1);
+            ve = (ve * nSample + gpsE) / (nSample + 1);
             v2 = (v2 * nSample + (gpsN * gpsN + gpsE * gpsE)) / (nSample + 1);
-            phi(0) = gpsN - vx;
-            phi(1) = gpsE - vy;
+            phi(0) = gpsN - vn;
+            phi(1) = gpsE - ve;
             y      = 0.5f * ((gpsN * gpsN + gpsE * gpsE) - v2);
 
             phiT = phi.transpose();
@@ -186,9 +189,10 @@ void WindEstimation::windEstimationScheme()
                    (y - phiT * getWindEstimationScheme());
             {
                 miosix::Lock<FastMutex> l(mutex);
-                wind          = wind + temp;
-                windLogger.vn = wind[0];
-                windLogger.ve = wind[1];
+                wind           = wind + temp;
+                windLogger.vn  = wind[0];
+                windLogger.ve  = wind[1];
+                windLogger.cal = false;
             }
             logStatus();
         }
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.h b/src/boards/Payload/WindEstimationScheme/WindEstimation.h
index 8f76fd78cd0ca4f168e5a48b39446ee02b3e844f..b190beee531fb54178264490db57079bca9a81b1 100644
--- a/src/boards/Payload/WindEstimationScheme/WindEstimation.h
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
+/* Copyright (c) 2024 Skyward Experimental Rocketry
  * Author: Federico Mandelli
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -90,12 +90,11 @@ private:
      */
     Eigen::Vector2f windCalibration{0.0f, 0.0f};
     uint8_t nSampleCal = 0;
-    Eigen::Matrix<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER, 2>
+    Eigen::Matrix<float, Config::WES::Calibration::SAMPLE_COUNT, 2>
         calibrationMatrix;
-    Eigen::Vector<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER>
-        calibrationV2;
-    float vx = 0;
-    float vy = 0;
+    Eigen::Vector<float, Config::WES::Calibration::SAMPLE_COUNT> calibrationV2;
+    float vn = 0;
+    float ve = 0;
     float v2 = 0;
 
     /**
@@ -114,7 +113,7 @@ private:
      * @brief PrintLogger
      */
     Boardcore::PrintLogger logger =
-        Boardcore::Logging::getLogger("PayloadTest");
+        Boardcore::Logging::getLogger("WindEstimation");
 
     /**
      * @brief Logging struct
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimationData.h b/src/boards/Payload/WindEstimationScheme/WindEstimationData.h
index 49d674fc96df107474c7b001e301530e1879770e..3104692f7dd2e80f09209e4f2a183569c6db4fa9 100644
--- a/src/boards/Payload/WindEstimationScheme/WindEstimationData.h
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimationData.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
+/* Copyright (c) 2024 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
@@ -34,12 +34,13 @@ struct WindLogging
 {
     long long timestamp = 0;
     float vn = 0, ve = 0;
+    bool cal = 0;
 
-    static std::string header() { return "timestamp,vn,ve\n"; }
+    static std::string header() { return "timestamp,vn,ve,cal\n"; }
 
     void print(std::ostream& os) const
     {
-        os << timestamp << "," << vn << "," << ve << "\n ";
+        os << timestamp << "," << vn << "," << ve << "," << cal << "\n ";
     }
 };
 
diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
index 2a93c8167870e2accb8132011b5b4e1ee3eb76f7..8639f619ae60d2ae6282256284f4ce8d488956b3 100644
--- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro, Radu Raul
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: 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
@@ -61,17 +61,25 @@ void AutomaticWingAlgorithm::step()
             getModule<WindEstimation>()->getWindEstimationScheme());
 
         // Actuate the result
+        // To see how to interpret the PI output
+        // https://www.geogebra.org/calculator/xrhwarpz
+        // to system is
+        /*    N ^
+                |
+                |
+                ----> E
+        */
         if (result > 0)
         {
-            // Activate the servo1 and reset servo2
-            getModule<Actuators>()->setServoAngle(servo1, result);
-            getModule<Actuators>()->setServoAngle(servo2, 0);
+            // Activate the servo2 and reset servo1
+            getModule<Actuators>()->setServoAngle(servo1, 0);
+            getModule<Actuators>()->setServoAngle(servo2, result);
         }
         else
         {
-            // Activate the servo2 and reset servo1
-            getModule<Actuators>()->setServoAngle(servo1, 0);
-            getModule<Actuators>()->setServoAngle(servo2, result * -1);
+            // Activate the servo1 and reset servo2
+            getModule<Actuators>()->setServoAngle(servo1, result * -1);
+            getModule<Actuators>()->setServoAngle(servo2, 0);
         }
 
         // Log the servo positions
@@ -102,24 +110,15 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
 
     float targetAngle = guidance.calculateTargetAngle(currentPosition, heading);
 
-    Vector2f relativeVelocity(state.vn - windNED[0], state.ve - windNED[1]);
+    // WES is currently unused
+    Vector2f relativeVelocity(state.vn, state.ve);
 
     // 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]);
-    }
+    // All angle are computed as angle from the north direction
+
+    velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
 
     // Compute the angle difference
     float error = angleDiff(targetAngle, velocityAngle);
@@ -132,7 +131,8 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
     result = result * (180.f / Constants::PI);
 
     // Flip the servo orientation
-    result *= -1;
+    // result *= -1;
+    //  TODO check if this is needed
 
     // Logs the outputs
     {
diff --git a/src/boards/Payload/Wing/WingAlgorithm.cpp b/src/boards/Payload/Wing/WingAlgorithm.cpp
index 2ec7936062f12461435b1f855ed2be4cdadda9e4..2195782c3889801060ea142eb17840466381ad4b 100644
--- a/src/boards/Payload/Wing/WingAlgorithm.cpp
+++ b/src/boards/Payload/Wing/WingAlgorithm.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 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
@@ -24,6 +24,7 @@
 #include <Payload/Wing/WingAlgorithm.h>
 #include <common/Events.h>
 #include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
 
 using namespace Boardcore;
 using namespace Payload::Config::Wing;
@@ -89,7 +90,7 @@ void WingAlgorithm::step()
         // Set the index to 0 in case of another future execution
         stepIndex = 0;
         // Terminate here
-        // EventBroker::getInstance().post(ALGORITHM_ENDED, TOPIC_ALGOS);
+        EventBroker::getInstance().post(WING_ALGORITHM_ENDED, TOPIC_WING);
         return;
     }
 
diff --git a/src/boards/Payload/Wing/WingTargetPositionData.h b/src/boards/Payload/Wing/WingTargetPositionData.h
index a1cab4dbf456c366d1a301487aacfd1b11a0099c..cb27979246a74c72cc61a22c67b13b6b2c89c634 100644
--- a/src/boards/Payload/Wing/WingTargetPositionData.h
+++ b/src/boards/Payload/Wing/WingTargetPositionData.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Radu Raul
+/* Copyright (c) 2024 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
diff --git a/src/boards/common/Events.h b/src/boards/common/Events.h
index 92b4fdfd4544d27ae684114e0c5439f5fb23c3c4..b274d2b579913a62ba7c0ba3fdc09f64bdfeca81 100644
--- a/src/boards/common/Events.h
+++ b/src/boards/common/Events.h
@@ -147,6 +147,7 @@ enum Events : uint8_t
     TARS_FILLING_DONE,
     TARS_REFINING_DONE,
     ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+    WING_ALGORITHM_ENDED,
     LAST_EVENT
 };
 
@@ -265,6 +266,7 @@ inline std::string getEventString(uint8_t event)
         {TARS_REFINING_DONE, "TARS_REFINING_DONE"},
         {ALTITUDE_TRIGGER_ALTITUDE_REACHED,
          "ALTITUDE_TRIGGER_ALTITUDE_REACHED"},
+        {WING_ALGORITHM_ENDED, "WING_ALGORITHM_ENDED"},
         {LAST_EVENT, "LAST_EVENT"},
     };
 
diff --git a/src/boards/common/Topics.h b/src/boards/common/Topics.h
index eaa0429e89f472c4efe71081ac28124aea898faf..8855f866f5776048489d3e7a36b63107658f5985 100644
--- a/src/boards/common/Topics.h
+++ b/src/boards/common/Topics.h
@@ -44,12 +44,13 @@ enum Topics : uint8_t
     TOPIC_MOTOR,
     TOPIC_TARS,
     TOPIC_ALT,
+    TOPIC_WING,
 };
 
 const std::vector<uint8_t> TOPICS_LIST{
-    TOPIC_ABK,    TOPIC_ADA,  TOPIC_MEA, TOPIC_DPL, TOPIC_CAN,
-    TOPIC_FLIGHT, TOPIC_FMM,  TOPIC_FSR, TOPIC_NAS, TOPIC_TMTC,
-    TOPIC_MOTOR,  TOPIC_TARS, TOPIC_ALT,
+    TOPIC_ABK,    TOPIC_ADA,  TOPIC_MEA, TOPIC_DPL,  TOPIC_CAN,
+    TOPIC_FLIGHT, TOPIC_FMM,  TOPIC_FSR, TOPIC_NAS,  TOPIC_TMTC,
+    TOPIC_MOTOR,  TOPIC_TARS, TOPIC_ALT, TOPIC_WING,
 };
 
 }  // namespace Common