diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index f54d49448d2254328f4d140e9db7443a1b0fa6d0..00048a404a8c0dbd879a95d16b2ae47b78bd5d68 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -36,6 +36,7 @@ set(PARAFOIL_COMPUTER
     src/boards/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
     src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
     src/boards/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+    src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
     src/boards/Parafoil/Wing/FileWingAlgorithm.cpp
     src/boards/Parafoil/Wing/WingAlgorithm.cpp
     src/boards/Parafoil/WindEstimationScheme/WindEstimation.cpp
diff --git a/skyward-boardcore b/skyward-boardcore
index f57081da20964030a07990dd985afe5c845241f4..093d5b3108b4dd27612222eb99ff2790c96886be 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit f57081da20964030a07990dd985afe5c845241f4
+Subproject commit 093d5b3108b4dd27612222eb99ff2790c96886be
diff --git a/src/boards/Parafoil/Actuators/Actuators.cpp b/src/boards/Parafoil/Actuators/Actuators.cpp
index 34d1dd26c5ff8c87ac93a87415c335e626b865d0..4f317e60d641387835d48dcbb7630b572950ea57 100644
--- a/src/boards/Parafoil/Actuators/Actuators.cpp
+++ b/src/boards/Parafoil/Actuators/Actuators.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -57,7 +57,6 @@ bool Actuators::start()
 
 bool Actuators::setServo(ServosList servoId, float percentage)
 {
-    percentage += offset;
     if (percentage > WingConfig::MAX_SERVO_APERTURE)
     {
         percentage = WingConfig::MAX_SERVO_APERTURE;
@@ -207,8 +206,16 @@ float Actuators::getServoAngle(ServosList servoId)
     return 0;
 }
 
-void Actuators::setServosOffset(float offset) { this->offset = offset; }
+void Actuators::startTwirl()
+{
+    setServo(PARAFOIL_LEFT_SERVO, SERVO_TWIRL_RADIUS);
+    setServo(PARAFOIL_RIGHT_SERVO, 0);
+}
 
-float Actuators::getServosOffset() { return offset; }
+void Actuators::stopTwirl()
+{
+    setServo(PARAFOIL_LEFT_SERVO, 0);
+    setServo(PARAFOIL_RIGHT_SERVO, 0);
+}
 
 }  // namespace Parafoil
diff --git a/src/boards/Parafoil/Actuators/Actuators.h b/src/boards/Parafoil/Actuators/Actuators.h
index 00291ac77f8783f52ec29dc619ed0f77b7f74a26..dca587a95430a4726236d37dc31b993f16733366 100644
--- a/src/boards/Parafoil/Actuators/Actuators.h
+++ b/src/boards/Parafoil/Actuators/Actuators.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -102,18 +102,14 @@ struct Actuators : public Boardcore::Module
     float getServoAngle(ServosList servoId);
 
     /**
-     * @brief Sets the offset for all servos.
-     *
-     * @param offset New offset [0-1].
+     * @brief Starts twirl (one servo is set to 0 and the other one is not).
      */
-    void setServosOffset(float offset);
+    void startTwirl();
 
     /**
-     * @brief Returns the current offset of the servos.
-     *
-     * @return Current offset [0-1].
+     * @brief Stops twirling (both servos are set to 0).
      */
-    float getServosOffset();
+    void stopTwirl();
 
 private:
     Boardcore::Servo* leftServo  = nullptr;
@@ -123,8 +119,6 @@ private:
     miosix::FastMutex leftServoMutex;
     miosix::FastMutex rightServoMutex;
     miosix::FastMutex rocketSignalingStateMutex;
-
-    float offset = 0;
 };
 
 }  // namespace Parafoil
