diff --git a/src/boards/Parafoil/Sensors/Sensors.cpp b/src/boards/Parafoil/Sensors/Sensors.cpp
index 682aee410792e05ed4ef138133680396971e1bf3..a3352b24beec1b5040d1c17028696cdb3ec943d7 100644
--- a/src/boards/Parafoil/Sensors/Sensors.cpp
+++ b/src/boards/Parafoil/Sensors/Sensors.cpp
@@ -115,10 +115,12 @@ BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
 
 Sensors::Sensors(TaskScheduler* sched) : scheduler(sched), sensorsCounter(0) {}
 
+// TODO check calibration of gyro
+// TODO check axis of bmx
+
 bool Sensors::start()
 {
     // Read the magnetometer calibration from predefined file
-    magCalibration.fromFile("magCalibration.csv");
 
     // Init all the sensors
     bmx160Init();
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
index 0a86b129ff209c0dc9afc26e634e9ba245a5bee9..46a119d4cef2303175298933568d730923929da7 100644
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
@@ -180,7 +180,6 @@ State WingController::state_controlled_descent(const Boardcore::Event& event)
         case EV_ENTRY:  // start automatic algorithm
         {
             logStatus(WingControllerState::ALGORITHM_CONTROLLED);
-            selectAlgorithm(0);
             setEarlyManeuverPoints(
                 convertTargetPositionToNED(targetPositionGEO),
                 {ModuleManager::getInstance()
@@ -403,6 +402,7 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED,
 
     float distFromCenterline = 20;  // the distance that the M1 and M2 points
                                     // must have from the center line
+                                    // TODO add parameter
 
     // 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
@@ -426,6 +426,9 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED,
         currentPosNED;
 
     emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position);
+
+    clGuidance.setPoints(targetNED);
+
     WingTargetPositionData data;
 
     data.receivedLat = targetPositionGEO[0];
diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
index d60fd5f45028204080a5f5e0981a0bbb7130cb53..e56bd236e788bd60de821f058a2be529f81ef34a 100644
--- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
@@ -65,17 +65,17 @@ void AutomaticWingAlgorithm::step()
             modules.get<WindEstimation>()->getWindEstimationScheme());
 
         // Actuate the result
-        if (result > 0)
+        if (result >= 0 && result <= 1)
         {
             // Activate the servo1 and reset servo2
-            modules.get<Actuators>()->setServoAngle(servo1, result);
-            modules.get<Actuators>()->setServoAngle(servo2, 0);
+            modules.get<Actuators>()->setServo(servo1, result);
+            modules.get<Actuators>()->setServo(servo2, 0);
         }
-        else
+        else if (result < 0 && result <= -1)
         {
             // Activate the servo2 and reset servo1
-            modules.get<Actuators>()->setServoAngle(servo1, 0);
-            modules.get<Actuators>()->setServoAngle(servo2, result * -1);
+            modules.get<Actuators>()->setServo(servo1, 0);
+            modules.get<Actuators>()->setServo(servo2, result * -1);
         }
 
         // Log the servo positions
@@ -116,25 +116,30 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
 
     // All angle are computed as angle from the north direction
 
-    if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0)
-    {
-        // If we are not moving velocityAngle is 0
-        velocityAngle = 0;
-    }
-    else if (relativeVelocity[0] == 0)
-    {
-        // If we are not moving in the N S axis we interpret velocity[1]
-        velocityAngle = (relativeVelocity[1] > 0 ? 1 : -1) * Constants::PI / 2;
-    }
-    else if (relativeVelocity[1] == 0)
-    {
-        // If we are not moving in the E O axis we interpret velocity[0]
-        velocityAngle = (relativeVelocity[0] > 0 ? 0 : 1) * Constants::PI;
-    }
-    else
-    {
-        velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
-    }
+    // if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0)
+    // {
+    //     // If we are not moving velocityAngle is 0
+    //     velocityAngle = 0;
+    // }
+    // else if (relativeVelocity[0] == 0)
+    // {
+    //     // If we are not moving in the N S axis we interpret velocity[1]
+    //     velocityAngle = (relativeVelocity[1] > 0 ? 1 : -1) * Constants::PI /
+    //     2;
+    // }
+    // else if (relativeVelocity[1] == 0)
+    // {
+    //     // If we are not moving in the E O axis we interpret velocity[0]
+    //     velocityAngle = (relativeVelocity[0] > 0 ? 0 : 1) * Constants::PI;
+    // }
+    // else
+    // {
+    //     // angle in radiants of the velocity
+    //     velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
+    // }
+
+    // TODO test if this is safe
+    velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
 
     // Compute the angle difference
     float error = angleDiff(targetAngle, velocityAngle);
@@ -147,7 +152,8 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
     result = result * (180.f / Constants::PI);
 
     // Flip the servo orientation
-    result *= -1;
+    // result *= -1;
+    //  TODO check if this is needed
 
     // Logs the outputs
     {
@@ -167,7 +173,7 @@ float AutomaticWingAlgorithm::angleDiff(float a, float b)
 {
     float diff = a - b;
 
-    // Angle difference
+    //  Angle difference
     if (diff < -Constants::PI || Constants::PI < diff)
     {
         diff += Constants::PI;