diff --git a/.vscode/settings.json b/.vscode/settings.json
index f4302917253850581626c8aebaa0f2f98370e74b..9f619c4c25cfc6271094ec17fa8889e1437a618a 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -148,6 +148,7 @@
         "Duca",
         "Eigen",
         "EXTI",
+        "funv",
         "Gpio",
         "GPIOF",
         "GPIOG",
@@ -155,6 +156,7 @@
         "JESOLO",
         "KALM",
         "kalman",
+        "ldlt",
         "Leds",
         "Lolli",
         "magcal",
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 6aa1c5f3cb9c85bb9f6ae98a04b336d73c76b218..d840a11fc0ab6e0f4f62d6d084b71595f20b47ac 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -120,7 +120,17 @@ set (LYRA_GS
 )
 
 set(PARAFOIL_COMPUTER 
+    src/Parafoil/Actuators/Actuators.cpp
+    src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
+    src/Parafoil/PinHandler/PinHandler.cpp
     src/Parafoil/Sensors/Sensors.cpp
-    # src/Parafoil/StateMachines/NASController/NASController.cpp
     src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+    src/Parafoil/StateMachines/NASController/NASController.cpp
+    src/Parafoil/StateMachines/WingController/WingController.cpp
+    src/Parafoil/WindEstimation/WindEstimation.cpp
+    src/Parafoil/Wing/AutomaticWingAlgorithm.cpp
+    src/Parafoil/Wing/FileWingAlgorithm.cpp
+    src/Parafoil/Wing/WingAlgorithm.cpp
+    src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
+    src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
 )
\ No newline at end of file
diff --git a/src/Parafoil/Actuators/Actuators.cpp b/src/Parafoil/Actuators/Actuators.cpp
index 02324701b39434240c40d66b4184080468742e67..b0cd1f8f8732878e347bed63e520f109cc74fc0d 100644
--- a/src/Parafoil/Actuators/Actuators.cpp
+++ b/src/Parafoil/Actuators/Actuators.cpp
@@ -26,7 +26,6 @@
 #include <Parafoil/Configs/ActuatorsConfig.h>
 #include <interfaces-impl/hwmapping.h>
 
-using namespace std::chrono;
 using namespace miosix;
 using namespace Boardcore;
 using namespace Boardcore::Units::Angle;
@@ -52,10 +51,6 @@ Actuators::Actuators()
 
 bool Actuators::start()
 {
-    using namespace std::chrono;
-
-    auto& scheduler = getModule<BoardScheduler>()->actuators();
-
     leftServo.servo->enable();
     rightServo.servo->enable();
 
diff --git a/src/Parafoil/Actuators/Actuators.h b/src/Parafoil/Actuators/Actuators.h
index 673b6bb81de5fa9d89d68b5734835d6c55a04e39..7554f2c585d3f427d245965b749883f32ae7dd81 100644
--- a/src/Parafoil/Actuators/Actuators.h
+++ b/src/Parafoil/Actuators/Actuators.h
@@ -42,8 +42,9 @@ public:
     struct ServoActuator
     {
         std::unique_ptr<Boardcore::Servo> servo;
-        Units::Angle::Degree fullRangeAngle =
-            Units::Angle::Degree(0);  ///< The full range of the servo [degrees]
+        Boardcore::Units::Angle::Degree fullRangeAngle =
+            Boardcore::Units::Angle::Degree(
+                0);  ///< The full range of the servo [degrees]
         miosix::FastMutex mutex;
     };
 
@@ -69,7 +70,8 @@ public:
      * @param angle Angle to set [degree].
      * @return True if the the angle was set.
      */
-    bool setServoAngle(ServosList servoId, Units::Angle::Degree angle);
+    bool setServoAngle(ServosList servoId,
+                       Boardcore::Units::Angle::Degree angle);
 
     /**
      * @brief Wiggles the servo for few seconds.
@@ -114,7 +116,7 @@ public:
      * @return float current Servo angle in range [0-180], (-1) if the servoId
      * is invalid.
      */
-    Units::Angle::Degree getServoAngle(ServosList servoId);
+    Boardcore::Units::Angle::Degree getServoAngle(ServosList servoId);
 
     /**
      * @brief Starts twirl (one servo is set to 0 and the other one is not).
diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h
index 4e8d51eefd7d589cb62c3554ef21a91b72e04c01..53a885511600d374961ecaf88bf8cfce72e9ff3a 100644
--- a/src/Parafoil/BoardScheduler.h
+++ b/src/Parafoil/BoardScheduler.h
@@ -56,6 +56,8 @@ public:
     Boardcore::TaskScheduler& sensors() { return high; }
     Boardcore::TaskScheduler& pinHandler() { return high; }
     Boardcore::TaskScheduler& altitudeTrigger() { return medium; }
+    Boardcore::TaskScheduler& wingController() { return medium; }
+    Boardcore::TaskScheduler& windEstimation() { return low; }
     Boardcore::TaskScheduler& actuators() { return low; }
 
     static Priority::PriorityLevel flightModeManagerPriority()
diff --git a/src/Parafoil/Configs/FlightModeManagerConfig.h b/src/Parafoil/Configs/FlightModeManagerConfig.h
index 2aed44f469f3af11af476dcdd1ea02c0a462378c..e616a5c8a9eeb148bcc100bbe00ac93eeb80acfd 100644
--- a/src/Parafoil/Configs/FlightModeManagerConfig.h
+++ b/src/Parafoil/Configs/FlightModeManagerConfig.h
@@ -22,6 +22,8 @@
 
 #pragma once
 
+#include <units/Time.h>
+
 #include <chrono>
 
 namespace Parafoil
@@ -31,10 +33,10 @@ namespace Config
 namespace FlightModeManager
 {
 
-/* linter-off */ using namespace std::chrono_literals;
+/* linter-off */ using namespace Boardcore::Units::Time;
 
-constexpr auto LOGGING_DELAY = 5s;
-constexpr auto CONTROL_DELAY = 5s;
+constexpr auto LOGGING_DELAY = 5_s;
+constexpr auto CONTROL_DELAY = 5_s;
 
 }  // namespace FlightModeManager
 }  // namespace Config
diff --git a/src/Parafoil/Configs/NASConfig.h b/src/Parafoil/Configs/NASConfig.h
index 6b0ee310d9c593c692d9ee02d17e13cb6648a957..6a9e041dcbbd471d71a8659ae36e9029b6a59d8f 100644
--- a/src/Parafoil/Configs/NASConfig.h
+++ b/src/Parafoil/Configs/NASConfig.h
@@ -24,7 +24,9 @@
 
 #include <algorithms/NAS/NASConfig.h>
 #include <common/ReferenceConfig.h>
+#include <units/Acceleration.h>
 #include <units/Frequency.h>
