diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index 52295a00f836775727a3dd8a8117b65635573f2f..6cef566dc4859f385e7fefd29ba97ee13661e003 100644
--- a/src/boards/Payload/Radio/MessageHandler.cpp
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -35,6 +35,7 @@
 #include <common/Topics.h>
 #include <diagnostic/CpuMeter/CpuMeter.h>
 #include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
 
 #include "Radio.h"
 
@@ -190,7 +191,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg)
             parent.getModule<AltitudeTrigger>()->setDeploymentAltitude(
                 altitude);
 
-            if (altitude < 0 || altitude > 3000)
+            if (altitude < 100 || altitude > 3000)
             {
                 return enqueueWack(msg);
             }
@@ -200,6 +201,45 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg)
             }
         }
 
+        case MAVLINK_MSG_ID_SET_TARGET_COORDINATES_TC:
+        {
+            float latitude =
+                mavlink_msg_set_target_coordinates_tc_get_latitude(&msg);
+            float longitude =
+                mavlink_msg_set_target_coordinates_tc_get_longitude(&msg);
+
+            bool targetSet =
+                parent.getModule<WingController>()->setTargetCoordinates(
+                    latitude, longitude);
+
+            if (targetSet)
+            {
+                return enqueueAck(msg);
+            }
+            else
+            {
+                return enqueueNack(msg);
+            }
+        }
+
+        case MAVLINK_MSG_ID_SET_ALGORITHM_TC:
+        {
+            uint8_t index =
+                mavlink_msg_set_algorithm_tc_get_algorithm_number(&msg);
+
+            bool algorithmSet =
+                parent.getModule<WingController>()->selectAlgorithm(index);
+
+            if (algorithmSet)
+            {
+                return enqueueAck(msg);
+            }
+            else
+            {
+                return enqueueNack(msg);
+            }
+        }
+
         case MAVLINK_MSG_ID_RAW_EVENT_TC:
         {
             uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg);
@@ -534,11 +574,13 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             mavlink_payload_stats_tm_t tm;
 
             auto* nas        = parent.getModule<NASController>();
+            auto* wnc        = parent.getModule<WingController>();
             auto* pinHandler = parent.getModule<PinHandler>();
             auto& logger     = Logger::getInstance();
 
             auto stats    = parent.getModule<FlightStatsRecorder>()->getStats();
             auto ref      = nas->getReferenceValues();
+            auto emPoints = wnc->getEarlyManeuverPoints();
             auto cpuStats = CpuMeter::getCpuStats();
             auto loggerStats = logger.getStats();
 
@@ -567,12 +609,12 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             tm.apogee_max_acc    = stats.apogeeMaxAcc;
 
             // Wing stats
-            tm.wing_emc_n = -1.0f;  // TODO
-            tm.wing_emc_e = -1.0f;  // TODO
-            tm.wing_m1_n  = -1.0f;  // TODO
-            tm.wing_m1_e  = -1.0f;  // TODO
-            tm.wing_m2_n  = -1.0f;  // TODO
-            tm.wing_m2_e  = -1.0f;  // TODO
+            tm.wing_emc_n = emPoints.emcN;
+            tm.wing_emc_e = emPoints.emcE;
+            tm.wing_m1_n  = emPoints.m1N;
+            tm.wing_m1_e  = emPoints.m1E;
+            tm.wing_m2_n  = emPoints.m2N;
+            tm.wing_m2_e  = emPoints.m2E;
 
             // Deployment stats
             tm.dpl_ts         = stats.dplTs;
@@ -596,14 +638,14 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             tm.log_number = loggerStats.logNumber;
 
             tm.nas_state = static_cast<uint8_t>(nas->getState());
-            tm.wes_state = 255;  // TODO
+            tm.wes_state = static_cast<uint8_t>(wnc->getState());
 
             // Pins
             tm.pin_launch =
                 pinHandler->getPinData(PinList::RAMP_DETACH_PIN).lastState;
             tm.pin_nosecone =
                 pinHandler->getPinData(PinList::NOSECONE_DETACH_PIN).lastState;
