diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp
index c3db6b0be3b32f99a3353d7079d4b6e95959f1bf..9158515d319e61cfc99f4eaa7c1984bff5c61edb 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 4d72f1514dbcb6523184e53131b2fdd2500623b9..32d5563b18e2ca7ed606820dd2af7bf6a1341162 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 ad40628486f35bad5fe72920d754b76557b681f5..059735760b59e030fda4415f9a8dc717a32db583 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