wip

bugobliterator · Jan 31, 2024 · c34cdca · c34cdca
1 parent 373d187
commit c34cdca
Show file tree

Hide file tree

Showing 6 changed files with 25 additions and 5 deletions.
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
@@ -2295,11 +2295,12 @@ void AP_GPS::Write_GPS(uint8_t i)
 
     /* write auxiliary accuracy information as well */
     float hacc = 0, vacc = 0, sacc = 0;
-    float undulation = 0;
+    float undulation = 0, lag = 0;
     horizontal_accuracy(i, hacc);
     vertical_accuracy(i, vacc);
     speed_accuracy(i, sacc);
     get_undulation(i, undulation);
+    get_lag(i, lag);
     struct log_GPA pkt2{
         LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
         time_us       : time_us,
@@ -2314,7 +2315,8 @@ void AP_GPS::Write_GPS(uint8_t i)
         delta_ms      : last_message_delta_time_ms(i),
         undulation    : undulation,
         rtcm_fragments_used: rtcm_stats.fragments_used,
-        rtcm_fragments_discarded: rtcm_stats.fragments_discarded
+        rtcm_fragments_discarded: rtcm_stats.fragments_discarded,
+        lag           : lag
     };
     AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
 }

diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
@@ -341,6 +341,7 @@ void AP_GPS_DroneCAN::update_fix_lag(uint64_t gnss_timestamp_usec)
         // calculate precise lag
         uint64_t current_time = timesync_correction_us + AP_HAL::micros64();
         _fix_lag = (current_time - gnss_timestamp_usec) / 1000000.0;
+        hal.console->printf("fix_lag[%d]: %f\n", _detected_modules[_detected_module].node_id, _fix_lag);
         _fix_lag = MAX(0.05, MIN(_fix_lag, 0.3)); // limit to [0.05,0.3] seconds
         last_gnss_timestamp_usec = gnss_timestamp_usec;
         fix_lag_valid = true;

diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h
@@ -81,6 +81,7 @@ struct PACKED log_GPA {
     float undulation;
     uint16_t rtcm_fragments_used;
     uint16_t rtcm_fragments_discarded;
+    float lag;
 };
 
 /*
@@ -207,7 +208,7 @@ struct PACKED log_GPS_RAWS {
     { LOG_GPS_MSG, sizeof(log_GPS), \
       "GPS",  "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#-s-S-DUmnhnh-", "F--C-0BGGB000--" , true }, \
     { LOG_GPA_MSG,  sizeof(log_GPA), \
-      "GPA",  "QBCCCCfBIHfHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTCMFU,RTCMFD", "s#-mmnd-ssm--", "F-BBBB0-CC0--" , true }, \
+      "GPA",  "QBCCCCfBIHfHHf", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTFU,RTFD,lag", "s#mmmnd-ssm--s", "F-BBBB0-CC0--0" , true }, \
     { LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
       "UBX1", "QBHBBHI",  "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------"  , true }, \
     { LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat
@@ -99,3 +99,5 @@ BOARD_VALIDATE $CHECK_IMU0_PRESENT $CHECK_IMU1_PRESENT $CHECK_IMU2_PRESENT $CHEC
 # build ABIN for flash-from-bootloader support:
 env BUILD_ABIN True
 define HAL_INS_HIGHRES_SAMPLE 6
+
+define AP_INDUCE_PPM_OFFSET 2000
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
@@ -72,7 +72,13 @@ static uint32_t system_time_u32_us(void)
 
 static uint32_t get_systime_us32(void)
 {
+#ifdef AP_INDUCE_PPM_OFFSET
     uint32_t now = system_time_u32_us();
+    uint32_t num_seconds = hrt_micros64() / 1000000U;
+    now += num_seconds * AP_INDUCE_PPM_OFFSET; // induce a PPM offset
+#else
+    uint32_t now = system_time_u32_us();
+#endif
 #ifdef AP_BOARD_START_TIME
     now += AP_BOARD_START_TIME;
 #endif
@@ -90,6 +96,10 @@ static uint64_t hrt_micros64I(void)
 #if CH_CFG_ST_FREQUENCY != 1000000U
     ret *= 1000000U/CH_CFG_ST_FREQUENCY;
 #endif
+#ifdef AP_INDUCE_PPM_OFFSET
+    uint64_t num_seconds = ret / 1000000U;
+    ret += num_seconds * AP_INDUCE_PPM_OFFSET; // induce a PPM offset
+#endif
 #ifdef AP_BOARD_START_TIME
     ret += AP_BOARD_START_TIME;
 #endif

diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -353,7 +353,9 @@ __FASTRAMFUNC__ uint32_t micros()
 {
 #if CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
     // special case optimisation for 32 bit timers
-#ifdef AP_BOARD_START_TIME
+#ifdef AP_INDUCE_PPM_OFFSET
+    return hrt_micros32(); // ppm offset is induced in hrt_micros32
+#elif defined(AP_BOARD_START_TIME)
     return st_lld_get_counter() + AP_BOARD_START_TIME;
 #else
     return st_lld_get_counter();
@@ -365,7 +367,9 @@ __FASTRAMFUNC__ uint32_t micros()
 
 uint16_t micros16()
 {
-#if CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
+#ifdef AP_INDUCE_PPM_OFFSET
+    return hrt_micros32() & 0xFFFF; // ppm offset is induced in hrt_micros32
+#elif CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
     return st_lld_get_counter() & 0xFFFF;
 #elif CH_CFG_ST_RESOLUTION == 16 && CH_CFG_ST_FREQUENCY==1000000U
     return st_lld_get_counter();