diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp index adc96f2c6ac6e0f43b20e1b10e2cede944e2b877..cb56f69e61033e8c783f165cca22e4264580bc78 100644 --- a/src/Groundstation/Automated/Hub.cpp +++ b/src/Groundstation/Automated/Hub.cpp @@ -60,6 +60,7 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) sendNack(msg, 306); } + // Message for ARP if (msg.sysid == MAV_SYSID_ARP) { switch (msg.msgid) @@ -239,6 +240,18 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) } } } + + // In case the message is spoofed from ethernet by another groundstation + if (msg.sysid == MAV_SYSID_MAIN) + { + TRACE( + "[info] Hub: A MAIN packet was received from ground packet (not " + "radio)\n"); + /* The message received by ethernet (outgoing) in reality is not a + * command but the telemetry spoofed, therefore is then used as incoming + */ + dispatchIncomingMsg(msg); + } } void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) @@ -255,6 +268,13 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) { mavlink_rocket_flight_tm_t rocketTM; mavlink_msg_rocket_flight_tm_decode(&msg, &rocketTM); + uint64_t timestamp = mavlink_msg_rocket_flight_tm_get_timestamp(&msg); + /* Messages older and within the discard interval are treated as old + * messages*/ + if (timestamp <= lastFlightTMTimestamp && + lastFlightTMTimestamp > timestamp + DISCARD_MSG_DELAY) + return; + lastFlightTMTimestamp = timestamp; NASState nasState{ mavlink_msg_rocket_flight_tm_get_timestamp(&msg), Eigen::Matrix<float, 13, 1>( @@ -273,6 +293,12 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) { mavlink_rocket_stats_tm_t rocketST; mavlink_msg_rocket_stats_tm_decode(&msg, &rocketST); + /* Messages older and within the discard interval are treated as old + * messages*/ + if (rocketST.timestamp <= lastStatsTMTimestamp && + lastStatsTMTimestamp > rocketST.timestamp + DISCARD_MSG_DELAY) + return; + lastStatsTMTimestamp = rocketST.timestamp; GPSData gpsState; gpsState = getRocketOrigin(); diff --git a/src/Groundstation/Automated/Hub.h b/src/Groundstation/Automated/Hub.h index 9130dd058466db23751f9c511fd702c40ef3a5de..5453eac7cb8e89f3339c2c1b099a54dfd075a12f 100644 --- a/src/Groundstation/Automated/Hub.h +++ b/src/Groundstation/Automated/Hub.h @@ -42,6 +42,13 @@ class BoardStatus; namespace Antennas { +/* This is used to avoid discarding messages in case the rocket timestamp is + * reset. Therefore if older than the discard msg delay, resets the messages + * last timestamp */ +static constexpr uint64_t DISCARD_MSG_DELAY = + 10 * 1000000; ///< Maximum time for which the message, if older is + ///< discarded. [micros] + /** * @brief Central hub connecting all outgoing and ingoing modules. */ @@ -97,6 +104,8 @@ private: miosix::FastMutex coordinatesMutex; miosix::FastMutex nasStateMutex; bool flagNasSet = false; + uint64_t lastFlightTMTimestamp; + uint64_t lastStatsTMTimestamp; }; } // namespace Antennas