From e39c8377a0d8d6cc65d1a3e43137279129e358b9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Mon, 9 Sep 2024 19:12:31 +0200
Subject: [PATCH] [Payload][Radio] Update telemetry for updated mavlink

---
 src/boards/Payload/Radio/MessageHandler.cpp   | 59 ++++++++++---------
 .../WingController/WingController.cpp         | 52 ++++++++++------
 .../WingController/WingController.h           | 32 ++++++++--
 .../EarlyManeuverGuidanceAlgorithm.cpp        | 17 ++++++
 .../EarlyManeuversGuidanceAlgorithm.h         |  8 ++-
 5 files changed, 117 insertions(+), 51 deletions(-)

diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index 6cef566dc..a9be8eb26 100644
--- a/src/boards/Payload/Radio/MessageHandler.cpp
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -502,19 +502,21 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             auto* wes     = parent.getModule<WindEstimation>();
             auto* fmm     = parent.getModule<FlightModeManager>();
 
-            auto imu         = sensors->getLSM6DSRXLastSample();
-            auto mag         = sensors->getLIS2MDLLastSample();
-            auto gps         = sensors->getUBXGPSLastSample();
-            auto pressDigi   = sensors->getLPS28DFWLastSample();
-            auto pressStatic = sensors->getStaticPressureLastSample();
-            auto nasState    = nas->getNasState();
-            auto wind        = wes->getWindEstimationScheme();
-
-            tm.timestamp       = TimestampTimer::getTimestamp();
-            tm.pressure_digi   = pressDigi.pressure;
-            tm.pressure_static = pressStatic.pressure;
-            tm.airspeed_pitot  = -1.0f;  // TODO
-            tm.altitude_agl    = -nasState.d;
+            auto imu          = sensors->getLSM6DSRXLastSample();
+            auto mag          = sensors->getLIS2MDLLastSample();
+            auto gps          = sensors->getUBXGPSLastSample();
+            auto pressDigi    = sensors->getLPS28DFWLastSample();
+            auto pressStatic  = sensors->getStaticPressureLastSample();
+            auto pressDynamic = sensors->getDynamicPressureLastSample();
+            auto nasState     = nas->getNasState();
+            auto wind         = wes->getWindEstimationScheme();
+
+            tm.timestamp        = TimestampTimer::getTimestamp();
+            tm.pressure_digi    = pressDigi.pressure;
+            tm.pressure_static  = pressStatic.pressure;
+            tm.pressure_dynamic = pressDynamic.pressure;
+            tm.airspeed_pitot   = -1.0f;  // TODO
+            tm.altitude_agl     = -nasState.d;
 
             // Sensors
             tm.acc_x   = imu.accelerationX;
@@ -551,8 +553,8 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             tm.nas_bias_x = nasState.bx;
             tm.nas_bias_y = nasState.by;
             tm.nas_bias_z = nasState.bz;
-            tm.wes_n      = wind[0];
-            tm.wes_e      = wind[1];
+            tm.wes_n      = wind.x();
+            tm.wes_e      = wind.y();
 
             tm.battery_voltage     = sensors->getBatteryVoltage().batVoltage;
             tm.cam_battery_voltage = sensors->getCamBatteryVoltage().batVoltage;
@@ -578,16 +580,20 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             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();
+            auto stats = parent.getModule<FlightStatsRecorder>()->getStats();
+            auto ref   = nas->getReferenceValues();
+            auto wingTarget       = wnc->getTargetCoordinates();
+            auto wingActiveTarget = wnc->getActiveTarget();
+            auto wingAlgorithm    = wnc->getSelectedAlgorithm();
+            auto cpuStats         = CpuMeter::getCpuStats();
+            auto loggerStats      = logger.getStats();
 
             // Log CPU stats and reset them
             CpuMeter::resetCpuStats();
             logger.log(cpuStats);
 
+            tm.timestamp = TimestampTimer::getTimestamp();
+
             // Liftoff stats
             tm.liftoff_ts         = stats.liftoffTs;
             tm.liftoff_max_acc_ts = stats.liftoffMaxAccTs;
@@ -609,12 +615,11 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             tm.apogee_max_acc    = stats.apogeeMaxAcc;
 
             // Wing stats
-            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;
+            tm.wing_target_lat      = wingTarget.x();
+            tm.wing_target_lon      = wingTarget.y();
+            tm.wing_active_target_n = wingActiveTarget.x();
+            tm.wing_active_target_e = wingActiveTarget.y();
+            tm.wing_algorithm       = wingAlgorithm;
 
             // Deployment stats
             tm.dpl_ts         = stats.dplTs;
@@ -638,7 +643,7 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             tm.log_number = loggerStats.logNumber;
 
             tm.nas_state = static_cast<uint8_t>(nas->getState());
