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