-            tm.cutter_presence = 255;  // TODO
+            tm.cutter_presence = miosix::sense::cutterSense::value();
 
             auto canStatus = parent.getModule<CanHandler>()->getCanStatus();
             tm.main_board_state  = canStatus.mainState;
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index 13d02817f39e78d06e9dbb718adb09688bad3064..156560b12be176499dee468d1549199d72b85210 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -324,7 +324,7 @@ bool WingController::isStarted() { return started; }
 
 WingControllerState WingController::getState() { return state; }
 
-bool WingController::setTargetPosition(Eigen::Vector2f targetPositionGEO)
+bool WingController::setTargetCoordinates(float latitude, float longitude)
 {
     // Allow changing the target position in the IDLE state only
     if (state != WingControllerState::IDLE)
@@ -332,11 +332,22 @@ bool WingController::setTargetPosition(Eigen::Vector2f targetPositionGEO)
         return false;
     }
 
-    this->targetPositionGEO = targetPositionGEO;
+    this->targetPositionGEO = Eigen::Vector2f{latitude, longitude};
+
+    // Log early maneuver points to highlight any discrepancies if any
+    auto earlyManeuverPoints = getEarlyManeuverPoints();
 
     auto data = WingTargetPositionData{
-        .targetLat = targetPositionGEO[0], .targetLon = targetPositionGEO[1],
-        // TODO: populate early maneuver points
+        .targetLat = targetPositionGEO[0],
+        .targetLon = targetPositionGEO[1],
+        .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);
 
@@ -375,6 +386,11 @@ bool WingController::selectAlgorithm(uint8_t index)
     }
 }
 
+EarlyManeuversPoints WingController::getEarlyManeuverPoints()
+{
+    return emGuidance.getPoints();
+}
+
 void WingController::loadAlgorithms()
 {
     // Early Maneuver Guidance Automatic Algorithm
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
index 66874623bff274daff39d7381e80922d22bda4ff..b1a5d3187f5e273fbe446b74bba2b76613e25d93 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.h
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -104,10 +104,9 @@ public:
     WingControllerState getState();
 
     /**
-     * @brief Sets the target position.
-     * @note The provided position must be using geodetic coordinates.
+     * @brief Sets the target coordinates.
      */
-    bool setTargetPosition(Eigen::Vector2f targetPositionGEO);
+    bool setTargetCoordinates(float latitude, float longitude);
 
     /**
      * @brief Changes the selected algorithm.
@@ -115,6 +114,11 @@ public:
      */
     bool selectAlgorithm(uint8_t index);
 
+    /**
+     * @brief Returns the currently set early maneuver points.
+     */
+    EarlyManeuversPoints getEarlyManeuverPoints();
+
     /**
      * @brief This is a forward method to the early maneuvers guidance algorithm
      * to calculate the yaw angle of the parafoil knowing the current position
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
index 698f5f6cb04493188ee52a8b65b7ba5df712e028..e4da61dd669860cf59608b4fa499abf3f9976832 100644
--- a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
@@ -147,4 +147,18 @@ void EarlyManeuversGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED,
     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],
+    };
+}
+
 }  // namespace Payload
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
index 30a432504dbf6404c0219f62d88c4490c8d0f4b0..c52f76189a7bd9f7b195f966e157a3d1af0f3a39 100644
--- a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
@@ -29,6 +29,18 @@
 namespace Payload
 {
 
+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
@@ -75,6 +87,11 @@ public:
     void setPoints(Eigen::Vector2f targetNED, Eigen::Vector2f EMC,
                    Eigen::Vector2f M1, Eigen::Vector2f M2);
 
+    /**
+     * @brief Get Early Maneuvers points.
+     */
+    EarlyManeuversPoints getPoints();
+
 private:
     /** @brief Updates the class target
      *