diff --git a/src/boards/Parafoil/Configs/ActuatorsConfigs.h b/src/boards/Parafoil/Configs/ActuatorsConfigs.h
index 61eb7ea9bf1874cf0da59ea7c0c96a88bd015198..b7a73497871927f232cf9639847b7292c801207e 100644
--- a/src/boards/Parafoil/Configs/ActuatorsConfigs.h
+++ b/src/boards/Parafoil/Configs/ActuatorsConfigs.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio, Federico Lolli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -36,7 +36,7 @@ static TIM_TypeDef* const SERVO_1_TIMER = TIM3;
 constexpr Boardcore::TimerUtils::Channel SERVO_1_PWM_CH =
     Boardcore::TimerUtils::Channel::CHANNEL_1;
 
-constexpr float LEFT_SERVO_ROTATION  = 120;  // [deg]
+constexpr float LEFT_SERVO_ROTATION  = 180;  // [deg]
 constexpr float LEFT_SERVO_MIN_PULSE = 900;  // [us]
 constexpr float LEFT_SERVO_MAX_PULSE =
     LEFT_SERVO_MIN_PULSE + 10 * LEFT_SERVO_ROTATION;  // [us]
@@ -46,11 +46,14 @@ static TIM_TypeDef* const SERVO_2_TIMER = TIM3;
 constexpr Boardcore::TimerUtils::Channel SERVO_2_PWM_CH =
     Boardcore::TimerUtils::Channel::CHANNEL_2;
 
-constexpr float RIGHT_SERVO_ROTATION  = 120;   // [deg]
+constexpr float RIGHT_SERVO_ROTATION  = 180;   // [deg]
 constexpr float RIGHT_SERVO_MIN_PULSE = 2100;  // [us]
 constexpr float RIGHT_SERVO_MAX_PULSE =
     RIGHT_SERVO_MIN_PULSE - 10 * RIGHT_SERVO_ROTATION;  // [us]
 
+// Parafoil twirl
+constexpr float SERVO_TWIRL_RADIUS = 0.75;  // [%]
+
 // Buzzer configs
 static TIM_TypeDef* const BUZZER_TIMER = TIM1;
 constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL =
