AP_NavEKF3: add logging of time delta between GPS sample timestamps

bugobliterator · Feb 11, 2024 · 1801bdd · 1801bdd
1 parent 3ab6422
commit 1801bdd
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 3 deletions.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -174,7 +174,8 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
         timeouts : timeoutStatus,
         solution : solutionStatus.value,
         gps : gpsCheckStatus.value,
-        primary : frontend->getPrimaryCoreIndex()
+        primary : frontend->getPrimaryCoreIndex(),
+        gps_time_delta_ms : gps_time_delta_ms
     };
     AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
 }

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -576,6 +576,13 @@ void NavEKF3_core::readGpsData()
     gps.get_lag(selected_gps, gps_delay_sec);
     gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);
 
+    if (last_gpsdata_time_ms && gpsDataNew.time_ms &&
+        (last_gpsdata_time_ms != gpsDataNew.time_ms)) {
+        gps_time_delta_ms = gpsDataNew.time_ms - last_gpsdata_time_ms;
+    } else {
+        gps_time_delta_ms = 0;
+    }
+    last_gpsdata_time_ms = gpsDataNew.time_ms;
     // Correct for the average intersampling delay due to the filter updaterate
     gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
 

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1200,7 +1200,8 @@ class NavEKF3_core : public NavEKF_core_common
     uint32_t firstInitTime_ms;      // First time the initialise function was called (msec)
     uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec)
     ftype tiltErrorVariance;        // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
-
+    int32_t gps_time_delta_ms; // time delta between GPS time measurements (msec)
+    uint32_t last_gpsdata_time_ms; // time of last GPS time measurement (msec)
     // variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
     struct {
         ftype pos;

diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h
@@ -189,6 +189,7 @@ struct PACKED log_XKF3 {
 // @Field: SS: Filter solution status
 // @Field: GPS: Filter GPS status
 // @Field: PI: Primary core index
+// @Field: GDT: GPS sample time delta
 struct PACKED log_XKF4 {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -206,6 +207,7 @@ struct PACKED log_XKF4 {
     uint32_t solution;
     uint16_t gps;
     int8_t primary;
+    int32_t gps_time_delta_ms; 
 };
 
 
@@ -431,7 +433,7 @@ struct PACKED log_XKV {
     { LOG_XKF3_MSG, sizeof(log_XKF3), \
       "XKF3","QBcccccchhhccff","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT,RErr,ErSc", "s#nnnmmmGGGd?--", "F-BBBBBBCCCBB00" , true }, \
     { LOG_XKF4_MSG, sizeof(log_XKF4), \
-      "XKF4","QBcccccfffHBIHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------mm-----", "F-------??-----" , true }, \
+      "XKF4","QBcccccfffHBIHbi","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI,GDT", "s#------mm-----s", "F-------??-----C" , true }, \
     { LOG_XKF5_MSG, sizeof(log_XKF5), \
       "XKF5","QBBhhhcccCCfff","TimeUS,C,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s#----m???mrnm", "F-----BBBBB000" , true }, \
     { LOG_XKFD_MSG, sizeof(log_XKFD), \