+#include <units/Time.h>
 
 namespace Parafoil
 {
@@ -33,16 +35,18 @@ namespace Config
 namespace NAS
 {
 
+/* linter off */ using namespace Boardcore::Units::Time;
 /* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace Boardcore::Units::Acceleration;
 
-constexpr Hertz UPDATE_RATE         = 50_hz;
-constexpr float UPDATE_RATE_SECONDS = 0.02;  // [s]
+constexpr auto UPDATE_RATE         = 50_hz;
+constexpr auto UPDATE_RATE_SECONDS = 0.02_s;
 
-constexpr int CALIBRATION_SAMPLES_COUNT       = 20;
-constexpr unsigned int CALIBRATION_SLEEP_TIME = 100;  // [ms]
+constexpr int CALIBRATION_SAMPLES_COUNT = 20;
+constexpr auto CALIBRATION_SLEEP_TIME   = 100_ms;
 
 static const Boardcore::NASConfig CONFIG = {
-    .T              = UPDATE_RATE_SECONDS,
+    .T              = UPDATE_RATE_SECONDS.value(),
     .SIGMA_BETA     = 0.0001,
     .SIGMA_W        = 0.0019,
     .SIGMA_ACC      = 0.202,
@@ -61,17 +65,14 @@ static const Boardcore::NASConfig CONFIG = {
     .SATS_NUM       = 6.0,
     .NED_MAG        = Common::ReferenceConfig::nedMag};
 
-// TODO: make sure this is correct!
-
 // Only use one out of every 50 samples (1 Hz)
 constexpr int MAGNETOMETER_DECIMATE = 50;
 
 // Maximum allowed acceleration to correct with GPS
-constexpr float DISABLE_GPS_ACCELERATION = 34.0f;  // [m/s^2].
+constexpr auto DISABLE_GPS_ACCELERATION_THRESHOLD = 34.0_mps2;
 
-// How much confidence (in m/s^2) to apply to the accelerometer to check if it
-// is 1g
-constexpr float ACCELERATION_1G_CONFIDENCE = 0.5;
+// How much confidence to apply to the accelerometer to check if it is 1g
+constexpr auto ACCELERATION_1G_CONFIDENCE = 0.5_mps2;
 // How many samples will determine that we are in fact measuring gravity
 // acceleration
 constexpr int ACCELERATION_1G_SAMPLES = 20;
diff --git a/src/Parafoil/Configs/WESConfig.h b/src/Parafoil/Configs/WESConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..4f6521100da10291c9e906b04aa8573af900c410
--- /dev/null
+++ b/src/Parafoil/Configs/WESConfig.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Federico Mandelli, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <miosix.h>
+#include <units/Time.h>
+
+namespace Parafoil
+{
+
+namespace Config
+{
+
+/**
+ * Configuration for the Wing Estimation Scheme (WES) algorithm.
+ */
+namespace WES
+{
+
+/* linter-off */ using namespace Boardcore::Units::Time;
+
+constexpr auto CALIBRATE = false;
+
+constexpr auto CALIBRATION_TIMEOUT = 5_s;  // time needed for the first loop
+constexpr auto ROTATION_PERIOD     = 10_s;
+constexpr auto CALIBRATION_SAMPLE_NUMBER =
+    20;  // number of samples to take in the first loop
+constexpr auto CALIBRATION_UPDATE_PERIOD =
+    CALIBRATION_TIMEOUT / CALIBRATION_SAMPLE_NUMBER;
+constexpr auto PREDICTION_UPDATE_PERIOD = 100_ms;  // update period of WES[ms]
+
+}  // namespace WES
+
+}  // namespace Config
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Configs/WingConfig.h b/src/Parafoil/Configs/WingConfig.h
index 68315dc5c836ea5fbb04069ab551eb630f8f2d5a..f1fde1c1aa8a4062f6f53552ac522473809d6869 100644
--- a/src/Parafoil/Configs/WingConfig.h
+++ b/src/Parafoil/Configs/WingConfig.h
@@ -25,10 +25,9 @@
 #include <units/Angle.h>
 #include <units/Frequency.h>
 #include <units/Length.h>
+#include <units/Time.h>
 #include <utils/Constants.h>
 
-#include <chrono>
-
 namespace Parafoil
 {
 namespace Config
@@ -36,15 +35,17 @@ namespace Config
 namespace Wing
 {
 
-/* linter off */ using namespace std::chrono_literals;
 /* linter off */ using namespace Boardcore::Units::Frequency;
 /* linter off */ using namespace Boardcore::Units::Length;
+/* linter off */ using namespace Boardcore::Units::Time;
 /* linter off */ using namespace Boardcore::Units::Angle;
 
-constexpr auto UPDATE_RATE             = 1_hz;
-constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15s;
-constexpr auto COMMAND_PERIOD          = 6s;
-constexpr auto WING_DECREMENT          = 30_deg;
+constexpr auto UPDATE_RATE                  = 1_hz;
+constexpr auto TARGET_UPDATE_RATE           = 10_hz;
+constexpr auto STRAIGHT_FLIGHT_TIMEOUT      = 15_s;
+constexpr auto PROGRESSIVE_ROTATION_TIMEOUT = 5_s;
+constexpr auto COMMAND_PERIOD               = 6_s;
+constexpr auto WING_DECREMENT               = 30_deg;
 
 /**
  * @brief The available algorithms for the wing controller.
@@ -56,6 +57,7 @@ enum class AlgorithmId : size_t
     SEQUENCE,
     ROTATION,
     PROGRESSIVE_ROTATION,
+    LAST  ///< Used to count the number of algorithms
 };
 
 namespace Default
diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
index 6e6048716f00ad61f50e0aa9c859c64cf31b24a2..94c15de3a31690dddb8bd37545efd3da453bd1b7 100644
--- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -31,7 +31,7 @@
 
 using namespace Boardcore;
 using namespace Common;
-using namespace std::chrono;
+using namespace Boardcore::Units::Time;
 namespace config = Parafoil::Config::FlightModeManager;
 
 namespace Parafoil
@@ -448,7 +448,7 @@ State FlightModeManager::FlyingWingDescent(const Event& event)
             // Send the event to the WingController
             controlDelayId = EventBroker::getInstance().postDelayed(
                 FLIGHT_WING_DESCENT, TOPIC_FLIGHT,
-                milliseconds{config::CONTROL_DELAY}.count());
+                Millisecond{config::CONTROL_DELAY}.value());
             return HANDLED;
         }
 
@@ -492,7 +492,7 @@ State FlightModeManager::Landed(const Event& event)
                                             TOPIC_FLIGHT);
             EventBroker::getInstance().postDelayed(
                 FMM_STOP_LOGGING, TOPIC_FMM,
-                milliseconds{config::LOGGING_DELAY}.count());
+                Millisecond{config::LOGGING_DELAY}.value());
 
             return HANDLED;
         }
@@ -543,4 +543,4 @@ void FlightModeManager::updateState(FlightModeManagerState newState)
     Logger::getInstance().log(status);
 }
 
-}  // namespace Parafoil
\ No newline at end of file
+}  // namespace Parafoil
diff --git a/src/Parafoil/StateMachines/NASController/NASController.cpp b/src/Parafoil/StateMachines/NASController/NASController.cpp
index c4cf7f5c9b17a0c726a06207862abb91b537b705..3317629072692bf12b5f11f539934044f53eed75 100644
--- a/src/Parafoil/StateMachines/NASController/NASController.cpp
+++ b/src/Parafoil/StateMachines/NASController/NASController.cpp
@@ -31,6 +31,8 @@
 #include <utils/SkyQuaternion/SkyQuaternion.h>
 
 using namespace Boardcore;
+using namespace Boardcore::Units::Time;
+using namespace Boardcore::Units::Acceleration;
 using namespace Eigen;
 using namespace Common;
 namespace config = Parafoil::Config::NAS;
@@ -220,7 +222,7 @@ void NASController::calibrate()
 
         baroSum += baro.pressure;
 
-        Thread::sleep(config::CALIBRATION_SLEEP_TIME);
+        Thread::sleep(Millisecond{config::CALIBRATION_SLEEP_TIME}.value());
     }
 
     Vector3f meanAcc = accSum / config::CALIBRATION_SAMPLES_COUNT;
@@ -239,7 +241,7 @@ void NASController::calibrate()
     ReferenceValues reference = nas.getReferenceValues();
     reference.refPressure     = meanBaro;
     reference.refAltitude     = Aeroutils::relAltitude(
-            reference.refPressure, reference.mslPressure, reference.mslTemperature);
+        reference.refPressure, reference.mslPressure, reference.mslTemperature);
 
     // Also update the reference with the GPS if we have fix
     UBXGPSData gps = sensors->getUBXGPSLastSample();
@@ -261,9 +263,7 @@ void NASController::update()
 {
     // Update the NAS state only if the FSM is active
     if (state != NASControllerState::ACTIVE)
-    {
         return;
-    }
 
     auto* sensors = getModule<Sensors>();
 
@@ -272,8 +272,8 @@ void NASController::update()
     auto baro = sensors->getLPS22DFLastSample();
 
     // Calculate acceleration
-    Vector3f acc    = static_cast<AccelerometerData>(imu);
-    float accLength = acc.norm();
+    Vector3f acc   = static_cast<AccelerometerData>(imu);
+    auto accLength = MeterPerSecondSquared{acc.norm()};
 
     miosix::Lock<miosix::FastMutex> l(nasMutex);
 
@@ -282,36 +282,28 @@ void NASController::update()
     nas.predictAcc(imu);
 
     if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 &&
-        accLength < Config::NAS::DISABLE_GPS_ACCELERATION)
+        accLength < Config::NAS::DISABLE_GPS_ACCELERATION_THRESHOLD)
     {
         nas.correctGPS(gps);
     }
 
     if (lastBaroTimestamp < baro.pressureTimestamp)
-    {
         nas.correctBaro(baro.pressure);
-    }
 
     // Correct with accelerometer if the acceleration is in specs
     if (lastAccTimestamp < imu.accelerationTimestamp && acc1g)
-    {
         nas.correctAcc(imu);
-    }
 
     // Check if the accelerometer is measuring 1g
-    if (accLength <
-            (Constants::g + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) &&
-        accLength >
-            (Constants::g - Config::NAS::ACCELERATION_1G_CONFIDENCE / 2))
+    if (accLength < (MeterPerSecondSquared{G{1}} +
+                     Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) &&
+        accLength > (MeterPerSecondSquared{G{1}} -
+                     Config::NAS::ACCELERATION_1G_CONFIDENCE / 2))
     {
         if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES)
-        {
             acc1gSamplesCount++;
-        }
         else
-        {
             acc1g = true;
-        }
     }
     else
     {
@@ -326,7 +318,7 @@ void NASController::update()
     lastBaroTimestamp = baro.pressureTimestamp;
 
     auto state = nas.getState();
-    auto ref   = nas.getReferenceValues();
+    // auto ref   = nas.getReferenceValues();
 
     Logger::getInstance().log(state);
 }
diff --git a/src/Parafoil/StateMachines/WingController/WingController.cpp b/src/Parafoil/StateMachines/WingController/WingController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90ab7e46e88bf061afda0b42e8763c70bb91d377
--- /dev/null
+++ b/src/Parafoil/StateMachines/WingController/WingController.cpp
@@ -0,0 +1,619 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#include "WingController.h"
+
+#include <Parafoil/BoardScheduler.h>
+#include <Parafoil/Configs/ActuatorsConfig.h>
+#include <Parafoil/Configs/WESConfig.h>
+#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/StateMachines/NASController/NASController.h>
+#include <Parafoil/WindEstimation/WindEstimation.h>
+#include <Parafoil/Wing/AutomaticWingAlgorithm.h>
+#include <Parafoil/Wing/FileWingAlgorithm.h>
+#include <Parafoil/Wing/WingAlgorithm.h>
+#include <Parafoil/Wing/WingAlgorithmData.h>
+#include <Parafoil/Wing/WingTargetPositionData.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <units/Length.h>
+#include <units/Time.h>
+
+using namespace Boardcore;
+using namespace Boardcore::Units::Time;
+using namespace Boardcore::Units::Length;
+using namespace Common;
+using namespace Parafoil::Config;
+
+namespace Parafoil
+{
+WingController::WingController()
+    : 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);
+
+    loadAlgorithms();
+}
+
+WingController::~WingController()
+{
+    EventBroker::getInstance().unsubscribe(this);
+}
+
+State WingController::Idle(const Boardcore::Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            updateState(WingControllerState::IDLE);
+            return HANDLED;
+        }
+
+        case FLIGHT_WING_DESCENT:
+        {
+            return transition(&WingController::Flying);
+        }
+
+        case EV_EMPTY:
+        {
+            return tranSuper(&WingController::state_top);
+        }
+
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+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::FlyingCalibration);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&WingController::OnGround);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State WingController::FlyingCalibration(const Boardcore::Event& event)
+{
+    static uint16_t calibrationTimeoutEventId;
+
+    switch (event)
+    {
+        case EV_ENTRY:  // starts twirling and calibration wes
+        {
+            updateState(WingControllerState::FLYING_CALIBRATION);
+
+            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::Flying);
+        }
+        case DPL_SERVO_ACTUATION_DETECTED:
+        {
+            resetWing();
+            calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+                DPL_WIGGLE, TOPIC_DPL, 1000);
+
+            return HANDLED;
+        }
+        case DPL_WIGGLE:
+        {
+            flareWing();
+            calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+                DPL_NC_OPEN, TOPIC_DPL, 2000);
+
+            return HANDLED;
+        }
+        case DPL_NC_OPEN:
+        {
+            resetWing();
+            if (WES::CALIBRATE)
+            {
+                calibrationTimeoutEventId =
+                    EventBroker::getInstance().postDelayed(
+                        DPL_WES_CAL_DONE, TOPIC_DPL,
+                        Millisecond{WES::CALIBRATION_TIMEOUT}.value());
+                getModule<WindEstimation>()->startAlgorithm();
+
+                getModule<Actuators>()->startTwirl();
+            }
+
+            return transition(&WingController::FlyingControlledDescent);
+        }
+        case DPL_WES_CAL_DONE:
+        {
+            getModule<Actuators>()->stopTwirl();
+
+            return transition(&WingController::FlyingControlledDescent);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State WingController::FlyingControlledDescent(const Boardcore::Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:  // start automatic algorithms
+        {
+            updateState(WingControllerState::FLYING_CONTROLLED_DESCENT);
+
+            startAlgorithm();
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&WingController::Flying);
+        }
+        case WING_ALGORITHM_ENDED:
+        {
+            return transition(&WingController::OnGround);
+        }
+        case EV_EXIT:
+        {
+            stopAlgorithm();
+
+            getModule<WindEstimation>()->stopAlgorithm();
+            getModule<WindEstimation>()->stopCalibration();
+
+            return HANDLED;
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State WingController::OnGround(const Boardcore::Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            updateState(WingControllerState::ON_GROUND);
+
+            // disable servos
+            getModule<Actuators>()->disableServo(PARAFOIL_LEFT_SERVO);
+            getModule<Actuators>()->disableServo(PARAFOIL_RIGHT_SERVO);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&WingController::state_top);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+void WingController::inject(DependencyInjector& injector)
+{
+    for (auto& algorithm : algorithms)
+        algorithm->inject(injector);
+    Super::inject(injector);
+}
+
+bool WingController::start()
+{
+    auto& scheduler = getModule<BoardScheduler>()->wingController();
+
+    bool algoStarted = std::all_of(
+        algorithms.begin(), algorithms.end(),
+        [](const std::unique_ptr<Parafoil::WingAlgorithm>& algorithm)
+        { return algorithm->init(); });
+
+    if (!algoStarted)
+    {
+        LOG_ERR(logger, "Failed to initialize wing algorithms");
+        return false;
+    }
+
+    auto updateTask =
+        scheduler.addTask([this] { update(); }, Wing::UPDATE_RATE);
+
+    if (updateTask == 0)
+    {
+        LOG_ERR(logger, "Failed to start wing controller update task");
+        return false;
+    }
+
+    auto activeTargetTask = scheduler.addTask(
+        [this]
+        {
+            // Do not update the active target if the wing is not flying
+            if (!running)
+                return;
+
+            auto nasState  = getModule<NASController>()->getNasState();
+            float altitude = -nasState.d;
+            emGuidance.updateActiveTarget(Meter{altitude});
+        },
+        Wing::TARGET_UPDATE_RATE);
+
+    if (activeTargetTask == 0)
+    {
+        LOG_ERR(logger, "Failed to add early maneuver active target task");
+        return false;
+    }
+
+    if (!HSM::start())
+    {
+        LOG_ERR(logger, "Failed to start WingController HSM active object");
+        return false;
+    }
+
+    started = true;
+    return true;
+}
+
+bool WingController::isStarted() { return started; }
+
+WingControllerState WingController::getState() { return state; }
+
+Eigen::Vector2f WingController::getTargetCoordinates()
+{
+    return targetPositionGEO.load();
+}
+
+bool WingController::setTargetCoordinates(float latitude, float longitude)
+{
+    // Allow changing the target position in the IDLE state only
+    if (state != WingControllerState::IDLE)
+        return false;
+
+    targetPositionGEO = Coordinates{latitude, longitude};
+
+    // Log early maneuver points to highlight any discrepancies if any
+    auto earlyManeuverPoints = getEarlyManeuverPoints();
+
+    auto data = WingTargetPositionData{
+        .targetLat = latitude,
+        .targetLon = longitude,
+        .targetN   = earlyManeuverPoints.targetN,
+        .targetE   = earlyManeuverPoints.targetE,
+        .emcN      = earlyManeuverPoints.emcN,
+        .emcE      = earlyManeuverPoints.emcE,
+        .m1N       = earlyManeuverPoints.m1N,
+        .m1E       = earlyManeuverPoints.m1E,
+        .m2N       = earlyManeuverPoints.m2N,
+        .m2E       = earlyManeuverPoints.m2E,
+    };
+    Logger::getInstance().log(data);
+
+    return true;
+}
+
+bool WingController::selectAlgorithm(Wing::AlgorithmId id)
+{
+    // Allow changing the algorithm in the IDLE state only
+    if (state != WingControllerState::IDLE)
+        return false;
+
+    switch (id)
+    {
+        case Wing::AlgorithmId::EARLY_MANEUVER:
+        case Wing::AlgorithmId::CLOSED_LOOP:
+        case Wing::AlgorithmId::ROTATION:
+        {
+            selectedAlgorithm = id;
+
+            auto data = WingControllerAlgorithmData{
+                .timestamp = TimestampTimer::getTimestamp(),
+                .algorithm = static_cast<uint8_t>(id),
+            };
+            Logger::getInstance().log(data);
+
+            return true;
+        }
+
+        default:
+        {
+            return false;
+        }
+    }
+}
+
+EarlyManeuversPoints WingController::getEarlyManeuverPoints()
+{
+    return emGuidance.getPoints();
+}
+
+Eigen::Vector2f WingController::getActiveTarget()
+{
+    return emGuidance.getActiveTarget();
+}
+
+void WingController::loadAlgorithms()
+{
+    using namespace Wing;
+    using namespace Actuators;
+
+    // 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);
+
+    // 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);
+
+    // Sequence
+    {
+        auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO,
+                                                         PARAFOIL_RIGHT_SERVO);
+        WingAlgorithmData step;
+
+        step.timestamp   = 0;
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = 120_deg;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{STRAIGHT_FLIGHT_TIMEOUT}.value();
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = 0_deg;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{STRAIGHT_FLIGHT_TIMEOUT}.value();
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = 0_deg;
+        algorithm->addStep(step);
+
+        algorithms[static_cast<size_t>(AlgorithmId::SEQUENCE)] =
+            std::move(algorithm);
+    }
+
+    // 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_deg;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = RightServo::ROTATION / 2;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = 0_deg;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+        step.servo1Angle = LeftServo::ROTATION;
+        step.servo2Angle = RightServo::ROTATION;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = RightServo::ROTATION / 2;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = 0_deg;
+        algorithm->addStep(step);
+
+        step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value();
+        step.servo1Angle = 0_deg;
+        step.servo2Angle = 0_deg;
+        algorithm->addStep(step);
+
+        algorithms[static_cast<size_t>(AlgorithmId::ROTATION)] =
+            std::move(algorithm);
+    }
+
+    // Progressive rotation
+    {
+        auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO,
+                                                         PARAFOIL_RIGHT_SERVO);
+        WingAlgorithmData step;
+
+        step.timestamp = Microsecond(PROGRESSIVE_ROTATION_TIMEOUT).value();
+
+        for (auto angle = 150_deg; angle >= 0_deg; angle -= WING_DECREMENT)
+        {
+            step.servo1Angle = angle;
+            step.servo2Angle = 0_deg;
+            algorithm->addStep(step);
+            step.timestamp += Microsecond(COMMAND_PERIOD).value();
+
+            step.servo1Angle = 0_deg;
+            step.servo2Angle = angle;
+            algorithm->addStep(step);
+            step.timestamp += Microsecond(COMMAND_PERIOD).value();
+        }
+
+        algorithms[static_cast<size_t>(AlgorithmId::PROGRESSIVE_ROTATION)] =
+            std::move(algorithm);
+    }
+}
+
+WingAlgorithm& WingController::getCurrentAlgorithm()
+{
+    auto index = static_cast<size_t>(selectedAlgorithm.load());
+    return *algorithms[index].get();
+}
+
+void WingController::startAlgorithm()
+{
+    updateEarlyManeuverPoints();
+    running = true;
+
+    getCurrentAlgorithm().begin();
+}
+
+void WingController::stopAlgorithm()
+{
+    if (running)
+    {
+        running = false;
+
+        getCurrentAlgorithm().end();
+    }
+}
+
+void WingController::updateEarlyManeuverPoints()
+{
+    using namespace Eigen;
+
+    auto nas       = getModule<NASController>();
+    auto nasState  = nas->getNasState();
+    auto nasRef    = nas->getReferenceValues();
+    auto targetGEO = targetPositionGEO.load();
+
+    Vector2f currentPositionNED = {nasState.n, nasState.e};
+    Vector2f targetNED          = Aeroutils::geodetic2NED(
+        targetGEO, {nasRef.refLatitude, nasRef.refLongitude});
+
+    Vector2f targetOffsetNED = targetNED - currentPositionNED;
+    Vector2f normPoint       = targetOffsetNED / targetOffsetNED.norm();
+    float psi0               = atan2(normPoint.y(), normPoint.x());
+
+    float distFromCenterline = 20;  // the distance that the M1 and M2
+                                    // points must have from the center line
+
+    // Calculate the angle between the lines <NED Origin, target> and <NED
+    // Origin, M1> This angle is the same for M2 since is symmetric to M1
+    // relatively to the center line
+    float psiMan = atan2(distFromCenterline, targetOffsetNED.norm());
+
+    float maneuverPointsMagnitude = distFromCenterline / sin(psiMan);
+    float m2Angle                 = psi0 + psiMan;
+    float m1Angle                 = psi0 - psiMan;
+
+    // EMC is calculated as target * 1.2
+    Vector2f emcPosition = targetOffsetNED * 1.2 + currentPositionNED;
+
+    Vector2f m1Position =
+        Vector2f{cos(m1Angle), sin(m1Angle)} * maneuverPointsMagnitude +
+        currentPositionNED;
+
+    Vector2f m2Position =
+        Vector2f{cos(m2Angle), sin(m2Angle)} * maneuverPointsMagnitude +
+        currentPositionNED;
+
+    emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position);
+    clGuidance.setPoints(targetNED);
+
+    // Log the updated points
+    auto data = WingTargetPositionData{
+        .targetLat = targetGEO.latitude,
+        .targetLon = targetGEO.longitude,
+        .targetN   = targetNED.x(),
+        .targetE   = targetNED.y(),
+        .emcN      = emcPosition.x(),
+        .emcE      = emcPosition.y(),
+        .m1N       = m1Position.x(),
+        .m1E       = m1Position.y(),
+        .m2N       = m2Position.x(),
+        .m2E       = m2Position.y(),
+    };
+    Logger::getInstance().log(data);
+}
+
+void WingController::update()
+{
+    if (running)
+        getCurrentAlgorithm().step();
+}
+
+void WingController::flareWing()
+{
+    getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f);
+    getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f);
+}
+
+void WingController::resetWing()
+{
+    getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f);
+    getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
+}
+
+void WingController::updateState(WingControllerState newState)
+{
+    state = newState;
+
+    auto status = WingControllerStatus{
+        .timestamp = TimestampTimer::getTimestamp(),
+        .state     = newState,
+    };
+    Logger::getInstance().log(status);
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/StateMachines/WingController/WingController.h b/src/Parafoil/StateMachines/WingController/WingController.h
new file mode 100644
index 0000000000000000000000000000000000000000..a38d95ece554b46e72d0acd7a15a377f90173ef2
--- /dev/null
+++ b/src/Parafoil/StateMachines/WingController/WingController.h
@@ -0,0 +1,267 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
+#include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
+#include <Parafoil/Wing/WingAlgorithm.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/HSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include <Eigen/Core>
+#include <atomic>
+
+#include "WingControllerData.h"
+
+/**
+ * @brief This class allows the user to select the wing algorithm
+ * that has to be used during the tests. It also registers his
+ * dedicated function in the task scheduler in order to be
+ * executed every fixed period and to update the two servos position
+ * depending on the selected algorithm.
+ *
+ * Use case example:
+ * controller = new WingController(scheduler);
+ *
+ * controller.addAlgorithm("filename");
+ * OR
+ * controller.addAlgorithm(algorithm);
+ *
+ * controller.selectAlgorithm(index);
+ *
+ * controller.start();
+ * controller.stop();  //If you want to abort the execution
+ * controller.start(); //If you want to start again from the beginning
+ */
+
+namespace Parafoil
+{
+class BoardScheduler;
+class Actuators;
+class NASController;
+class FlightStatsRecorder;
+class WindEstimation;
+
+/**
+ * State machine that manages the wings of the Parafoil.
+ *
+ * HSM Schema:
+ *
+ * Idle
+ *
+ * Flying
+ * ├── FlyingCalibration
+ * ├── FlyingDeployment
+ * └── FlyingControlledDescent
+ *
+ * OnGround
+ */
+class WingController
+    : public Boardcore::HSM<WingController>,
+      public Boardcore::InjectableWithDeps<BoardScheduler, Actuators,
+                                           NASController, FlightStatsRecorder,
+                                           WindEstimation>
+{
+public:
+    /**
+     * @brief Initializes the wing controller.
+     *
+     * Sets the initial FSM state to idle, subscribes to DPL and flight events
+     * and instantiates the wing algorithms.
+     */
+    WingController();
+
+    /**
+     * @brief Destroy the Wing Controller object.
+     */
+    ~WingController();
+
+    // HSM states
+    Boardcore::State Idle(const Boardcore::Event& event);
+    Boardcore::State Flying(const Boardcore::Event& event);
+    Boardcore::State FlyingCalibration(const Boardcore::Event& event);
+    Boardcore::State FlyingDeployment(const Boardcore::Event& event);
+    Boardcore::State FlyingControlledDescent(const Boardcore::Event& event);
+    Boardcore::State OnGround(const Boardcore::Event& event);
+
+    /**
+     * @brief Override the inject method to inject dependencies into the
+     * algorithms, which are instantiated later than top-level modules.
+     */
+    void inject(Boardcore::DependencyInjector& injector) override;
+
+    bool start() override;
+
+    bool isStarted();
+
+    WingControllerState getState();
+
+    /**
+     * @brief Returns the target coordinates.
+     * @return The GEO coordinates of the active target.
+     */
+    Eigen::Vector2f getTargetCoordinates();
+
+    /**
+     * @brief Sets the target coordinates.
+     */
+    bool setTargetCoordinates(float latitude, float longitude);
+
+    /**
+     * @brief Returns the selected algorithm.
+     */
+    uint8_t getSelectedAlgorithm();
+
+    /**
+     * @brief Changes the selected algorithm.
+     * @return Whether the provided index selected a valid algorithm.
+     */
+    bool selectAlgorithm(Config::Wing::AlgorithmId id);
+
+    /**
+     * @brief Returns the currently set early maneuver points.
+     */
+    EarlyManeuversPoints getEarlyManeuverPoints();
+
+    /**
+     * @brief Returns the early maneuver active target.
+     */
+    Eigen::Vector2f getActiveTarget();
+
+    /**
+     * @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
+     * @param[out] heading Saves the heading vector for logging purposes
+     *
+     * @returns the yaw angle of the parafoil in rad
+     */
+    Boardcore::Units::Angle::Radian calculateTargetAngle(
+        const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
+    {
+        return emGuidance.calculateTargetAngle(currentPositionNED, heading);
+    }
+
+private:
+    /**
+     * @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.
+     */
+    void loadAlgorithms();
+
+    /**
+     * @brief Returns the currently selected algorithm.
+     */
+    WingAlgorithm& getCurrentAlgorithm();
+
+    /**
+     * @brief Starts the currently selected algorithm. If the algorithm is
+     * already running, it resets the algorithm.
+     */
+    void startAlgorithm();
+
+    /**
+     * @brief Stops the currently selected algorithm.
+     */
+    void stopAlgorithm();
+
+    /**
+     * @brief Update early maneuver guidance points (EMC, M1, M2) based on the
+     * current position and the target position.
+     */
+    void updateEarlyManeuverPoints();
+
+    /**
+     * @brief Periodic update method that steps the currently selected
+     * algorithm.
+     */
+    void update();
+
+    /**
+     * @brief Flare the wing.
+     * Pulls the two ropes as skydiving people do.
+     */
+    void flareWing();
+
+    /**
+     * @brief Twirl the wing to the left.
+     */
+    void twirlWing();
+
+    /**
+     * @brief Reset the wing to the initial position.
+     */
+    void resetWing();
+
+    void updateState(WingControllerState newState);
+
+    struct Coordinates
+    {
+        float latitude;
+        float longitude;
+
+        operator Eigen::Vector2f() const { return {latitude, longitude}; }
+    };
+
+    std::atomic<Coordinates> targetPositionGEO{Coordinates{
+        .latitude  = Config::Wing::Default::TARGET_LAT,
+        .longitude = Config::Wing::Default::TARGET_LON,
+    }};
+
+    std::atomic<Config::Wing::AlgorithmId> selectedAlgorithm{
+        Config::Wing::Default::ALGORITHM};
+
+    std::atomic<WingControllerState> state{WingControllerState::IDLE};
+
+    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
+     * AutomaticWingAlgorithm
+     */
+    EarlyManeuversGuidanceAlgorithm emGuidance;
+
+    /**
+     * @brief Instance of the Closed Loop Guidance Algorithm used by
+     * AutomaticWingAlgorithm
+     */
+    ClosedLoopGuidanceAlgorithm clGuidance;
+
+    std::atomic<bool> started{false};
+    std::atomic<bool> running{false};  ///< Whether the algorithm is running
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("WingController");
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/StateMachines/WingController/WingControllerData.h b/src/Parafoil/StateMachines/WingController/WingControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b239e58463db25f9d5692b5cbc326cbe493c8f0
--- /dev/null
+++ b/src/Parafoil/StateMachines/WingController/WingControllerData.h
@@ -0,0 +1,67 @@
+/* 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
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <iostream>
+#include <string>
+
+namespace Parafoil
+{
+
+enum class WingControllerState : uint8_t
+{
+    IDLE = 0,
+    FLYING_CALIBRATION,
+    FLYING_DEPLOYMENT,
+    FLYING_CONTROLLED_DESCENT,
+    ON_GROUND,
+};
+
+struct WingControllerStatus
+{
+    uint64_t timestamp        = 0;
+    WingControllerState state = WingControllerState::IDLE;
+
+    static std::string header() { return "timestamp,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "\n";
+    }
+};
+
+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 Parafoil
diff --git a/src/Parafoil/WindEstimation/WindEstimation.cpp b/src/Parafoil/WindEstimation/WindEstimation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3fec873993b0dd8a8f3db23ca6d87515607cbaf6
--- /dev/null
+++ b/src/Parafoil/WindEstimation/WindEstimation.cpp
@@ -0,0 +1,271 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Federico Mandelli, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "WindEstimation.h"
+
+#include <Parafoil/BoardScheduler.h>
+#include <Parafoil/Configs/WESConfig.h>
+#include <Parafoil/Sensors/Sensors.h>
+
+using namespace Boardcore;
+using namespace Parafoil::Config;
+using namespace Units::Speed;
+
+namespace Parafoil
+{
+WindEstimation::WindEstimation() { funv << 1.0f, 0.0f, 0.0f, 1.0f; }
+
+WindEstimation::~WindEstimation()
+{
+    stopAlgorithm();
+    stopCalibration();
+}
+
+bool WindEstimation::start()
+{
+    auto& scheduler = getModule<BoardScheduler>()->windEstimation();
+
+    auto calibrationTask = scheduler.addTask([this] { updateCalibration(); },
+                                             WES::CALIBRATION_UPDATE_PERIOD);
+
+    if (calibrationTask == 0)
+    {
+        LOG_ERR(logger, "Failed to start wind estimation calibration task");
+        return false;
+    }
+
+    auto algorithmTask = scheduler.addTask([this] { updateAlgorithm(); },
+                                           WES::PREDICTION_UPDATE_PERIOD);
+
+    if (algorithmTask == 0)
+    {
+        LOG_ERR(logger, "Failed to start wind estimation algorithm task");
+        return false;
+    }
+
+    started = true;
+    return true;
+}
+
+bool WindEstimation::isStarted() { return started; }
+
+void WindEstimation::startCalibration()
+{
+    if (!calibrating)
+    {
+        calibrating        = true;
+        nSampleCalibration = 0;
+
+        velocity.vn = 0_mps;
+        velocity.ve = 0_mps;
+
+        speedSquared = 0;
+
+        LOG_INFO(logger, "Wind estimation calibration started");
+    }
+    else
+    {
+        LOG_WARN(logger, "Wind estimation calibration already started");
+    }
+}
+
+void WindEstimation::stopCalibration()
+{
+    if (calibrating)
+    {
+        LOG_INFO(logger, "Wind estimation calibration stopped");
+
+        if (!running)
+        {
+            miosix::Lock<FastMutex> l(mutex);
+            wind = calibratingWind;
+        }
+        else
+        {
+            LOG_WARN(logger,
+                     "Wind estimation algorithm is running, calibration wind "
+                     "will not be applied");
+        }
+
+        logStatus();
+        calibrating = false;
+    }
+    else
+    {
+        LOG_WARN(logger, "Wind estimation calibration already stopped");
+    }
+}
+
+void WindEstimation::updateCalibration()
+{
+    if (calibrating)
+    {
+        auto gps = getModule<Sensors>()->getUBXGPSLastSample();
+
+        if (gps.fix != 0)
+        {
+            if (nSampleCalibration < WES::CALIBRATION_SAMPLE_NUMBER)
+            {
+                auto gpsVelocity =
+                    GeoVelocity{MeterPerSecond{gps.velocityNorth},
+                                MeterPerSecond{gps.velocityEast}};
+
+                calibrationMatrix(nSampleCalibration, 0) =
+                    gpsVelocity.vn.value();
+                calibrationMatrix(nSampleCalibration, 1) =
+                    gpsVelocity.ve.value();
+                calibrationV2(nSampleCalibration) = gpsVelocity.normSquared();
+
+                velocity.vn += gpsVelocity.vn;
+                velocity.ve += gpsVelocity.ve;
+                speedSquared += gpsVelocity.normSquared();
+
+                nSampleCalibration++;
+            }
+            else
+            {
+                velocity.vn /= nSampleCalibration;
+                velocity.ve /= nSampleCalibration;
+                speedSquared /= nSampleCalibration;
+
+                // Update calibration matrix
+                for (int i = 0; i < nSampleCalibration; i++)
+                {
+                    calibrationMatrix(i, 0) =
+                        calibrationMatrix(i, 0) - velocity.vn.value();
+                    calibrationMatrix(i, 1) =
+                        calibrationMatrix(i, 1) - velocity.ve.value();
+                    calibrationV2(i) = 0.5f * (calibrationV2(i) - speedSquared);
+                }
+
+                Eigen::MatrixXf calibrationMatrixT =
+                    calibrationMatrix.transpose();
+                auto calibration =
+                    (calibrationMatrixT * calibrationMatrix)
+                        .ldlt()
+                        .solve(calibrationMatrixT * calibrationV2);
+                calibratingWind.vn = MeterPerSecond(calibration(0));
+                calibratingWind.ve = MeterPerSecond(calibration(1));
+
+                stopCalibration();
+                startAlgorithm();
+            }
+        }
+    }
+}
+
+void WindEstimation::startAlgorithm()
+{
+    if (!running)
+    {
+        running          = true;
+        nSampleAlgorithm = nSampleCalibration;
+
+        LOG_INFO(logger, "Wind estimation algorithm started");
+    }
+    else
+    {
+        LOG_WARN(logger, "Wind estimation algorithm already started");
+    }
+}
+
+void WindEstimation::stopAlgorithm()
+{
+    if (running)
+    {
+        running = false;
+
+        LOG_INFO(logger, "Wind estimation algorithm stopped");
+    }
+    else
+    {
+        LOG_WARN(logger, "Wind estimation algorithm already stopped");
+    }
+}
+
+void WindEstimation::updateAlgorithm()
+{
+    if (running)
+    {
+        auto gps = getModule<Sensors>()->getUBXGPSLastSample();
+        if (gps.fix != 0)
+        {
+            Eigen::Vector2f phi;
+            Eigen::Matrix<float, 1, 2> phiT;
+            Eigen::Vector2f temp;
+
+            float y;
+
+            auto gpsVelocity = GeoVelocity{MeterPerSecond{gps.velocityNorth},
+                                           MeterPerSecond{gps.velocityEast}};
+
+            // update avg
+            nSampleAlgorithm++;
+            velocity.vn = (velocity.vn * nSampleAlgorithm + gpsVelocity.vn) /
+                          (nSampleAlgorithm + 1);
+            velocity.ve = (velocity.ve * nSampleAlgorithm + gpsVelocity.ve) /
+                          (nSampleAlgorithm + 1);
+            speedSquared = (speedSquared * nSampleAlgorithm +
+                            (gpsVelocity.normSquared())) /
+                           (nSampleAlgorithm + 1);
+            phi(0) = gpsVelocity.vn.value() - velocity.vn.value();
+            phi(1) = gpsVelocity.ve.value() - velocity.ve.value();
+            y      = 0.5f * ((gpsVelocity.normSquared()) - speedSquared);
+
+            phiT = phi.transpose();
+            funv =
+                (funv - (funv * phi * phiT * funv) / (1 + (phiT * funv * phi)));
+            temp = (0.5 * (funv + funv.transpose()) * phi) *
+                   (y - phiT * getPrediction().asVector());
+
+            {
+                miosix::Lock<FastMutex> l(mutex);
+                wind.vn = MeterPerSecond(wind.vn.value() + temp(0));
+                wind.ve = MeterPerSecond(wind.ve.value() + temp(1));
+            }
+
+            logStatus();
+        }
+    }
+}
+
+GeoVelocity WindEstimation::getPrediction()
+{
+    miosix::Lock<FastMutex> l(mutex);
+    return wind;
+}
+
+void WindEstimation::logStatus()
+{
+    auto timestamp = TimestampTimer::getTimestamp();
+
+    WindEstimationData data{
+        .timestamp     = timestamp,
+        .velocityNorth = wind.vn,
+        .velocityEast  = wind.ve,
+        .calibration   = calibrating,
+    };
+
+    Logger::getInstance().log(data);
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/WindEstimation/WindEstimation.h b/src/Parafoil/WindEstimation/WindEstimation.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9277c71f6eafc25c54bf53bee7396903cc586ee
--- /dev/null
+++ b/src/Parafoil/WindEstimation/WindEstimation.h
@@ -0,0 +1,134 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Federico Mandelli, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Parafoil/BoardScheduler.h>
+#include <Parafoil/Configs/WESConfig.h>
+#include <Parafoil/Sensors/Sensors.h>
+#include <diagnostic/PrintLogger.h>
+#include <logger/Logger.h>
+
+#include <Eigen/Core>
+#include <atomic>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "WindEstimationData.h"
+
+namespace Parafoil
+{
+class BoardScheduler;
+class Sensors;
+
+/**
+ * @brief This class implements the Wind Estimation Scheme(WES) prediction
+ * algorithm, the first part is the initial setup, and then the continuos
+ * algorithms runs;
+ */
+class WindEstimation
+    : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>
+{
+public:
+    WindEstimation();
+
+    ~WindEstimation();
+
+    /**
+     * @brief Starts the wind estimation module, subscribing the calibration and
+     * algorithm update tasks to the scheduler.
+     * @return true if the module started correctly, false otherwise.
+     */
+    bool start();
+
+    bool isStarted();
+
+    /**
+     * @brief Starts the calibration of the wind estimation.
+     */
+    void startCalibration();
+
+    /**
+     * @brief Stops the calibration of the wind estimation. Overrides the
+     * current wind estimation with the calibrated one.
+     */
+    void stopCalibration();
+
+    /**
+     * @brief Starts the algorithm of the wind estimation.
+     */
+    void startAlgorithm();
+
+    /**
+     * @brief Stops the algorithm of the wind estimation.
+     */
+    void stopAlgorithm();
+
+    /**
+     * @brief Returns the current wind estimation prediction.
+     */
+    GeoVelocity getPrediction();
+
+private:
+    /**
+     * @brief Updates the calibration matrix with the new calibration values.
+     * Automatically stops the calibration when the calibration matrix is full
+     * and starts the algorithm.
+     */
+    void updateCalibration();
+
+    /**
+     * @brief Updates the wind matrix with the updated wind prediction values.
+     */
+    void updateAlgorithm();
+
+    /**
+     * @brief Logs the current prediction.
+     */
+    void logStatus();
+
+    GeoVelocity calibratingWind;  ///< Wind currently being calibrated
+    GeoVelocity wind;             ///< Wind after calibration
+
+    // Calibration & Algorithm variables
+    uint8_t nSampleCalibration = 0;
+    uint8_t nSampleAlgorithm   = 0;
+
+    Eigen::Matrix<float, Config::WES::CALIBRATION_SAMPLE_NUMBER, 2>
+        calibrationMatrix;
+    Eigen::Vector<float, Config::WES::CALIBRATION_SAMPLE_NUMBER> calibrationV2;
+    Eigen::Matrix2f funv;
+
+    GeoVelocity velocity;   ///< Wind velocity (North, East)
+    float speedSquared{0};  ///< Wind speed squared
+
+    miosix::FastMutex mutex;
+
+    // Status variables
+    std::atomic<bool> started{false};
+    std::atomic<bool> running{false};  ///< Whether the algorithm is running
+    std::atomic<bool> calibrating{
+        false};  ///< Whether the algorithm is being calibrated
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("WindEstimation");
+};
+}  // namespace Parafoil
diff --git a/src/Parafoil/WindEstimation/WindEstimationData.h b/src/Parafoil/WindEstimation/WindEstimationData.h
new file mode 100644
index 0000000000000000000000000000000000000000..60e9668095b11b24d5cf3883a85eb4ea40c76e92
--- /dev/null
+++ b/src/Parafoil/WindEstimation/WindEstimationData.h
@@ -0,0 +1,73 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <units/Speed.h>
+
+#include <Eigen/Core>
+#include <iostream>
+#include <string>
+
+namespace Parafoil
+{
+
+/**
+ * Wind Estimation Scheme data.
+ */
+struct WindEstimationData
+{
+    uint64_t timestamp = 0;
+    Boardcore::Units::Speed::MeterPerSecond velocityNorth;
+    Boardcore::Units::Speed::MeterPerSecond velocityEast;
+    bool calibration = false;  ///< True if the wind estimation is in
+                               ///< calibration mode, false otherwise
+
+    static std::string header()
+    {
+        return "timestamp,velocityNorth,velocityEast,calibration\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << velocityNorth << "," << velocityEast << ","
+           << calibration << "\n ";
+    }
+};
+
+struct GeoVelocity
+{
+    Boardcore::Units::Speed::MeterPerSecond vn{0};  // North velocity
+    Boardcore::Units::Speed::MeterPerSecond ve{0};  // East velocity
+
+    /**
+     * @brief Calculate the squared norm of the velocity vector.
+     */
+    float normSquared() const
+    {
+        return vn.value() * vn.value() + ve.value() * ve.value();
+    }
+
+    Eigen::Vector2f asVector() const { return {vn.value(), ve.value()}; }
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3c750418f2be5a0290e11b73ac3231ea6de2c7a4
--- /dev/null
+++ b/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp
@@ -0,0 +1,162 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Niccolò Betto, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "AutomaticWingAlgorithm.h"
+
+#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/Sensors/Sensors.h>
+#include <Parafoil/StateMachines/NASController/NASController.h>
+#include <Parafoil/StateMachines/WingController/WingController.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <math.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/Constants.h>
+
+using namespace Boardcore;
+using namespace Eigen;
+using namespace Parafoil::Config::Wing;
+using namespace Units::Angle;
+
+namespace Parafoil
+{
+AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki,
+                                               ServosList servo1,
+                                               ServosList servo2,
+                                               GuidanceAlgorithm& guidance)
+    : Super(servo1, servo2), guidance(guidance)
+{
+    // PIController needs the sample period in floating point seconds
+    auto samplePeriod = 1.0f / UPDATE_RATE.value();
+
+    controller = std::make_unique<PIController>(Kp, Ki, samplePeriod,
+                                                PI::SATURATION_MIN_LIMIT,
+                                                PI::SATURATION_MAX_LIMIT);
+}
+
+void AutomaticWingAlgorithm::step()
+{
+    if (getModule<Sensors>()->getUBXGPSLastSample().fix == 3)
+    {
+        // The PI calculated result
+        auto result = algorithmStep(getModule<NASController>()->getNasState());
+
+        // Actuate the result
+        // To see how to interpret the PI output
+        // https://www.geogebra.org/calculator/xrhwarpz
+        //
+        // Reference system
+        // N ^
+        //   |
+        //   |
+        //   ----> E
+        if (result > 0_deg)
+        {
+            // Activate the servo2 and reset servo1
+            getModule<Actuators>()->setServoAngle(servo1, 0_deg);
+            getModule<Actuators>()->setServoAngle(servo2, result);
+        }
+        else
+        {
+            // Activate the servo1 and reset servo2
+            getModule<Actuators>()->setServoAngle(servo1, -result);
+            getModule<Actuators>()->setServoAngle(servo2, 0_deg);
+        }
+
+        // Log the servo positions
+        {
+            miosix::Lock<FastMutex> l(mutex);
+
+            data.timestamp   = TimestampTimer::getTimestamp();
+            data.servo1Angle = getModule<Actuators>()->getServoAngle(servo1);
+            data.servo2Angle = getModule<Actuators>()->getServoAngle(servo2);
+            SDlogger->log(data);
+        }
+    }
+    else
+    {
+        // If we loose fix we set both servo at 0
+        getModule<Actuators>()->setServoAngle(servo1, 0_deg);
+        getModule<Actuators>()->setServoAngle(servo2, 0_deg);
+    }
+}
+
+Degree AutomaticWingAlgorithm::algorithmStep(const NASState& state)
+{
+    // For some algorithms the third component is needed!
+    Vector3f currentPosition(state.n, state.e, state.d);
+
+    Vector2f heading;  // used for logging purposes
+
+    auto targetAngle = guidance.calculateTargetAngle(currentPosition, heading);
+
+    Vector2f relativeVelocity(state.vn, state.ve);
+
+    // Compute the angle of the current velocity
+    // All angle are computed as angle from the north direction
+    auto velocityAngle =
+        Radian(atan2(relativeVelocity[1], relativeVelocity[0]));
+
+    // Compute the angle difference
+    auto error = angleDiff(targetAngle, velocityAngle);
+
+    // Call the PI with the just calculated error. The result is in RADIANS,
+    // if positive we activate one servo, if negative the other
+    // We also need to convert the result from radians back to degrees
+    auto result = Degree(Radian(controller->update(error.value())));
+
+    // Logs the outputs
+    {
+        miosix::Lock<FastMutex> l(mutex);
+        data.targetX       = heading[0];
+        data.targetY       = heading[1];
+        data.targetAngle   = targetAngle;
+        data.velocityAngle = velocityAngle;
+        data.error         = error;
+        data.pidOutput     = result;
+    }
+
+    return result;
+}
+
+Radian AutomaticWingAlgorithm::angleDiff(Radian a, Radian b)
+{
+    auto diff = (a - b).value();
+
+    // Angle difference
+    if (diff < -Constants::PI || Constants::PI < diff)
+    {
+        diff += Constants::PI;
+        bool positiveInput = diff > 0;
+
+        diff = diff - floor(diff / (2 * Constants::PI)) * (2 * Constants::PI);
+
+        // diff = fmod(diff, 2 * Constants::PI);
+        if (diff == 0 && positiveInput)
+            diff = 2 * Constants::PI;
+
+        diff -= Constants::PI;
+    }
+
+    return Radian{diff};
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/AutomaticWingAlgorithm.h b/src/Parafoil/Wing/AutomaticWingAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..8074a44670352a508aab439ec51392e74e2eb856
--- /dev/null
+++ b/src/Parafoil/Wing/AutomaticWingAlgorithm.h
@@ -0,0 +1,101 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <Parafoil/Wing/Guidance/GuidanceAlgorithm.h>
+#include <Parafoil/Wing/WingAlgorithm.h>
+#include <algorithms/NAS/NASState.h>
+#include <algorithms/PIController.h>
+#include <algorithms/ReferenceValues.h>
+#include <units/Angle.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include <Eigen/Core>
+
+namespace Parafoil
+{
+
+class Sensors;
+class NASController;
+class Actuators;
+
+class AutomaticWingAlgorithm : public Boardcore::InjectableWithDeps<
+                                   Boardcore::InjectableBase<WingAlgorithm>,
+                                   Sensors, NASController, Actuators>
+{
+public:
+    /**
+     * @brief Construct a new Automatic Wing Algorithm object
+     *
+     * @param Kp Proportional value for PI controller
+     * @param Ki Integral value for PI controller
+     * @param servo1 The first servo
+     * @param servo2 The second servo
+     * @param guidance The algorithm used to compute the target yaw and the
+     * heading
+     */
+    AutomaticWingAlgorithm(float Kp, float Ki, ServosList servo1,
+                           ServosList servo2, GuidanceAlgorithm& guidance);
+
+protected:
+    // Guidance algorithm that sets the yaw.
+    GuidanceAlgorithm& guidance;
+
+    // PI controller tuned on the Kp and Ki passed through constructor
+    std::unique_ptr<Boardcore::PIController> controller;
+
+    /**
+     * @brief Actual algorithm implementation, all parameters should be in NED
+     *
+     * @return The angle to set to the servo, positive is right and negative is
+     * left
+     */
+    Boardcore::Units::Angle::Degree algorithmStep(
+        const Boardcore::NASState& state);
+
+    /**
+     * @brief This method implements the automatic algorithm that will steer the
+     * parafoil according to its position and velocity. IN THIS METHOD THE
+     * GUIDANCE IS TRANSLATED
+     */
+    void step() override;
+
+    /**
+     * @brief Computes the difference between two angles
+     *
+     * @param a The first angle
+     * @param b The second angle
+     *
+     * @returns angle(a) - angle(b)
+     */
+    Boardcore::Units::Angle::Radian angleDiff(
+        Boardcore::Units::Angle::Radian a, Boardcore::Units::Angle::Radian b);
+
+    // Logging structure
+    WingAlgorithmData data;
+
+    /**
+     * @brief Mutex
+     */
+    miosix::FastMutex mutex;
+};
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/FileWingAlgorithm.cpp b/src/Parafoil/Wing/FileWingAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b30c7bb20da1d2aedccef538f437885ff9483cd7
--- /dev/null
+++ b/src/Parafoil/Wing/FileWingAlgorithm.cpp
@@ -0,0 +1,67 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "FileWingAlgorithm.h"
+
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+
+namespace Parafoil
+{
+std::istream& operator>>(std::istream& input, WingAlgorithmData& data)
+{
+    input >> data.timestamp;
+    input.ignore(1, ',');
+    input >> data.servo1Angle;
+    input.ignore(1, ',');
+    input >> data.servo2Angle;
+    return input;
+}
+
+FileWingAlgorithm::FileWingAlgorithm(ServosList servo1, ServosList servo2,
+                                     const char* filename)
+    : WingAlgorithm(servo1, servo2), parser(filename)
+{
+    setServo(servo1, servo2);
+}
+
+bool FileWingAlgorithm::init()
+{
+    // Returns a std::vector which contains
+    // all the csv parsed with the data structure in mind
+    steps = parser.collect();
+
+    // Return if the size collected is greater than 0
+    fileValid = steps.size() > 0;
+
+    // Communicate it via serial
+    if (fileValid)
+        LOG_INFO(logger, "File valid");
+
+    // Close the file
+    parser.close();
+
+    return fileValid;
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/FileWingAlgorithm.h b/src/Parafoil/Wing/FileWingAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b2d61f792906695735ab30ba778ce8db22f28cc
--- /dev/null
+++ b/src/Parafoil/Wing/FileWingAlgorithm.h
@@ -0,0 +1,91 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+#include <Parafoil/Wing/WingAlgorithm.h>
+#include <utils/CSVReader/CSVReader.h>
+
+/**
+ * @brief This class abstracts the concept of wing timestamp based
+ * algorithm. These algorithms are stored in files (formatted in csv).
+ * We use a CSV parser to properly parse the procedure and every step
+ * we check if it is time to advance and in case actuate the step with
+ * the two servos.
+ *
+ * Brief use case:
+ *
+ * Actuators::getInstance().enableServo(PARAFOIL_LEFT_SERVO);
+ * Actuators::getInstance().enableServo(PARAFOIL_RIGHT_SERVO);
+ *
+ * WingAlgorithm algorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, "Optional
+ * File") algorithm.init();
+ *
+ * //In case of a non valid file/empty string you can add the steps
+ * algorithm.addStep(WingAlgorithmData{timestamp, angle1, angle2});
+ *
+ * algorithm.begin();
+ *
+ * algorithm.update()...
+ *
+ * //End of algorithm
+ *
+ * algorithm.begin();
+ *
+ * algorithm.update()...
+ */
+
+namespace Parafoil
+{
+
+class FileWingAlgorithm : public WingAlgorithm
+{
+public:
+    /**
+     * @brief Construct a new Wing Algorithm object
+     *
+     * @param servo1 The first servo
+     * @param servo2 The second servo
+     * @param filename The csv file where all the operations are stored
+     */
+    FileWingAlgorithm(ServosList servo1, ServosList servo2,
+                      const char* filename);
+
+    /**
+     * @brief This method parses the file and stores it into a std::vector
+     *
+     * @return true If the initialization finished correctly
+     * @return false If something went wrong
+     */
+    bool init() override;
+
+protected:
+    /**
+     * @brief CSV format file parser
+     */
+    Boardcore::CSVParser<WingAlgorithmData> parser;
+
+    /**
+     * @brief Indicates whether the current file of the algorithm is readable
+     */
+    bool fileValid = false;
+};
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9686dc8180947288a89d9f7d8c232fb5e1062c98
--- /dev/null
+++ b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
@@ -0,0 +1,44 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Radu Raul, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
+#include <math.h>
+
+using namespace Boardcore::Units::Angle;
+
+namespace Parafoil
+{
+
+Radian ClosedLoopGuidanceAlgorithm::calculateTargetAngle(
+    const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
+{
+    heading[0] = targetNED[0] - currentPositionNED[0];
+    heading[1] = targetNED[1] - currentPositionNED[1];
+    return Radian{atan2(heading[1], heading[0])};
+}
+
+void ClosedLoopGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED)
+{
+    this->targetNED = targetNED;
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..50a053edfb3b6866e312670eaf42a2aa9e3acd86
--- /dev/null
+++ b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h
@@ -0,0 +1,57 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Parafoil/Wing/Guidance/GuidanceAlgorithm.h>
+
+#include <Eigen/Core>
+
+namespace Parafoil
+{
+
+/**
+ * This class is the implementation of the Simple Closed Loop guidance.
+ * It calculates the yaw between the current position and the target position by
+ * calculating the difference between the two vectors and the angle formed by
+ * the diff vector
+ */
+class ClosedLoopGuidanceAlgorithm : public GuidanceAlgorithm
+{
+    /**
+     * @brief This method calculates the yaw angle of the parafoil knowing
+     * the current position and the target position.
+     *
+     * @param[in] position the current NED position of the parafoil in [m]
+     * @param[out] heading Saves the heading vector for logging purposes
+     *
+     * @returns the yaw angle of the parafoil in [rad]
+     */
+    Boardcore::Units::Angle::Radian calculateTargetAngle(
+        const Eigen::Vector3f& currentPositionNED,
+        Eigen::Vector2f& heading) override;
+
+public:
+    void setPoints(Eigen::Vector2f targetNED);
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85011723cbdeabaffa89a8c05c7f243c28ee98a5
--- /dev/null
+++ b/src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
@@ -0,0 +1,198 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/Wing/WingTargetPositionData.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <logger/Logger.h>
+
+#include <Eigen/Core>
+
+#include "EarlyManeuversGuidanceAlgorithm.h"
+
+using namespace Boardcore;
+using namespace Parafoil::Config::Wing;
+
+namespace Parafoil
+{
+
+EarlyManeuversGuidanceAlgorithm::EarlyManeuversGuidanceAlgorithm()
+    : activeTarget(Target::EMC), targetAltitudeConfidence(0),
+      m2AltitudeConfidence(0), m1AltitudeConfidence(0),
+      emcAltitudeConfidence(0){};
+
+EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){};
+
+Radian EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
+    const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
+{
+    switch (activeTarget)
+    {
+        case Target::EMC:
+            heading[0] = EMC[0] - currentPositionNED[0];
+            heading[1] = EMC[1] - currentPositionNED[1];
+            break;
+        case Target::M1:
+            heading[0] = M1[0] - currentPositionNED[0];
+            heading[1] = M1[1] - currentPositionNED[1];
+            break;
+        case Target::M2:
+            heading[0] = M2[0] - currentPositionNED[0];
+            heading[1] = M2[1] - currentPositionNED[1];
+            break;
+        case Target::FINAL:
+            heading[0] = targetNED[0] - currentPositionNED[0];
+            heading[1] = targetNED[1] - currentPositionNED[1];
+            break;
+    }
+
+    return Radian{static_cast<float>(atan2(heading[1], heading[0]))};
+}
+
+void EarlyManeuversGuidanceAlgorithm::updateActiveTarget(Meter altitude)
+{
+    if (altitude <=
+        Guidance::TARGET_ALTITUDE_THRESHOLD)  // Altitude is low, head directly
+                                              // to target
+    {
+        targetAltitudeConfidence++;
+    }
+    else if (altitude <= Guidance::M2_ALTITUDE_THRESHOLD)  // Altitude is almost
+                                                           // okay, go to M2
+    {
+        m2AltitudeConfidence++;
+    }
+    else if (altitude <=
+             Guidance::M1_ALTITUDE_THRESHOLD)  // Altitude is high, go to M1
+    {
+        m1AltitudeConfidence++;
+    }
+    else
+    {
+        emcAltitudeConfidence++;  // Altitude is too high, head to the emc
+    }
+
+    auto currentTarget = activeTarget.load();
+    auto newTarget     = currentTarget;
+
+    switch (currentTarget)
+    {
+        case Target::EMC:
+            if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
+            {
+                newTarget             = Target::FINAL;
+                emcAltitudeConfidence = 0;
+            }
+            else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
+            {
+                newTarget             = Target::M2;
+                emcAltitudeConfidence = 0;
+            }
+            else if (m1AltitudeConfidence >= Guidance::CONFIDENCE)
+            {
+                newTarget             = Target::M1;
+                emcAltitudeConfidence = 0;
+            }
+            break;
+
+        case Target::M1:
+            if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
+            {
+                newTarget            = Target::FINAL;
+                m1AltitudeConfidence = 0;
+            }
+            else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
+            {
+                newTarget            = Target::M2;
+                m1AltitudeConfidence = 0;
+            }
+            break;
+
+        case Target::M2:
+            if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
+            {
+                newTarget            = Target::FINAL;
+                m2AltitudeConfidence = 0;
+            }
+            break;
+
+        case Target::FINAL:
+            break;
+    }
+
+    if (newTarget != currentTarget)
+    {
+        activeTarget = newTarget;
+
+        // Log the active target change
+        auto data = EarlyManeuversActiveTargetData{
+            .timestamp = TimestampTimer::getTimestamp(),
+            .target    = static_cast<uint32_t>(newTarget),
+            .altitude  = altitude,
+        };
+        Logger::getInstance().log(data);
+    }
+}
+
+void EarlyManeuversGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED,
+                                                Eigen::Vector2f EMC,
+                                                Eigen::Vector2f M1,
+                                                Eigen::Vector2f M2)
+{
+    this->targetNED = targetNED;
+    this->EMC       = EMC;
+    this->M1        = M1;
+    this->M2        = M2;
+}
+
+EarlyManeuversPoints EarlyManeuversGuidanceAlgorithm::getPoints()
+{
+    return EarlyManeuversPoints{
+        .targetN = targetNED[0],
+        .targetE = targetNED[1],
+        .emcN    = EMC[0],
+        .emcE    = EMC[1],
+        .m1N     = M1[0],
+        .m1E     = M1[1],
+        .m2N     = M2[0],
+        .m2E     = M2[1],
+    };
+}
+
+Eigen::Vector2f EarlyManeuversGuidanceAlgorithm::getActiveTarget()
+{
+    switch (activeTarget)
+    {
+        case Target::EMC:
+            return EMC;
+        case Target::M1:
+            return M1;
+        case Target::M2:
+            return M2;
+        case Target::FINAL:
+            return targetNED;
+        default:
+            return {0, 0};
+    }
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..b1f341203571974327b5116de32c97cdc67223a7
--- /dev/null
+++ b/src/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
@@ -0,0 +1,126 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Radu Raul, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Parafoil/Wing/Guidance/GuidanceAlgorithm.h>
+#include <units/Length.h>
+
+#include <Eigen/Core>
+
+namespace Parafoil
+{
+
+struct EarlyManeuversPoints
+{
+    float targetN;
+    float targetE;
+    float emcN;
+    float emcE;
+    float m1N;
+    float m1E;
+    float m2N;
+    float m2E;
+};
+
+/**
+ * This class is the implementation of the Simple Closed Loop guidance.
+ * It calculates the yaw between the current position and the target position by
+ * calculating the difference between the two vectors and the angle formed by
+ * the diff vector
+ *
+ * requires: WingControllers
+ */
+class EarlyManeuversGuidanceAlgorithm : public GuidanceAlgorithm
+{
+public:
+    /**
+     * @brief Enumerates all the possible targets of the EM algorithm
+     */
+    enum class Target : uint32_t
+    {
+        EMC = 0,
+        M1,
+        M2,
+        FINAL
+    };
+
+    EarlyManeuversGuidanceAlgorithm();
+
+    virtual ~EarlyManeuversGuidanceAlgorithm();
+
+    /**
+     * @brief This method calculates the yaw angle of the parafoil knowing
+     * the current position and the target position.
+     *
+     * @param[in] currentPositionNED the current NED position of the parafoil in
+     * meters
+     * @param[out] heading Saves the heading vector for logging purposes
+     *
+     * @returns the yaw angle of the parafoil in rad
+     */
+    Boardcore::Units::Angle::Radian calculateTargetAngle(
+        const Eigen::Vector3f& currentPositionNED,
+        Eigen::Vector2f& heading) override;
+
+    /**
+     * @brief Set Early Maneuvers points
+     *
+     * @param[in] EMC NED
+     * @param[in] M1 NED
+     * @param[in] M2 NED
+     */
+    void setPoints(Eigen::Vector2f targetNED, Eigen::Vector2f EMC,
+                   Eigen::Vector2f M1, Eigen::Vector2f M2);
+
+    /**
+     * @brief Get Early Maneuvers points.
+     */
+    EarlyManeuversPoints getPoints();
+
+    /**
+     * @brief Get the active target.
+     * @return The NED coordinates of the active target.
+     */
+    Eigen::Vector2f getActiveTarget();
+
+    /**
+     * @brief Updates the active target based on the current altitude.
+     */
+    void updateActiveTarget(Boardcore::Units::Length::Meter altitude);
+
+private:
+    // Point we are currently poinying to
+    std::atomic<Target> activeTarget;
+
+    // Eigen::Vector2f targetNED;  // NED, defined in the base class
+    Eigen::Vector2f EMC;  // NED
+    Eigen::Vector2f M1;   // NED
+    Eigen::Vector2f M2;   // NED
+
+    uint32_t targetAltitudeConfidence;
+    uint32_t m2AltitudeConfidence;
+    uint32_t m1AltitudeConfidence;
+    uint32_t emcAltitudeConfidence;
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/Guidance/GuidanceAlgorithm.h b/src/Parafoil/Wing/Guidance/GuidanceAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f296b33c8b94a8b7aa31bd59cc46b2afb1e458a
--- /dev/null
+++ b/src/Parafoil/Wing/Guidance/GuidanceAlgorithm.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Radu Raul, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <units/Angle.h>
+
+#include <Eigen/Core>
+
+namespace Parafoil
+{
+
+/**
+ * This class acts as an interface for a generic guidance algorithm that is used
+ * by the Automatic Wing Algorithm.
+ */
+class GuidanceAlgorithm
+{
+public:
+    /**
+     * @brief This method must calculate the yaw angle of the parafoil knowing
+     * the current position and the target position without changing the vectors
+     * passed as arguments.
+     *
+     * @note the args are const references to reduce access time by avoiding
+     * copying objects that will be read-only.
+     *
+     * @param position the current NED position of the parafoil in [m]
+     * @param heading The current heading, it is used for logging purposes
+     *
+     * @returns the yaw angle of the parafoil in [rad]
+     */
+    virtual Boardcore::Units::Angle::Radian calculateTargetAngle(
+        const Eigen::Vector3f& currentPositionNED,
+        Eigen::Vector2f& heading) = 0;
+
+    Eigen::Vector2f getTargetNED() { return targetNED; }
+
+protected:
+    Eigen::Vector2f targetNED{0, 0};  // NED
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/WingAlgorithm.cpp b/src/Parafoil/Wing/WingAlgorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e73a21e7d68e63572ca0702fcfc073e3c2ab86a
--- /dev/null
+++ b/src/Parafoil/Wing/WingAlgorithm.cpp
@@ -0,0 +1,121 @@
+/* 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
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Parafoil/Configs/WingConfig.h>
+#include <Parafoil/Wing/WingAlgorithm.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Parafoil::Config::Wing;
+using namespace Common;
+
+namespace Parafoil
+{
+WingAlgorithm::WingAlgorithm(ServosList servo1, ServosList servo2)
+{
+    this->servo1 = servo1;
+    this->servo2 = servo2;
+    stepIndex    = 0;
+    // Create the vector for algorithm data
+    steps = std::vector<WingAlgorithmData>();
+}
+
+bool WingAlgorithm::init()
+{
+    return true;  // In this case the init is always true
+}
+
+void WingAlgorithm::setServo(ServosList servo1, ServosList servo2)
+{
+    this->servo1 = servo1;
+    this->servo2 = servo2;
+}
+
+void WingAlgorithm::addStep(WingAlgorithmData step) { steps.push_back(step); }
+
+void WingAlgorithm::begin()
+{
+    running     = true;
+    shouldReset = true;
+
+    // Set the reference timestamp
+    timeStart = TimestampTimer::getTimestamp();
+}
+
+void WingAlgorithm::end()
+{
+    running = false;
+
+    // Set the reference timestamp to 0
+    timeStart = 0;
+}
+
+void WingAlgorithm::step()
+{
+    uint64_t currentTimestamp = TimestampTimer::getTimestamp();
+
+    if (shouldReset)
+    {
+        // If the algorithm has been stopped
+        // i want to start from the beginning
+        stepIndex   = 0;
+        shouldReset = false;
+    }
+
+    if (stepIndex >= steps.size())
+    {
+        LOG_INFO(logger, "Algorithm end {:d} >= {:d}", stepIndex, steps.size());
+        // End the procedure so it won't be executed
+        // Set the index to 0 in case of another future execution
+        stepIndex = 0;
+        // Terminate here
+        EventBroker::getInstance().post(WING_ALGORITHM_ENDED, TOPIC_WING);
+        return;
+    }
+
+    if (currentTimestamp - timeStart >= steps[stepIndex].timestamp)
+    {
+        // I need to execute the current step
+        getModule<Actuators>()->setServoAngle(servo1,
+                                              steps[stepIndex].servo1Angle);
+        getModule<Actuators>()->setServoAngle(servo2,
+                                              steps[stepIndex].servo2Angle);
+
+        // Log the data setting the timestamp to the absolute one
+        WingAlgorithmData data;
+        data.timestamp   = TimestampTimer::getTimestamp();
+        data.servo1Angle = steps[stepIndex].servo1Angle;
+        data.servo2Angle = steps[stepIndex].servo2Angle;
+
+        // After copy i can log the actual step
+        SDlogger->log(data);
+
+        // finally increment the stepIndex
+        stepIndex++;
+
+        LOG_INFO(logger, "Step");
+    }
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/WingAlgorithm.h b/src/Parafoil/Wing/WingAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff84671bb4204b2843df7c1a2bfd607d8f25c333
--- /dev/null
+++ b/src/Parafoil/Wing/WingAlgorithm.h
@@ -0,0 +1,109 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <Parafoil/Actuators/Actuators.h>
+#include <Parafoil/Wing/WingAlgorithmData.h>
+#include <algorithms/Algorithm.h>
+#include <diagnostic/PrintLogger.h>
+#include <logger/Logger.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Parafoil
+{
+class WingAlgorithm : public Boardcore::Algorithm,
+                      public Boardcore::InjectableWithDeps<Actuators>
+{
+public:
+    /**
+     * @brief Construct a new Wing Algorithm object
+     *
+     * @param servo1 The first servo
+     * @param servo2 The second servo
+     */
+    WingAlgorithm(ServosList servo1, ServosList servo2);
+
+    /**
+     * @brief Method to initialize the algorithm
+     *
+     * @return true If the init process goes well
+     * @return false If the init process doesn't go well
+     */
+    bool init() override;
+
+    /**
+     * @brief Set the Servos object
+     *
+     * @param servo1 The first algorithm servo
+     * @param servo2 The second algorithm servo
+     */
+    void setServo(ServosList servo1, ServosList servo2);
+
+    /**
+     * @brief Add a step to the algorithm sequence
+     *
+     * @param step The step to add
+     */
+    void addStep(WingAlgorithmData step);
+
+    /**
+     * @brief This sets the reference timestamp for the algorithm
+     */
+    void begin();
+
+    /**
+     * @brief This method disables the algorithm
+     */
+    void end();
+    /**
+     * @brief This method implements the algorithm step based on the current
+     * timestamp
+     */
+    void step() override;
+
+protected:
+    // The actuators
+    ServosList servo1, servo2;
+
+    // Reference timestamp for relative algorithm timestamps
+    uint64_t timeStart;
+
+    // Procedure array to memorize all the steps needed to perform the algorithm
+    std::vector<WingAlgorithmData> steps;
+
+    // PrintLogger
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("WingAlgorithm");
+
+    // SD logger
+    Boardcore::Logger* SDlogger = &Boardcore::Logger::getInstance();
+
+    // This boolean is used to understand when to reset
+    // the index where the algorithm has stopped.
+    // In case of end call, we want to be able to perform
+    // another time this algorithm starting from 0
+    std::atomic<bool> shouldReset;
+
+    // Variable to remember what step that has to be done
+    unsigned int stepIndex;
+};
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/WingAlgorithmData.h b/src/Parafoil/Wing/WingAlgorithmData.h
new file mode 100644
index 0000000000000000000000000000000000000000..17146089442ebe3382cd36c797ca0cce0337c01e
--- /dev/null
+++ b/src/Parafoil/Wing/WingAlgorithmData.h
@@ -0,0 +1,64 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+#include <sensors/SensorData.h>
+#include <units/Angle.h>
+
+namespace Parafoil
+{
+
+/**
+ * This class represents the algorithm data structure that needs to be logged
+ * into the onboard SD card. It has the timestamp(absolute) and the servo
+ * position set by the selected algorithm
+ */
+struct WingAlgorithmData
+{
+    uint64_t timestamp = 0;  // First timestamp is 0 (in microseconds)
+    Boardcore::Units::Angle::Degree servo1Angle{0};  // Angle of the first servo
+    Boardcore::Units::Angle::Degree servo2Angle{
+        0};  // Angle of the second servo
+    Boardcore::Units::Angle::Radian targetAngle{
+        0};  // Angle tracked by the algorithm
+    Boardcore::Units::Angle::Radian velocityAngle{
+        0};             // Angle of the velocity vector
+    float targetX = 0;  // NED (only automatic algorithm)
+    float targetY = 0;  // NED (only automatic algorithm)
+    Boardcore::Units::Angle::Radian error{
+        0};  // Error between target and velocity angle
+    Boardcore::Units::Angle::Degree pidOutput{
+        0};  // Output of the PID controller
+
+    static std::string header()
+    {
+        return "WingAlgorithmTimestamp,servo1Angle,servo2Angle,targetAngle,"
+               "velocityAngle,targetX,targetY,error,pidOutput\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << servo1Angle << "," << servo2Angle << ","
+           << targetAngle << "," << velocityAngle << "," << targetX << ","
+           << targetY << "," << error << "," << pidOutput << "\n";
+    }
+};
+}  // namespace Parafoil
diff --git a/src/Parafoil/Wing/WingTargetPositionData.h b/src/Parafoil/Wing/WingTargetPositionData.h
new file mode 100644
index 0000000000000000000000000000000000000000..baac961b9985b05b0a083cc1ecfe29efa847fcae
--- /dev/null
+++ b/src/Parafoil/Wing/WingTargetPositionData.h
@@ -0,0 +1,75 @@
+/* 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
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <units/Length.h>
+
+#include <ostream>
+#include <string>
+
+namespace Parafoil
+{
+
+struct WingTargetPositionData
+{
+    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 "targetLat, "
+               "targetLon,targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << targetLat << "," << targetLon << "," << targetN << "," << targetE
+           << "," << emcN << "," << emcE << "," << m1N << "," << m1E << ","
+           << m2N << "," << m2E << "\n";
+    }
+};
+
+struct EarlyManeuversActiveTargetData
+{
+    uint64_t timestamp = 0;
+    uint32_t target    = 0;  ///< Active target enumeration
+    Boardcore::Units::Length::Meter altitude{
+        0};  ///< Altitude when the target was changed
+
+    static std::string header() { return "timestamp,target,altitude\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << target << "," << altitude << "\n";
+    }
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index 971f6cf58802a55afe3b2f8a5a50f79d35b9130a..857e93de8c958512a84481581837be503a1a1574 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -28,6 +28,8 @@
 #include <Parafoil/Sensors/Sensors.h>
 #include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
 #include <Parafoil/StateMachines/NASController/NASController.h>
+#include <Parafoil/StateMachines/WingController/WingController.h>
+#include <Parafoil/WindEstimation/WindEstimation.h>
 #include <common/Events.h>
 #include <common/Topics.h>
 #include <diagnostic/PrintLogger.h>
@@ -116,7 +118,10 @@ int main()
     // Flight algorithms
     auto altitudeTrigger = new AltitudeTrigger();
     initResult &= depman.insert(altitudeTrigger);
-    // TODO: Wing Algorithm
+    auto wingController = new WingController();
+    initResult &= depman.insert(wingController);
+    auto windEstimation = new WindEstimation();
+    initResult &= depman.insert(windEstimation);
 
     // Actuators
     auto actuators = new Actuators();
@@ -130,11 +135,13 @@ int main()
     }
 
     START_MODULE(flightModeManager);
+
     START_MODULE(pinHandler);
     // START_MODULE(radio) { miosix::led2On(); }
     START_MODULE(nasController);
     START_MODULE(altitudeTrigger);
-    // START_MODULE(wingController);
+    START_MODULE(windEstimation);
+    START_MODULE(wingController);
     START_MODULE(actuators);
 
     START_MODULE(scheduler);
diff --git a/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
index a30a15872a6c1548c76fbe2dfa55d8f191d59b9e..1eb821bf5c95f0aba7cf37c417d52f8452a92687 100644
--- a/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+++ b/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
@@ -37,9 +37,9 @@ namespace Payload
 EarlyManeuversGuidanceAlgorithm::EarlyManeuversGuidanceAlgorithm()
     : activeTarget(Target::EMC), targetAltitudeConfidence(0),
       m2AltitudeConfidence(0), m1AltitudeConfidence(0),
-      emcAltitudeConfidence(0) {};
+      emcAltitudeConfidence(0){};
 
-EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm() {};
+EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){};
 
 float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
     const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
@@ -64,7 +64,7 @@ float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
             break;
     }
 
-    return atan2(heading[1], heading[0]);
+    return static_cast<float>(atan2(heading[1], heading[0]));
 }
 
 void EarlyManeuversGuidanceAlgorithm::updateActiveTarget(float altitude)
diff --git a/src/common/Events.h b/src/common/Events.h
index fc524997e62255a4e295a0af299ca491f8c49618..54b2b1cda1fa558489548f78430d6a08e50a0f0b 100644
--- a/src/common/Events.h
+++ b/src/common/Events.h
@@ -67,6 +67,9 @@ enum Events : uint8_t
     DPL_NC_RESET,
     DPL_FLARE_START,
     DPL_FLARE_STOP,
+    DPL_SERVO_ACTUATION_DETECTED,
+    DPL_WIGGLE,
+    DPL_WES_CAL_DONE,
     DPL_DONE,
     CAN_FORCE_INIT,
     CAN_ARM,