diff --git a/src/boards/Parafoil/Configs/WingConfig.h b/src/boards/Parafoil/Configs/WingConfig.h
index 45b2014be52551919db529c476b251b6e212d5bf..9c5fc145cbe7f0598ae57ee91ba0cd09a75c8374 100644
--- a/src/boards/Parafoil/Configs/WingConfig.h
+++ b/src/boards/Parafoil/Configs/WingConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -31,6 +31,16 @@ namespace WingConfig
 {
 // Algorithm configuration
 
+#if defined(CLOSED_LOOP)
+constexpr int SELECTED_ALGORITHM = 0;
+#elif EARLY_MANEUVER
+constexpr int SELECTED_ALGORITHM = 1;
+#elif SEQUENCE
+constexpr int SELECTED_ALGORITHM = 2;
+#else
+constexpr int SELECTED_ALGORITHM = 0;
+#endif
+
 #if defined(JESOLO)
 constexpr float DEFAULT_TARGET_LAT = 45.565835;
 constexpr float DEFAULT_TARGET_LON = 12.577307;
@@ -39,8 +49,7 @@ constexpr float DEFAULT_TARGET_LAT = 45.501148;
 constexpr float DEFAULT_TARGET_LON = 9.156301;
 #endif
 
-constexpr float OFFSET             = 0.25;
-constexpr float MAX_SERVO_APERTURE = 0.5 + OFFSET;
+constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000;  // [ms]
 
 constexpr int WING_UPDATE_PERIOD = 1000;  // [ms]
 
@@ -53,15 +62,14 @@ constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD     = 150;  //[m]
 constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50;   //[m]
 
 // TODO check this parameter preflight
-constexpr float KP = 0.4;   //[m]
-constexpr float KI = 0.08;  //[m]
+constexpr float KP = 0.0075;  //[m]
+constexpr float KI = 0.0007;  //[m]
 
 constexpr float ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE = 300;  // [meters]
 constexpr int ALTITUDE_TRIGGER_CONFIDENCE = 10;   // [number of sample]
 constexpr int ALTITUDE_TRIGGER_PERIOD     = 100;  //[ms]
 
-// TODO add algorithm selection
-constexpr int SELECTED_ALGORITHM = 0;
+constexpr float MAX_SERVO_APERTURE = 0.5;  //[%]
 
 struct WingConfigStruct
 {
diff --git a/src/boards/Parafoil/Sensors/Sensors.cpp b/src/boards/Parafoil/Sensors/Sensors.cpp
index ed9621a30772def2e5c5aeccdfc69100493a12ce..06383e24621d911e273cb7207b1ab5c0c75f28f4 100644
--- a/src/boards/Parafoil/Sensors/Sensors.cpp
+++ b/src/boards/Parafoil/Sensors/Sensors.cpp
@@ -418,9 +418,9 @@ void Sensors::batteryVoltageInit()
     LOG_INFO(logger, "Battery voltage sensor setup done!");
 }
 
-std::array<SensorInfo, 8> Sensors::getSensorInfo()
+std::array<SensorInfo, NUMBER_OF_SENSORS> Sensors::getSensorInfo()
 {
-    std::array<SensorInfo, 8> sensorState;
+    std::array<SensorInfo, NUMBER_OF_SENSORS> sensorState;
     for (size_t i = 0; i < sensorsInit.size(); i++)
     {
         sensorState[i] = sensorsInit[i]();
diff --git a/src/boards/Parafoil/Sensors/Sensors.h b/src/boards/Parafoil/Sensors/Sensors.h
index 2455fc52b9be707ac04bdd6d2fc99784b5892c76..fbb7996d8fa2859647a75850439259a645b21a1e 100644
--- a/src/boards/Parafoil/Sensors/Sensors.h
+++ b/src/boards/Parafoil/Sensors/Sensors.h
@@ -81,7 +81,8 @@ public:
     Boardcore::InternalADCData getInternalADCLastSample();
     Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
 
-    std::array<Boardcore::SensorInfo, 8> getSensorInfo();
+    std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
+    getSensorInfo();
 
     Boardcore::BMX160* bmx160 = nullptr;
 
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
index 1afbd283fff2494b386b11b329db189a3f81bf77..31788a9a03dc76e8effd45ffef01a525f6bf4235 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
index a89a89f37f07964786a30d480af07f1255e7dcfd..4c411a110498fc7e3b525834d8b17f61200ae85e 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
index 7d8dcaa65f0e7d24d52dc9a011e1177d0f6cc668..0a86b129ff209c0dc9afc26e634e9ba245a5bee9 100644
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -135,38 +135,38 @@ State WingController::state_flying(const Event& event)
 State WingController::state_calibration(const Boardcore::Event& event)
 {
     ModuleManager& modules = ModuleManager::getInstance();
+    static uint16_t calibrationTimeoutEventId;
+
     switch (event)
     {
         case EV_ENTRY:  // starts twirling and calibration wes
         {
             logStatus(WingControllerState::CALIBRATION);
-            // TODO
-            // modules.get<Actuators>()->setServo(ServosList::PARAFOIL_LEFT_SERVO,
-            //                                    1);
-            // modules.get<Actuators>()->setServo(ServosList::PARAFOIL_RIGHT_SERVO,
-            //                                    0);
-            // EventBroker::getInstance().postDelayed(DPL_WES_CAL_DONE,
-            // TOPIC_DPL,
-            //                                        WES_CALIBRATION_TIMEOUT);
+
+            modules.get<Actuators>()->startTwirl();
+            calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+                DPL_WES_CAL_DONE, TOPIC_DPL, WES_CALIBRATION_TIMEOUT);
+
             modules.get<WindEstimation>()
                 ->startWindEstimationSchemeCalibration();
-            return transition(&WingController::state_controlled_descent);
+
+            return HANDLED;
         }
         case EV_EXIT:
         {
+            EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId);
             return HANDLED;
         }
         case EV_EMPTY:
         {
             return tranSuper(&WingController::state_flying);
         }
-        // case DPL_WES_CAL_DONE:
-        // {
-        //     reset();
-        //     // Turn off the cutters
-        //     ModuleManager::getInstance().get<Actuators>()->cuttersOff();
-        //     return transition(&WingController::state_controlled_descent);
-        // }
+        case DPL_WES_CAL_DONE:
+        {
+            modules.get<Actuators>()->stopTwirl();
+
+            return transition(&WingController::state_controlled_descent);
+        }
         default:
         {
             return UNHANDLED;
@@ -191,8 +191,7 @@ State WingController::state_controlled_descent(const Boardcore::Event& event)
                      .get<NASController>()
                      ->getNasState()
                      .e});
-            ModuleManager::getInstance().get<Actuators>()->setServosOffset(
-                OFFSET);
+
             startAlgorithm();
             return HANDLED;
         }
@@ -259,52 +258,38 @@ bool WingController::addAlgorithms()
     WingAlgorithm* algorithm;
     WingAlgorithmData step;
 
-    bool result;
-    // We add an AutomaticWingAlgorithm and a FileWingAlgorithm
+    bool result = false;
 
+    // Algorithm 0
+    algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
+                                           PARAFOIL_RIGHT_SERVO, clGuidance);
+    result    = algorithm->init();
+    algorithms.push_back(algorithm);
+    // Algorithm 1
     algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
                                            PARAFOIL_RIGHT_SERVO, emGuidance);
