wip

libraries/AP_GPS/AP_GPS_DroneCAN.cpp

@@ -37,7 +37,7 @@
 
 extern const AP_HAL::HAL& hal;
 
-#define GPS_UAVCAN_DEBUGGING 0
+#define GPS_UAVCAN_DEBUGGING 1
 
 #if GPS_UAVCAN_DEBUGGING
 #if defined(HAL_BUILD_AP_PERIPH)
@@ -335,44 +335,37 @@ void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const floa
     }
 }
 
-void AP_GPS_DroneCAN::update_fix_lag(uint64_t gnss_timestamp_usec)
-{
-    if (timesync_correction) {
-        // 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]: %lld/%lld %lld %f\n", _detected_modules[_detected_module].node_id, current_time, AP_HAL::micros64(), gnss_timestamp_usec, _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;
-    }
-}
-
 void AP_GPS_DroneCAN::update_relposned_lag()
 {
     if (timesync_correction && (last_gnss_timestamp_usec != 0)) {
         // calculate current time in GPS timescale using timesync_correction_us
         uint64_t current_time = timesync_correction_us + AP_HAL::micros64();
         // calculate lag between reported gnss timestamp and current time in gps timescale
-        _relposned_lag = (current_time - last_gnss_timestamp_usec) / 1000000.0;
-        _relposned_lag = MAX(0.05, MIN(_relposned_lag, 0.3)); // limit to [0.05,0.3] seconds
-        relposned_lag_valid = true;
+        _relposned_lag_us = current_time - last_gnss_timestamp_usec;
+        hal.console->printf("relposned_lag[%d]: %lu\n",  _detected_modules[_detected_module].node_id, _fix_lag_us);
+
+        _relposned_lag_us = MAX(0.05, MIN(_relposned_lag_us, 0.3));
     }
 }
 
 bool AP_GPS_DroneCAN::get_fix_lag(float &lag) const
 {
-    if (fix_lag_valid) {
-        lag = _fix_lag;
+    if (_fix_lag_us) {
+        // we have gps message time synchronized using the timesync message
+        // which is driven by PPS. So we have accurate timestamp of 
+        // which timeslice the solution was calculated for
+        lag = _fix_lag_us/1000000.0f;
         return true;
     }
     return AP_GPS_Backend::get_lag(lag);
 }
 
+// calculate the lag between the current time and the time the
+// relposned message was calculated for
 bool AP_GPS_DroneCAN::get_relposned_lag(float &lag) const
 {
-    if (relposned_lag_valid) {
-        lag = _relposned_lag;
+    if (_relposned_lag_us) {
+        lag = _relposned_lag_us/1000000.0f;
         return true;
     }
     return get_fix_lag(lag);
@@ -457,7 +450,6 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin
         }
 
         interim_state.num_sats = msg.sats_used;
-        update_fix_lag(msg.gnss_timestamp.usec);
     } else {
         interim_state.have_vertical_velocity = false;
         interim_state.have_vertical_accuracy = false;
@@ -473,11 +465,14 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin
     }
     uint64_t gps_message_timestamp_usec = msg.timestamp.usec;
     if (timesync_correction) {
-       // we have gps time synchronized even more accurately with the timesync message
-       gps_message_timestamp_usec = timestamp_usec + timesync_correction_us;
-    }
-    // fallback to using the timestamp from the message for correction
-    if ((gps_message_timestamp_usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) {
+        interim_state.last_corrected_gps_time_us = msg.gnss_timestamp.usec - timesync_correction_us;
+        interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
+        uint32_t fix_lag_us = AP_HAL::micros() - interim_state.last_corrected_gps_time_us;
+        if (fix_lag_us > 5000 && fix_lag_us < 250000 ) {
+            _fix_lag_us = fix_lag_us;
+        }
+        Debug("fix_lag[%d]: %lu\n",  _detected_modules[_detected_module].node_id, _fix_lag_us);
+    } else if ((gps_message_timestamp_usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) {
         // we have a valid timestamp based on gnss_timestamp timescale, we can use that to correct our gps message time
         interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(gps_message_timestamp_usec, timestamp_usec);
         interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
@@ -730,13 +725,14 @@ void AP_GPS_DroneCAN::handle_timesync_msg_trampoline(AP_DroneCAN *ap_dronecan, c
         // gnss timestamp timescale on GPS module. For AP_Periph, that would be calculated
         // using: Ublox Message Timestamp[GPS Time Domain] + DroneCAN transmit timestamp[Periph Time Domain] - Ublox Message Timestamp[Periph Time Domain]
         // Also it is expected that Ublox Message Timestamp[Periph Time Domain] is kept using PPS signal synced with GPS NAV messages.
+        int64_t last_timesync_correction_us = driver->timesync_correction_us;
         driver->timesync_correction_us = msg.previous_transmission_timestamp_usec - driver->last_timesync_timestamp_us;
+        driver->clock_drift = driver->timesync_correction_us - last_timesync_correction_us;
         driver->timesync_correction = true;
-        hal.console->printf("[%u]: ts: %lld/%lld correction: %lld\n",
+        Debug("[%u]: correction: %lld/%lld\n",
                       transfer.source_node_id,
-                      msg.previous_transmission_timestamp_usec,
-                      driver->last_timesync_timestamp_us,
-                      driver->timesync_correction_us);
+                      driver->timesync_correction_us,
+                      driver->clock_drift);
     }
     driver->last_timesync_timestamp_us = transfer.timestamp_usec;
     driver->last_transfer_id = transfer.transfer_id;

libraries/AP_GPS/AP_GPS_DroneCAN.h

@@ -90,9 +90,6 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     // returns true once configuration has finished
     bool do_config(void);
 
-    void update_fix_lag(uint64_t gnss_timestamp_usec);
-    void update_relposned_lag();
-
     bool get_lag(float &lag) const override {
         return get_fix_lag(lag);
     }
@@ -115,13 +112,11 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     static void give_registry();
     static AP_GPS_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
 
-    float _fix_lag;
-    bool fix_lag_valid;
     uint64_t last_gnss_timestamp_usec;
 
-    float _relposned_lag;
-    bool relposned_lag_valid;
-
+    void update_relposned_lag(void);
+    uint32_t _relposned_lag_us;
+    uint32_t _fix_lag_us;
     bool _new_data;
     AP_GPS::GPS_State interim_state;
 
@@ -173,6 +168,7 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     } _rtcm_stream;
 
     uint16_t last_transfer_id;
+    int64_t clock_drift;
 };
 
 #endif  // AP_GPS_DRONECAN_ENABLED

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -53,6 +53,9 @@
 // debug VALGET/VALSET configuration
 #define UBLOX_CFG_DEBUGGING 0
 
+// debug Lag calculation
+#define UBLOX_LAG_DEBUGGING 1 
+
 extern const AP_HAL::HAL& hal;
 
 #if UBLOX_DEBUGGING
@@ -94,6 +97,19 @@ extern const AP_HAL::HAL& hal;
  # define CFG_Debug(fmt, args ...)
 #endif
 
+#if UBLOX_LAG_DEBUGGING
+#if defined(HAL_BUILD_AP_PERIPH)
+ extern "C" {
+   void can_printf(const char *fmt, ...);
+ }
+ # define Lag_Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
+#else
+    # define Lag_Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#endif
+#else
+ # define Lag_Debug(fmt, args ...)
+#endif
+
 AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role _role) :
     AP_GPS_Backend(_gps, _state, _port),
     _next_message(STEP_PVT),
@@ -1578,6 +1594,9 @@ AP_GPS_UBLOX::_parse_gps(void)
         Debug("MSG_PVT");
 
         havePvtMsg = true;
+#ifdef HAL_GPIO_PPS
+        update_lag();
+#endif
         // position
         _check_new_itow(_buffer.pvt.itow);
         _last_pvt_itow = _buffer.pvt.itow;
@@ -2074,6 +2093,12 @@ AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const {
  */
 bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
 {
+#ifdef HAL_GPIO_PPS
+    if (_lag_us) {
+        lag_sec = _lag_us * 1.0e-6f;
+        return true;
+    }
+#endif
     switch (_hardware_generation) {
     case UBLOX_UNKNOWN_HARDWARE_GENERATION:
         lag_sec = 0.22f;
@@ -2131,6 +2156,22 @@ void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
     check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
 }
 
+#ifdef HAL_GPIO_PPS
+void AP_GPS_UBLOX::update_lag()
+{
+    // calculate lag
+    if (_last_pps_time_us != 0 &&
+        (state.status >= AP_GPS::GPS_OK_FIX_2D)) {
+        uint32_t last_message_rx_time = port->receive_time_constraint_us(_payload_length + sizeof(ubx_header) + 2) & 0xFFFFFFFF;
+        _lag_us = last_message_rx_time - _last_pps_time_us;
+        Lag_Debug("trx lag: %lu", AP_HAL::micros() - last_message_rx_time);
+        Lag_Debug("lag:%lu", _lag_us);
+        _lag_us = constrain_uint32(_lag_us, 5000, 250000); // [5ms, 250ms]
+        _last_pps_time_us = 0;
+    }
+}
+#endif
+
 // support for retrieving RTCMv3 data from a moving baseline base
 bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
 {

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -851,6 +851,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 #ifdef HAL_GPIO_PPS
     void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
     void set_pps_desired_freq(uint8_t freq) override;
+    void update_lag();
+    uint32_t _lag_us;
 #endif
 
     // status of active configuration for a role

libraries/AP_GPS/GPS_Backend.cpp

@@ -245,11 +245,7 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
 
         // get the time the packet arrived on the UART
         uint64_t uart_us;
-        if (_last_pps_time_us != 0 && (state.status >= AP_GPS::GPS_OK_FIX_2D)) {
-            // pps is only reliable when we have some sort of GPS fix
-            uart_us = _last_pps_time_us;
-            _last_pps_time_us = 0;
-        } else if (port) {
+        if (port) {
             uart_us = port->receive_time_constraint_us(msg_length);
         } else {
             uart_us = AP_HAL::micros64();

libraries/AP_GPS/LogStructure.h

@@ -66,6 +66,8 @@ struct PACKED log_GPS {
 // @Field: Und: Undulation
 // @Field: RTCMFU: RTCM fragments used
 // @Field: RTCMFD: RTCM fragments discarded
+// @Field: lag: GPS lag
+// @Field: C: clock drift in us
 struct PACKED log_GPA {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -82,6 +84,7 @@ struct PACKED log_GPA {
     uint16_t rtcm_fragments_used;
     uint16_t rtcm_fragments_discarded;
     float lag;
+    uint32_t C;
 };
 
 /*
@@ -208,7 +211,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",  "QBCCCCfBIHfHHf", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTFU,RTFD,lag", "s#mmmnd-ssm--s", "F-BBBB0-CC0--0" , true }, \
+      "GPA",  "QBCCCCfBIHfHHf", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RFU,RFD,lag,C", "s#mmmnd-ssm--ss", "F-BBBB0-CC0--0F" , 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), \

libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat

@@ -99,5 +99,3 @@ 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

libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat

@@ -134,3 +134,5 @@ define HAL_PERIPH_ENABLE_RC_OUT
 define HAL_PERIPH_ENABLE_NOTIFY
 
 define HAL_PERIPH_ENABLE_GLOBALTIMESYNC 1
+
+#define AP_INDUCE_PPM_OFFSET 6000

libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c

@@ -72,12 +72,12 @@ 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();
+#ifdef AP_INDUCE_PPM_OFFSET
+    // Please note that this will induce incorrect
+    // clock drift after 71 minutes, use hrt_micros64()
+    // as "now" here if you want to avoid that
+    now += (((uint64_t)now) * AP_INDUCE_PPM_OFFSET) / 1000000U;
 #endif
 #ifdef AP_BOARD_START_TIME
     now += AP_BOARD_START_TIME;
@@ -97,8 +97,8 @@ static uint64_t hrt_micros64I(void)
     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
+    // induce clock drift
+    ret += (ret * AP_INDUCE_PPM_OFFSET) / 1000000U;
 #endif
 #ifdef AP_BOARD_START_TIME
     ret += AP_BOARD_START_TIME;

libraries/AP_HAL_ChibiOS/system.cpp

@@ -353,8 +353,17 @@ __FASTRAMFUNC__ uint32_t micros()
 {
 #if CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
     // special case optimisation for 32 bit timers
-#ifdef AP_INDUCE_PPM_OFFSET
-    return hrt_micros32(); // ppm offset is induced in hrt_micros32
+#if defined(AP_INDUCE_PPM_OFFSET)
+    uint64_t now = st_lld_get_counter();
+#if defined(AP_BOARD_START_TIME)
+    now += AP_BOARD_START_TIME;
+#else
+    // Please note that this will induce incorrect
+    // clock drift after 71 minutes, use hrt_micros64()
+    // as "now" here if you want to avoid that
+    now += (now * AP_INDUCE_PPM_OFFSET) / 1000000U;
+    return now & 0xFFFFFFFF;
+#endif
 #elif defined(AP_BOARD_START_TIME)
     return st_lld_get_counter() + AP_BOARD_START_TIME;
 #else
@@ -367,9 +376,7 @@ __FASTRAMFUNC__ uint32_t micros()
 
 uint16_t micros16()
 {
-#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
+#if 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();