diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp index 21d366c1eae2141fa73d6e08713d774584385d94..f16bfaa0ff0bf959b36045aa6f2a09da5cb6e3c7 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 5c9cc2276b980eb726136e7b197be153d8b49d02..013444a71f88cfe74f400f97f60181c327addd94 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); }