-    // Ensure that the servos are correct
-    algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
-
-    // Init the algorithm
-    result = algorithm->init();
-
-    // Add the algorithm to the vector
+    result &= algorithm->init();
     algorithms.push_back(algorithm);
 
+    // Algorithm 2
     algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
-
-    for (int i = 0; i < 3; i++)
-    {
-        step.servo1Angle = 120;
-        step.servo2Angle = 0;
-        step.timestamp   = 0 + i * 40000000;
-        algorithm->addStep(step);
-        step.servo1Angle = 0;
-        step.servo2Angle = 0;
-        step.timestamp += 10000000 + i * 40000000;
-        algorithm->addStep(step);
-        step.servo1Angle = 0;
-        step.servo2Angle = 120;
-        step.timestamp += 10000000 + i * 40000000;
-        algorithm->addStep(step);
-        step.servo1Angle = 0;
-        step.servo2Angle = 0;
-        step.timestamp += 10000000 + i * 40000000;
-        algorithm->addStep(step);
-    }
-
-    // Ensure that the servos are correct
-    algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
-
-    // Init the algorithm
-    result = result && algorithm->init();
-
+    step.servo1Angle = 0;
+    step.servo2Angle = 120;
+    step.timestamp   = 0;
+    algorithm->addStep(step);
+    step.servo1Angle = 0;
+    step.servo2Angle = 0;
+    step.timestamp += 1000 * WingConfig::WING_STRAIGHT_FLIGHT_TIMEOUT;
+    algorithm->addStep(step);
+    step.servo1Angle = 0;
+    step.servo2Angle = 0;
+    step.timestamp += WingConfig::WING_STRAIGHT_FLIGHT_TIMEOUT;
+    algorithm->addStep(step);
+    result &= algorithm->init();
     // Add the algorithm to the vector
     algorithms.push_back(algorithm);
 
-    selectAlgorithm(0);
+    selectAlgorithm(SELECTED_ALGORITHM);
 
     return result;
 }
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.h b/src/boards/Parafoil/StateMachines/WingController/WingController.h
index bb845e3a2f7c74a2cef87a08299ceef508ad61e0..aa2f17f593c86225b960ca2aea0b1795bf734ff5 100644
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.h
+++ b/src/boards/Parafoil/StateMachines/WingController/WingController.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, Federico Mandelli, Angelo Prete
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
 #include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
 #include <Parafoil/Wing/WingAlgorithm.h>
 #include <events/HSM.h>
@@ -154,6 +155,12 @@ private:
      */
     EarlyManeuversGuidanceAlgorithm emGuidance;
 
+    /**
+     * @brief Instance of the Closed Loop Guidance Algorithm used by
+     * AutomaticWingAlgorithm
+     */
+    ClosedLoopGuidanceAlgorithm clGuidance;
+
     /**
      * @brief  starts the selected algorithm
      */