diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index c9c0d1b7b734ca9feac1d3d4156fdb9f9555f6f2..61651be6e71f7c446019347991a52a41ac58d388 100644
--- a/src/boards/Payload/Configs/WingConfig.h
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -35,7 +35,7 @@ namespace WingConfig
 
 // Algorithm configuration
 constexpr uint32_t WING_UPDATE_PERIOD = 100;  // [ms]
-constexpr uint8_t WING_CONTROLLER_ID  = 100;  // TODO define a correct ID
+constexpr uint8_t WING_CONTROLLER_ID  = 100;
 
 // Arm, start and flare thresholds
 constexpr float WING_ALGORITHM_ARM_ALTITUDE   = 250;   // [m]
diff --git a/src/boards/Payload/TMRepository/TMRepository.cpp b/src/boards/Payload/TMRepository/TMRepository.cpp
index 6121cf8a6a7c4de5619255d3bc5d872b20f5ef46..e548b8878027ad61e8192e315e20f43d580b6730 100644
--- a/src/boards/Payload/TMRepository/TMRepository.cpp
+++ b/src/boards/Payload/TMRepository/TMRepository.cpp
@@ -183,6 +183,9 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
             tm.pressure_dpl    = sensors.getDplPressureLastSample().pressure;
             tm.airspeed_pitot  = sensors.getPitotLastSample().airspeed;
 
+            // Altitude agl
+            tm.altitude_agl = -nasState.d;
+
             // IMU
             tm.acc_x  = imuData.accelerationX;
             tm.acc_y  = imuData.accelerationY;
@@ -225,7 +228,12 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
             tm.pin_nosecone =
                 PinHandler::getInstance().getPinsData()[NOSECONE_PIN].lastState;
 
-            // TODO: Add servo positions
+            // Servo positions
+            tm.left_servo_angle =
+                Actuators::getInstance().getServoAngle(PARAFOIL_LEFT_SERVO);
+
+            tm.right_servo_angle =
+                Actuators::getInstance().getServoAngle(PARAFOIL_RIGHT_SERVO);
 
             // Board status
             tm.vbat         = sensors.getBatteryVoltageLastSample().batVoltage;
diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
index 897032c3b7d94d8e30ce24c270452b07f2fe89d1..6543ea5dc2a5f5140ce9b338815ba1dbbc8c8640 100644
--- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
@@ -41,7 +41,6 @@ AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki,
                                                ServosList servo2)
     : WingAlgorithm(servo1, servo2)
 {
-    // TODO define umin and umax for antiwindup purposes
     controller = new PIController(Kp, Ki, WING_UPDATE_PERIOD / 1000.0f,
                                   -2.09439, 2.09439);
 }
@@ -60,7 +59,6 @@ void AutomaticWingAlgorithm::step()
         // UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample();
 
         // Target direction in respect to the current one
-        // TODO to be logged
         ReferenceValues reference =
             NASController::getInstance().getReferenceValues();
         Vector2f startingPosition =