-            tm.wes_state = static_cast<uint8_t>(wnc->getState());
+            tm.wnc_state = static_cast<uint8_t>(wnc->getState());
 
             // Pins
             tm.pin_launch =
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index 156560b12..3bd2144a6 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -324,6 +324,11 @@ 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
@@ -332,14 +337,14 @@ bool WingController::setTargetCoordinates(float latitude, float longitude)
         return false;
     }
 
-    this->targetPositionGEO = Eigen::Vector2f{latitude, longitude};
+    targetPositionGEO = Coordinates{latitude, longitude};
 
     // Log early maneuver points to highlight any discrepancies if any
     auto earlyManeuverPoints = getEarlyManeuverPoints();
 
     auto data = WingTargetPositionData{
-        .targetLat = targetPositionGEO[0],
-        .targetLon = targetPositionGEO[1],
+        .targetLat = latitude,
+        .targetLon = longitude,
         .targetN   = earlyManeuverPoints.targetN,
         .targetE   = earlyManeuverPoints.targetE,
         .emcN      = earlyManeuverPoints.emcN,
@@ -354,6 +359,11 @@ bool WingController::setTargetCoordinates(float latitude, float longitude)
     return true;
 }
 
+uint8_t WingController::getSelectedAlgorithm()
+{
+    return static_cast<uint8_t>(selectedAlgorithm.load());
+}
+
 bool WingController::selectAlgorithm(uint8_t index)
 {
     // Allow changing the algorithm in the IDLE state only
@@ -391,6 +401,11 @@ EarlyManeuversPoints WingController::getEarlyManeuverPoints()
     return emGuidance.getPoints();
 }
 
+Eigen::Vector2f WingController::getActiveTarget()
+{
+    return emGuidance.getActiveTarget();
+}
+
 void WingController::loadAlgorithms()
 {
     // Early Maneuver Guidance Automatic Algorithm
@@ -505,17 +520,18 @@ void WingController::updateEarlyManeuverPoints()
 {
     using namespace Eigen;
 
-    auto nas      = getModule<NASController>();
-    auto nasState = nas->getNasState();
-    auto nasRef   = nas->getReferenceValues();
+    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(
-                 targetPositionGEO, {nasRef.refLatitude, nasRef.refLongitude});
+                 targetGEO, {nasRef.refLatitude, nasRef.refLongitude});
 
     Vector2f targetOffsetNED = targetNED - currentPositionNED;
     Vector2f normPoint       = targetOffsetNED / targetOffsetNED.norm();
-    float psi0               = atan2(normPoint[1], normPoint[0]);
+    float psi0               = atan2(normPoint.y(), normPoint.x());
 
     float distFromCenterline = 20;  // the distance that the M1 and M2 points
                                     // must have from the center line
@@ -545,16 +561,16 @@ void WingController::updateEarlyManeuverPoints()
 
     // Log the updated points
     auto data = WingTargetPositionData{
-        .targetLat = targetPositionGEO[0],
-        .targetLon = targetPositionGEO[1],
-        .targetN   = targetNED[0],
-        .targetE   = targetNED[1],
-        .emcN      = emcPosition[0],
-        .emcE      = emcPosition[1],
-        .m1N       = m1Position[0],
-        .m1E       = m1Position[1],
-        .m2N       = m2Position[0],
-        .m2E       = m2Position[1],
+        .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);
 }
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
index b1a5d3187..4c3ada8ed 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.h
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -103,11 +103,22 @@ public:
 
     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.
@@ -119,6 +130,11 @@ public:
      */
     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
@@ -193,12 +209,18 @@ private:
 
     void updateState(WingControllerState newState);
 
-    Eigen::Vector2f targetPositionGEO{Config::Wing::Default::TARGET_LAT,
-                                      Config::Wing::Default::TARGET_LON};
+    struct Coordinates
+    {
+        float latitude;
+        float longitude;
+
+        operator Eigen::Vector2f() const { return {latitude, longitude}; }
+    };
 
-    std::atomic<bool> targetPositionDirty{
-        false};  ///< Whether the target position was changed and EM points need
-                 ///< to be updated
+    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};
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
index e4da61dd6..aed38bf29 100644
--- a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
@@ -161,4 +161,21 @@ EarlyManeuversPoints EarlyManeuversGuidanceAlgorithm::getPoints()
     };
 }
 
+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 Payload
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
index c52f76189..58ef53d3f 100644
--- a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
@@ -92,6 +92,12 @@ public:
      */
     EarlyManeuversPoints getPoints();
 
+    /**
+     * @brief Get the active target.
+     * @return The NED coordinates of the active target.
+     */
+    Eigen::Vector2f getActiveTarget();
+
 private:
     /** @brief Updates the class target
      *
@@ -110,7 +116,7 @@ private:
     };
 
     // Point we are currently poinying to
-    Target activeTarget;
+    std::atomic<Target> activeTarget;
 
     // Eigen::Vector2f targetNED;  // NED
 
-- 
GitLab