From d80c7220663c576abc8abfea3acb2092b46b3d84 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:13 +0200
Subject: [PATCH] [ARP] Now using the ref messages from rocket_stat for the NAS
origin
Now the NAS origin uses the reference fields given by the stat_tm low frequency telemetry messages
---
src/boards/Groundstation/Automated/Hub.cpp | 52 +++++++++++++++----
src/boards/Groundstation/Automated/Hub.h | 8 +--
.../Groundstation/Automated/SMA/SMA.cpp | 2 +-
3 files changed, 46 insertions(+), 16 deletions(-)
diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index c3db6b0be..9158515d3 100644
--- a/src/boards/Groundstation/Automated/Hub.cpp
+++ b/src/boards/Groundstation/Automated/Hub.cpp
@@ -267,20 +267,50 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg)
mavlink_msg_rocket_flight_tm_get_nas_bias_y(&msg),
mavlink_msg_rocket_flight_tm_get_nas_bias_z(&msg))};
- GPSData gpsState;
- gpsState.gpsTimestamp =
- mavlink_msg_rocket_flight_tm_get_timestamp(&msg);
- gpsState.latitude = mavlink_msg_rocket_flight_tm_get_gps_lat(&msg);
- gpsState.longitude = mavlink_msg_rocket_flight_tm_get_gps_lon(&msg);
- gpsState.height = mavlink_msg_rocket_flight_tm_get_gps_alt(&msg);
- gpsState.fix = mavlink_msg_rocket_flight_tm_get_gps_fix(&msg);
+ GPSData gpsState = getRocketOrigin();
+ /** In case there is no previous correct gps data, we first set the
+ * origin with the first packet with a fix >= 3 */
+ if (gpsState.fix < 3)
+ {
+ gpsState.fix = mavlink_msg_rocket_flight_tm_get_gps_fix(&msg);
+ if (gpsState.fix >= 3)
+ {
+ gpsState.gpsTimestamp =
+ mavlink_msg_rocket_flight_tm_get_timestamp(&msg);
+ gpsState.latitude =
+ mavlink_msg_rocket_flight_tm_get_gps_lat(&msg);
+ gpsState.longitude =
+ mavlink_msg_rocket_flight_tm_get_gps_lon(&msg);
+ gpsState.height =
+ mavlink_msg_rocket_flight_tm_get_gps_alt(&msg);
+
+ setRocketOrigin(gpsState);
+
+ Logger::getInstance().log(gpsState);
+ }
+ }
// Set the rocket NAS
setRocketNasState(nasState);
- setRocketCoordinates(gpsState);
Logger::getInstance().log(nasState);
- Logger::getInstance().log(gpsState);
+ }
+ else if (msg.msgid == MAVLINK_MSG_ID_ROCKET_STATS_TM)
+ {
+ 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.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);
+
+ Logger::getInstance().log(gpsState);
+ }
}
LyraGS::EthernetGS* ethernet = getModule<LyraGS::EthernetGS>();
@@ -295,7 +325,7 @@ void Hub::sendAck(const mavlink_message_t& msg)
dispatchIncomingMsg(ackMsg);
}
-GPSData Hub::getRocketCoordinates()
+GPSData Hub::getRocketOrigin()
{
Lock<FastMutex> lock(coordinatesMutex);
return lastRocketCoordinates;
@@ -317,7 +347,7 @@ void Hub::setRocketNasState(const NASState& newRocketNasState)
lastRocketNasState = newRocketNasState;
}
-void Hub::setRocketCoordinates(const GPSData& newRocketCoordinates)
+void Hub::setRocketOrigin(const GPSData& newRocketCoordinates)
{
Lock<FastMutex> lock(coordinatesMutex);
lastRocketCoordinates = newRocketCoordinates;
diff --git a/src/boards/Groundstation/Automated/Hub.h b/src/boards/Groundstation/Automated/Hub.h
index 4d72f1514..32d5563b1 100644
--- a/src/boards/Groundstation/Automated/Hub.h
+++ b/src/boards/Groundstation/Automated/Hub.h
@@ -69,9 +69,9 @@ public:
void sendAck(const mavlink_message_t& msg);
/**
- * @brief Synchronized getter for the last rocket coordinates.
+ * @brief Synchronized getter for the last rocket origin for NAS.
*/
- Boardcore::GPSData getRocketCoordinates();
+ Boardcore::GPSData getRocketOrigin();
/**
* @brief Synchronized getter for the last rocket NAS state.
@@ -87,9 +87,9 @@ private:
void setRocketNasState(const Boardcore::NASState& newRocketNasState);
/**
- * @brief Synchronized setter for the last rocket coordinates.
+ * @brief Synchronized setter for the last rocket origin for NAS.
*/
- void setRocketCoordinates(const Boardcore::GPSData& newRocketCoordinates);
+ void setRocketOrigin(const Boardcore::GPSData& newRocketCoordinates);
Boardcore::GPSData lastRocketCoordinates;
Boardcore::NASState lastRocketNasState;
diff --git a/src/boards/Groundstation/Automated/SMA/SMA.cpp b/src/boards/Groundstation/Automated/SMA/SMA.cpp
index ad4062848..059735760 100644
--- a/src/boards/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/boards/Groundstation/Automated/SMA/SMA.cpp
@@ -193,7 +193,7 @@ void SMA::update()
Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
- rocketCoordinates = hub->getRocketCoordinates();
+ rocketCoordinates = hub->getRocketOrigin();
if (rocketCoordinates.fix != 0)
{
// update follower with the rocket GPS data
--
GitLab