From 8dd9f5a455b5d2c5c6cd277f0f5ebefd70036d59 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Fri, 26 Jul 2024 19:25:10 +0200
Subject: [PATCH] [ARPxPRF] Serial changes to feed parafoil telemetry to ARP

---
 src/Groundstation/Automated/Hub.cpp       | 34 +++++++++++++++++++++++
 src/Groundstation/Common/Ports/Serial.cpp |  4 +++
 2 files changed, 38 insertions(+)

diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index 21d366c1e..f16bfaa0f 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -356,6 +356,40 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
         // Logger::getInstance().log(rocketST);
         Logger::getInstance().log(gpsState);
     }
+    else if (msg.msgid == MAVLINK_MSG_ID_PAYLOAD_FLIGHT_TM)
+    {
+        NASState nasState{
+            mavlink_msg_rocket_flight_tm_get_timestamp(&msg),
+            Eigen::Matrix<float, 13, 1>(
+                mavlink_msg_payload_flight_tm_get_nas_n(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_e(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_d(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_vn(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_ve(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_vd(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_qx(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_qy(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_qz(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_qw(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_bias_x(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_bias_y(&msg),
+                mavlink_msg_payload_flight_tm_get_nas_bias_z(&msg))};
+
+        GPSData gpsState;
+        gpsState.gpsTimestamp =
+            mavlink_msg_payload_flight_tm_get_timestamp(&msg);
+        gpsState.latitude  = mavlink_msg_payload_flight_tm_get_gps_lat(&msg);
+        gpsState.longitude = mavlink_msg_payload_flight_tm_get_gps_lon(&msg);
+        gpsState.height    = mavlink_msg_payload_flight_tm_get_gps_alt(&msg);
+        gpsState.fix       = mavlink_msg_payload_flight_tm_get_gps_fix(&msg);
+
+        // Set the rocket NAS
+        setRocketNasState(nasState);
+        setRocketOrigin(gpsState);
+
+        Logger::getInstance().log(nasState);
+        Logger::getInstance().log(gpsState);
+    }
 
     LyraGS::EthernetGS* ethernet = getModule<LyraGS::EthernetGS>();
     ethernet->sendMsg(msg);
diff --git a/src/Groundstation/Common/Ports/Serial.cpp b/src/Groundstation/Common/Ports/Serial.cpp
index 5c9cc2276..013444a71 100644
--- a/src/Groundstation/Common/Ports/Serial.cpp
+++ b/src/Groundstation/Common/Ports/Serial.cpp
@@ -47,6 +47,10 @@ void Serial::sendMsg(const mavlink_message_t& msg)
 
 void Serial::handleMsg(const mavlink_message_t& msg)
 {
+    // Patch for serial communication reception from payload
+    if (msg.msgid == MAVLINK_MSG_ID_PAYLOAD_FLIGHT_TM ||
+        msg.msgid == MAVLINK_MSG_ID_PAYLOAD_STATS_TM)
+        getModule<HubBase>()->dispatchIncomingMsg(msg);
     // Dispatch the message through the hub.
     getModule<HubBase>()->dispatchOutgoingMsg(msg);
 }
-- 
GitLab