From d27819188a6f1163e60537caf8f308f286145810 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Wed, 2 Oct 2024 15:17:14 +0200
Subject: [PATCH] [ARP] Logging incoming FLIGHT_TM and STATS_TM messages
Now logging as raw telemetry messages the FLIGHT_TM and STATS_TM
---
src/boards/Groundstation/Automated/Hub.cpp | 44 +++++++++-------------
1 file changed, 17 insertions(+), 27 deletions(-)
diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index f6da796c5..0d28dd9cc 100644
--- a/src/boards/Groundstation/Automated/Hub.cpp
+++ b/src/boards/Groundstation/Automated/Hub.cpp
@@ -44,8 +44,6 @@ using namespace miosix;
void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg)
{
- bool send_ok = false;
-
LyraGS::BoardStatus* status = getModule<LyraGS::BoardStatus>();
LyraGS::RadioMain* radioMain = getModule<LyraGS::RadioMain>();
@@ -261,46 +259,38 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
// Extracting NAS rocket state
if (msg.msgid == MAVLINK_MSG_ID_ROCKET_FLIGHT_TM)
{
+ mavlink_rocket_flight_tm_t rocketTM;
+ mavlink_msg_rocket_flight_tm_decode(&msg, &rocketTM);
NASState nasState{
mavlink_msg_rocket_flight_tm_get_timestamp(&msg),
Eigen::Matrix<float, 13, 1>(
- mavlink_msg_rocket_flight_tm_get_nas_n(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_e(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_d(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_vn(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_ve(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_vd(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_qx(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_qy(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_qz(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_qw(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_bias_x(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_bias_y(&msg),
- mavlink_msg_rocket_flight_tm_get_nas_bias_z(&msg))};
+ rocketTM.nas_n, rocketTM.nas_e, rocketTM.nas_d, rocketTM.nas_vn,
+ rocketTM.nas_ve, rocketTM.nas_vd, rocketTM.nas_qx,
+ rocketTM.nas_qy, rocketTM.nas_qz, rocketTM.nas_qw,
+ rocketTM.nas_bias_x, rocketTM.nas_bias_y, rocketTM.nas_bias_z)};
// Set the rocket NAS
setRocketNasState(nasState);
+ Logger::getInstance().log(rocketTM);
Logger::getInstance().log(nasState);
}
else if (msg.msgid == MAVLINK_MSG_ID_ROCKET_STATS_TM)
{
+ mavlink_rocket_stats_tm_t rocketST;
+ mavlink_msg_rocket_stats_tm_decode(&msg, &rocketST);
GPSData gpsState;
gpsState = getRocketOrigin();
- /** In case a first gps coordinate with a good fix has been set, we
- * take then the origin reference*/
- if (gpsState.fix >= 3)
- {
- gpsState.gpsTimestamp =
- mavlink_msg_rocket_stats_tm_get_timestamp(&msg);
- gpsState.latitude = mavlink_msg_rocket_stats_tm_get_ref_lat(&msg);
- gpsState.longitude = mavlink_msg_rocket_stats_tm_get_ref_lon(&msg);
- gpsState.height = mavlink_msg_rocket_stats_tm_get_ref_alt(&msg);
- setRocketOrigin(gpsState);
+ gpsState.gpsTimestamp = rocketST.timestamp;
+ gpsState.latitude = rocketST.ref_lat;
+ gpsState.longitude = rocketST.ref_lon;
+ gpsState.height = rocketST.ref_alt;
- Logger::getInstance().log(gpsState);
- }
+ setRocketOrigin(gpsState);
+
+ Logger::getInstance().log(rocketST);
+ Logger::getInstance().log(gpsState);
}
LyraGS::EthernetGS* ethernet = getModule<LyraGS::EthernetGS>();
--
GitLab