From 2d09c65fb3b42f500f0c5c10115d776164f7b87c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Wed, 9 Apr 2025 14:05:47 +0200
Subject: [PATCH] [ARPxPRF] Parafoil stats TM used to set the origin
- Hub.cpp: Before the origin was fixed from the GPS data from the flight tm message while now it should be with the PAYLOAD_STATS_TM reference fields
---
src/Groundstation/Automated/Hub.cpp | 28 +++++++++++++++++++---------
1 file changed, 19 insertions(+), 9 deletions(-)
diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp
index d3168a273..63d8b212d 100644
--- a/src/Groundstation/Automated/Hub.cpp
+++ b/src/Groundstation/Automated/Hub.cpp
@@ -355,19 +355,29 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& 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);
+ }
+ else if (msg.msgid == MAVLINK_MSG_ID_PAYLOAD_STATS_TM)
+ {
+ mavlink_payload_stats_tm_t payloadST;
+ mavlink_msg_payload_stats_tm_decode(&msg, &payloadST);
+
+ // TODO: The origin should have its own struct since only timestamp and
+ // [lat, lon, alt] are needed
+ GPSData gpsState;
+ getRocketOrigin(gpsState);
+ gpsState.gpsTimestamp = payloadST.timestamp;
+ gpsState.latitude = payloadST.ref_lat;
+ gpsState.longitude = payloadST.ref_lon;
+ gpsState.height = payloadST.ref_alt;
+ gpsState.fix = 3;
+
+ setRocketOrigin(gpsState);
+
+ // Logger::getInstance().log(rocketST);
Logger::getInstance().log(gpsState);
}
--
GitLab