From 54307bfdd851d4aaff873a2119c8bce247dae26c Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 24 Oct 2023 12:44:29 +1100 Subject: [PATCH 1/9] AP_GPS: add support for synchronizing time using Global Time Sync --- libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 41 ++++++++++++++++++++++------ libraries/AP_GPS/AP_GPS_DroneCAN.h | 5 ++++ 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 5da56758a517c..a7f67649345b4 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -116,6 +116,10 @@ void AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) AP_BoardConfig::allocation_error("relposheading_sub"); } #endif + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_timesync_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("timesync_sub"); + } } AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) @@ -428,25 +432,29 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin // hdop from pdop. Some GPS modules don't provide the Aux message interim_state.hdop = interim_state.vdop = msg.pdop * 100.0; } - - if ((msg.timestamp.usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) { + 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)) { // 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(msg.timestamp.usec, (timestamp_usec + NATIVE_TIME_OFFSET)); + interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(gps_message_timestamp_usec, (timestamp_usec + NATIVE_TIME_OFFSET)); interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U; - interim_state.last_corrected_gps_time_us -= msg.timestamp.usec - msg.gnss_timestamp.usec; + interim_state.last_corrected_gps_time_us -= gps_message_timestamp_usec - msg.gnss_timestamp.usec; // this is also the time the message was received on the UART on other end. interim_state.corrected_timestamp_updated = true; } else { - interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, timestamp_usec + NATIVE_TIME_OFFSET)/1000U; + interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(gps_message_timestamp_usec, timestamp_usec + NATIVE_TIME_OFFSET)/1000U; } #if GPS_PPS_EMULATION // Emulates a PPS signal, can be used to check how close are we to real GPS time static virtual_timer_t timeout_vt; - hal.gpio->pinMode(51, 1); - auto handle_timeout = [](void *arg) + hal.gpio->pinMode(51, HAL_GPIO_OUTPUT); + auto handle_timeout = [](virtual_timer_t*,void *) { - (void)arg; //we are called from ISR context chSysLockFromISR(); hal.gpio->toggle(51); @@ -455,7 +463,7 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin static uint64_t next_toggle, last_toggle; - next_toggle = (msg.timestamp.usec) + (1000000ULL - ((msg.timestamp.usec) % 1000000ULL)); + next_toggle = (gps_message_timestamp_usec) + (1000000ULL - ((gps_message_timestamp_usec) % 1000000ULL)); next_toggle += jitter_correction.get_link_offset_usec(); if (next_toggle != last_toggle) { @@ -661,6 +669,21 @@ void AP_GPS_DroneCAN::handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronec } #endif +// precise time synchronisation +void AP_GPS_DroneCAN::handle_timesync_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_protocol_GlobalTimeSync& msg) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); + if (driver != nullptr) { + if ((driver->last_timesync_timestamp_us != 0) && (msg.previous_transmission_timestamp_usec != 0)) { + driver->timesync_correction_us = msg.previous_transmission_timestamp_usec - (driver->last_timesync_timestamp_us + NATIVE_TIME_OFFSET); + } + driver->last_timesync_timestamp_us = transfer.timestamp_usec; + driver->timesync_correction = true; + } +} + bool AP_GPS_DroneCAN::do_config() { AP_DroneCAN *ap_dronecan = _detected_modules[_detected_module].ap_dronecan; diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h index 10f6d71954a3c..9c09b474efe3d 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.h +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h @@ -56,6 +56,8 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { static void handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg); static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg); #endif + static void handle_timesync_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_protocol_GlobalTimeSync& msg); + static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len); void inject_data(const uint8_t *data, uint16_t len) override; @@ -81,6 +83,9 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { }; uint8_t cfg_step; bool requires_save_and_reboot; + bool timesync_correction; + uint64_t last_timesync_timestamp_us; + int64_t timesync_correction_us; // returns true once configuration has finished bool do_config(void); From 07799e6272d085798a8ad6895f0fb5e7a98f5d40 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 8 Dec 2023 16:01:35 +1100 Subject: [PATCH 2/9] AP_GPS: add support for calculating gps message lag over DroneCAN --- libraries/AP_GPS/AP_GPS.cpp | 18 ++++++- libraries/AP_GPS/AP_GPS.h | 5 ++ libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 75 +++++++++++++++++++++------- libraries/AP_GPS/AP_GPS_DroneCAN.h | 16 ++++++ libraries/AP_GPS/GPS_Backend.cpp | 3 ++ libraries/AP_GPS/GPS_Backend.h | 4 ++ libraries/AP_GPS/LogStructure.h | 10 ++-- 7 files changed, 109 insertions(+), 22 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 25a790787a8d4..78081985fb179 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -598,6 +598,13 @@ uint64_t AP_GPS::last_message_epoch_usec(uint8_t instance) const return istate_time_to_epoch_ms(istate.time_week, drivers[instance]->get_last_itow_ms()) * 1000ULL; } +uint64_t AP_GPS::last_pps_time_usec(uint8_t instance) const +{ + if (drivers[instance] == nullptr) { + return 0; + } + return drivers[instance]->get_last_pps_time_us(); +} /* send some more initialisation string bytes if there is room in the UART transmit buffer @@ -2288,11 +2295,16 @@ 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; + int64_t clock_drift = 0; horizontal_accuracy(i, hacc); vertical_accuracy(i, vacc); speed_accuracy(i, sacc); get_undulation(i, undulation); + get_lag(i, lag); + if (drivers[i] != nullptr) { + clock_drift = drivers[i]->get_clock_drift(); + } struct log_GPA pkt2{ LOG_PACKET_HEADER_INIT(LOG_GPA_MSG), time_us : time_us, @@ -2307,7 +2319,9 @@ 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, + clock_drift : clock_drift }; AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index a5ca9cdceaddd..0c475b12cb78b 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -519,6 +519,11 @@ class AP_GPS return last_message_epoch_usec(primary_instance); } + uint64_t last_pps_time_usec(uint8_t instance) const; + uint64_t last_pps_time_usec() const { + return last_pps_time_usec(primary_instance); + } + // convert GPS week and millis to unix epoch in ms static uint64_t istate_time_to_epoch_ms(uint16_t gps_week, uint32_t gps_ms); diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index a7f67649345b4..d7056b523d1d9 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -54,11 +54,6 @@ extern const AP_HAL::HAL& hal; #define LOG_TAG "GPS" -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::micros64()) -#else -#define NATIVE_TIME_OFFSET 0 -#endif AP_GPS_DroneCAN::DetectedModules AP_GPS_DroneCAN::_detected_modules[]; HAL_Semaphore AP_GPS_DroneCAN::_sem_registry; @@ -340,6 +335,18 @@ void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const floa } } +bool AP_GPS_DroneCAN::get_fix_lag(float &lag) const +{ + 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); +} + void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec) { bool process = false; @@ -433,20 +440,25 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin interim_state.hdop = interim_state.vdop = msg.pdop * 100.0; } 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)) { + if (timesync_correction && msg.gnss_timestamp.usec) { + interim_state.last_corrected_gps_time_us = AP_HAL::micros64(); + interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U; + int32_t fix_lag_us = interim_state.last_corrected_gps_time_us - (msg.gnss_timestamp.usec - timesync_correction_us); + Debug("fix_lag[%d]: %ld\n", _detected_modules[_detected_module].node_id, fix_lag_us); + interim_state.corrected_timestamp_updated = true; + if (fix_lag_us > 5000 && fix_lag_us < 250000 ) { + _fix_lag_us = fix_lag_us; + } + last_gnss_timestamp_usec = msg.gnss_timestamp.usec; // for use by update_relposned_lag + } 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 + NATIVE_TIME_OFFSET)); + 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; interim_state.last_corrected_gps_time_us -= gps_message_timestamp_usec - msg.gnss_timestamp.usec; // this is also the time the message was received on the UART on other end. interim_state.corrected_timestamp_updated = true; } else { - interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(gps_message_timestamp_usec, timestamp_usec + NATIVE_TIME_OFFSET)/1000U; + interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(gps_message_timestamp_usec, timestamp_usec)/1000U; } #if GPS_PPS_EMULATION @@ -675,13 +687,42 @@ void AP_GPS_DroneCAN::handle_timesync_msg_trampoline(AP_DroneCAN *ap_dronecan, c WITH_SEMAPHORE(_sem_registry); AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); - if (driver != nullptr) { - if ((driver->last_timesync_timestamp_us != 0) && (msg.previous_transmission_timestamp_usec != 0)) { - driver->timesync_correction_us = msg.previous_transmission_timestamp_usec - (driver->last_timesync_timestamp_us + NATIVE_TIME_OFFSET); + if (driver == nullptr) { + return; + } + bool packet_dropped = false; + // check that we didn't drop a packet, + // if we dropped the packet we can't calculate the timesync correction + // using this packet + if (transfer.transfer_id != ((driver->last_transfer_id+1)%32)) { + packet_dropped = true; + } + if (!packet_dropped && (driver->last_timesync_timestamp_us != 0) && (msg.previous_transmission_timestamp_usec != 0)) { + // It is expected that previous_transmission_timestamp_usec is set using + // 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; + int64_t clock_drift = driver->timesync_correction_us - last_timesync_correction_us; + if (abs(clock_drift) > 10000 && last_timesync_correction_us) { + // if the clock drift is more than 10ms, we might have a correction dropped +#if HAL_LOGGING_ENABLED + AP::logger().Write_MessageF("GPS %d: timesync drop, o:%lld n:%lld", (unsigned int)(driver->state.instance + 1), driver->timesync_correction_us, last_timesync_correction_us); +#endif + driver->timesync_correction_us = last_timesync_correction_us; + } else if (last_timesync_correction_us) { + // add to the clock drift to keep track of drift over time + driver->clock_drift += clock_drift; } - driver->last_timesync_timestamp_us = transfer.timestamp_usec; driver->timesync_correction = true; + Debug("[%u]: correction: %lld/%lld\n", + transfer.source_node_id, + driver->timesync_correction_us, + driver->clock_drift); } + driver->last_timesync_timestamp_us = transfer.timestamp_usec; + driver->last_transfer_id = transfer.transfer_id; } bool AP_GPS_DroneCAN::do_config() diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h index 9c09b474efe3d..50a3a86aefaab 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.h +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h @@ -72,6 +72,10 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { static bool instance_exists(const AP_DroneCAN* ap_dronecan); #endif + int64_t get_clock_drift() override { + return clock_drift; + } + private: bool param_configured = true; @@ -90,6 +94,12 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { // returns true once configuration has finished bool do_config(void); + bool get_lag(float &lag) const override { + return get_fix_lag(lag); + } + + bool get_fix_lag(float &lag) const; + void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec); void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg); void handle_heading_msg(const ardupilot_gnss_Heading& msg); @@ -105,6 +115,9 @@ 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); + uint64_t last_gnss_timestamp_usec; + + uint32_t _fix_lag_us; bool _new_data; AP_GPS::GPS_State interim_state; @@ -154,6 +167,9 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { uint32_t last_send_ms; ByteBuffer *buf; } _rtcm_stream; + + uint16_t last_transfer_id; + int64_t clock_drift; }; #endif // AP_GPS_DRONECAN_ENABLED diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index f6d08fc761705..572d700e128ed 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -326,6 +326,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(float reported_heading_deg, const bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) { constexpr float minimum_antenna_seperation = 0.05; // meters constexpr float permitted_error_length_pct = 0.2; // percentage + #if HAL_LOGGING_ENABLED || AP_AHRS_ENABLED float min_D = 0.0f; float max_D = 0.0f; @@ -402,6 +403,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, goto bad_yaw; } } +#else + (void)lag; #endif // AP_AHRS_ENABLED { diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 2fdae674d9ba4..c54610531aa7f 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -105,6 +105,8 @@ class AP_GPS_Backend virtual bool get_error_codes(uint32_t &error_codes) const { return false; } + virtual int64_t get_clock_drift() { return 0; } + // return iTOW of last message, or zero if not supported uint32_t get_last_itow_ms(void) const; @@ -113,6 +115,8 @@ class AP_GPS_Backend return gps.option_set(option); } + uint64_t get_last_pps_time_us() const { return _last_pps_time_us; } + protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters) diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h index 5ed1d2794693d..112783671ed1f 100644 --- a/libraries/AP_GPS/LogStructure.h +++ b/libraries/AP_GPS/LogStructure.h @@ -64,8 +64,10 @@ struct PACKED log_GPS { // @Field: SMS: time since system startup this sample was taken // @Field: Delta: system time delta between the last two reported positions // @Field: Und: Undulation -// @Field: RTCMFU: RTCM fragments used -// @Field: RTCMFD: RTCM fragments discarded +// @Field: RFU: RTCM fragments used +// @Field: RFD: RTCM fragments discarded +// @Field: lag: GPS lag +// @Field: C: clock drift struct PACKED log_GPA { LOG_PACKET_HEADER; uint64_t time_us; @@ -81,6 +83,8 @@ struct PACKED log_GPA { float undulation; uint16_t rtcm_fragments_used; uint16_t rtcm_fragments_discarded; + float lag; + int64_t clock_drift; }; /* @@ -207,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", "QBCCCCfBIHfHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTCMFU,RTCMFD", "s#-mmnd-ssm--", "F-BBBB0-CC0--" , true }, \ + "GPA", "QBCCCCfBIHfHHfq", "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), \ From d05643e0910795499c1dbc62db81542248902848 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 31 Jan 2024 07:45:43 +1100 Subject: [PATCH 3/9] AP_GPS: fix pps signal handling for ublox modules it was being used to correct the time of message arrival. The signal is actually in line with time the solution is calculated for. The fix is to calculate the lag instead. --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 54 +++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_UBLOX.h | 2 ++ libraries/AP_GPS/GPS_Backend.cpp | 6 +--- 3 files changed, 57 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index f073201373b55..3d72a5536b17c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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 0 + 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; @@ -2128,9 +2153,38 @@ void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const // uBlox specific check_new_itow(), handling message length void AP_GPS_UBLOX::_check_new_itow(uint32_t itow) { +#ifdef HAL_GPIO_PPS + if (_lag_us) { + // we have calculated exact lag so no need to + // jitter correct gps message arrival + return; + } +#endif 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_3D) && + !(_unconfigured_messages & CONFIG_TP5)) { + uint64_t last_message_rx_time = port->receive_time_constraint_us(_payload_length + sizeof(ubx_header) + 2); + _lag_us = uint32_t(last_message_rx_time & 0xFFFFFFFF) - _last_pps_time_us; + Lag_Debug("trx lag: %lu", AP_HAL::micros64() - 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; + // since we have an accurate lag measurement, + // we can set last_corrected_gps_time_us without + // jitter correction + state.last_corrected_gps_time_us = last_message_rx_time; + state.corrected_timestamp_updated = true; + } +} +#endif + // support for retrieving RTCMv3 data from a moving baseline base bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index cbf21f925dddb..d4b6e44ed535d 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/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 diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 572d700e128ed..6d84f4dcf085b 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/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(); From 229a10f86f0303d6deb83a83c4b41783caead3ae Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 18 Oct 2023 18:06:08 +1100 Subject: [PATCH 4/9] AP_HAL: add apis setting and tracking transmit timestamp on CAN --- libraries/AP_HAL/CANIface.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index 1b2e88fb33f23..eed8dc7dd0b87 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -264,10 +264,17 @@ class AP_HAL::CANIface // register a frame callback function virtual bool register_frame_callback(FrameCb cb); + // get timestamp of last packet sent with matching mask and value + virtual uint64_t get_tracked_tx_timestamp() { + return 0; + } + + // set mask and value on packet id to track tx timestamp for + virtual void set_track_tx_timestamp(uint32_t mask, uint32_t value) {} + protected: virtual int8_t get_iface_num() const = 0; virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0; - FrameCb frame_callback; uint32_t bitrate_; OperatingMode mode_; From 038079a538be1a82b6097183d6c3ebd95b3e9156 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 18 Oct 2023 18:06:54 +1100 Subject: [PATCH 5/9] AP_HAL_ChibiOS: add implementation for tracking transmit timestamp on CAN --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 3 +++ libraries/AP_HAL_ChibiOS/CANFDIface.h | 16 ++++++++++++++++ libraries/AP_HAL_ChibiOS/CANIface.h | 16 ++++++++++++++++ libraries/AP_HAL_ChibiOS/CanIface.cpp | 3 +++ 4 files changed, 38 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index cc4001940a4ec..f45676d4cfb79 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -838,6 +838,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) stats.fdf_tx_success++; } pending_tx_[i].pushed = true; + if ((pending_tx_[i].frame.id & tracked_tx_ts_mask) == tracked_tx_ts_value) { + tracked_tx_timestamp_us = timestamp_us; + } } else { continue; } diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index fb6a9719c7fcb..8453be94ae6e1 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -123,6 +123,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface const uint8_t self_index_; + // track tx timestamps + uint32_t tracked_tx_ts_mask; + uint32_t tracked_tx_ts_value; + uint64_t tracked_tx_timestamp_us; + bool computeTimings(uint32_t target_bitrate, Timings& out_timings) const; bool computeFDTimings(uint32_t target_bitrate, Timings& out_timings) const; @@ -253,6 +258,17 @@ class ChibiOS::CANIface : public AP_HAL::CANIface // register. Indexed by locical interface number static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST }; + // set mask and value on packet id to track tx timestamp for + void set_track_tx_timestamp(uint32_t mask, uint32_t value) override { + tracked_tx_ts_mask = mask; + tracked_tx_ts_value = value; + } + + // get timestamp of last packet sent with matching mask and value + uint64_t get_tracked_tx_timestamp() override { + return tracked_tx_timestamp_us; + } + protected: bool add_to_rx_queue(const CanRxItem &rx_item) override { return rx_queue_.push(rx_item); diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index 363c002d491ec..fca0f830a9014 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -112,6 +112,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface const uint8_t self_index_; + // track tx timestamps + uint32_t tracked_tx_ts_mask; + uint32_t tracked_tx_ts_value; + uint64_t tracked_tx_timestamp_us; + bool computeTimings(uint32_t target_bitrate, Timings& out_timings); void setupMessageRam(void); @@ -238,6 +243,17 @@ class ChibiOS::CANIface : public AP_HAL::CANIface // CAN Peripheral register structure static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST }; + // get timestamp of last packet sent with matching mask and value + uint64_t get_tracked_tx_timestamp() override { + return tracked_tx_timestamp_us; + } + + // set mask and value on packet id to track tx timestamp for + void set_track_tx_timestamp(uint32_t mask, uint32_t value) override { + tracked_tx_ts_mask = mask; + tracked_tx_ts_value = value; + } + protected: bool add_to_rx_queue(const CanRxItem &rx_item) override { return rx_queue_.push(rx_item); diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index f6704b6438d09..75a28efcdea51 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -500,6 +500,9 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const #if !defined(HAL_BOOTLOADER_BUILD) stats.last_transmit_us = timestamp_us; #endif + if ((txi.frame.id & tracked_tx_ts_mask) == tracked_tx_ts_value) { + tracked_tx_timestamp_us = timestamp_us; + } } } From 1898e980a3bd4b9c1b699eea1ef8f9d758c82193 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 31 Jan 2024 12:23:59 +1100 Subject: [PATCH 6/9] AP_Periph: add support for Global Time Sync --- Tools/AP_Periph/AP_Periph.h | 3 +++ Tools/AP_Periph/can.cpp | 11 +++++++++++ Tools/AP_Periph/gps.cpp | 35 +++++++++++++++++++++++++++++++++++ 3 files changed, 49 insertions(+) diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 424e341c1c8c1..071d04350ba81 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -425,6 +425,9 @@ class AP_Periph_FW { #ifdef HAL_PERIPH_ENABLE_GPS uint32_t last_gps_update_ms; uint32_t last_gps_yaw_ms; +#ifdef HAL_PERIPH_ENABLE_GLOBALTIMESYNC + uint64_t get_tracked_tx_timestamp(uint8_t i); +#endif #endif uint32_t last_relposheading_ms; #ifdef HAL_PERIPH_ENABLE_BARO diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index c985403fd282d..4f8b418bc1bcf 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1627,6 +1627,7 @@ void AP_Periph_FW::can_start() #else can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode); #endif + can_iface_periph[i]->set_track_tx_timestamp((0xFFFFLU << 8), ((uint32_t)UAVCAN_PROTOCOL_GLOBALTIMESYNC_ID)<<8); } } @@ -1742,6 +1743,16 @@ void AP_Periph_FW::apd_esc_telem_update() #endif // HAL_PERIPH_ENABLE_ESC_APD #endif // HAL_PERIPH_ENABLE_RC_OUT +#if defined(HAL_PERIPH_ENABLE_GLOBALTIMESYNC) && defined(HAL_PERIPH_ENABLE_GPS) +uint64_t AP_Periph_FW::get_tracked_tx_timestamp(uint8_t i) +{ + if (can_iface_periph[i]) { + return can_iface_periph[i]->get_tracked_tx_timestamp(); + } + return 0; +} +#endif + void AP_Periph_FW::can_update() { const uint32_t now = AP_HAL::millis(); diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp index 029676f26d1b8..d0dc935feed30 100644 --- a/Tools/AP_Periph/gps.cpp +++ b/Tools/AP_Periph/gps.cpp @@ -65,7 +65,13 @@ void AP_Periph_FW::can_gps_update(void) if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) { return; } + +#ifdef HAL_PERIPH_ENABLE_GLOBALTIMESYNC + // we need to record this time as its reset when we call gps.update() + uint64_t last_message_local_time_us = periph.gps.last_pps_time_usec(); +#endif gps.update(); + send_moving_baseline_msg(); send_relposheading_msg(); if (last_gps_update_ms == gps.last_message_time_ms()) { @@ -73,6 +79,35 @@ void AP_Periph_FW::can_gps_update(void) } last_gps_update_ms = gps.last_message_time_ms(); +#ifdef HAL_PERIPH_ENABLE_GLOBALTIMESYNC + // send time sync message every second + uavcan_protocol_GlobalTimeSync ts {}; + for (uint8_t i=0; i Date: Wed, 31 Jan 2024 12:24:35 +1100 Subject: [PATCH 7/9] AP_HAL_ChibiOS: enable global time sync on Here4AP --- libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat index d91a47cd1ee4a..e3dfd4041cd40 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat @@ -122,9 +122,9 @@ define GPS_MOVING_BASELINE 1 define HAL_PERIPH_GPS_PORT_DEFAULT 3 #PD11 GPS_TP1 OUTPUT HIGH GPIO(5) -#PD7 GPS_PPS INPUT FLOATING GPIO(6) -#define CONFIGURE_PPS_PIN TRUE -#define HAL_GPIO_PPS 6 +PD7 GPS_PPS INPUT FLOATING GPIO(6) +define CONFIGURE_PPS_PIN TRUE +define HAL_GPIO_PPS 6 # GPS enable pin # PD13 GPS_ENABLE OUTPUT HIGH GPIO(20) @@ -132,3 +132,5 @@ define HAL_PERIPH_GPS_PORT_DEFAULT 3 # for ProfiLed we need RC out and notify define HAL_PERIPH_ENABLE_RC_OUT define HAL_PERIPH_ENABLE_NOTIFY + +define HAL_PERIPH_ENABLE_GLOBALTIMESYNC 1 From 3ab642276e64fc038f92717ef12cbdb6d4bface9 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 31 Jan 2024 07:53:51 +1100 Subject: [PATCH 8/9] AP_HAL_ChibiOS: add support for inducing clock drift --- libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c | 10 ++++++++++ libraries/AP_HAL_ChibiOS/system.cpp | 13 ++++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c index f1e510ccebd07..e2881f68727e1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c @@ -73,6 +73,12 @@ static uint32_t system_time_u32_us(void) static uint32_t get_systime_us32(void) { 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; #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 + // induce clock drift + ret += (ret * AP_INDUCE_PPM_OFFSET) / 1000000U; +#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 index 09cb666821500..b63d833eba5b8 100644 --- a/libraries/AP_HAL_ChibiOS/system.cpp +++ b/libraries/AP_HAL_ChibiOS/system.cpp @@ -353,7 +353,18 @@ __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 +#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 return st_lld_get_counter(); From 1801bdd77968104687845c826db08cc187e3aaf3 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 5 Feb 2024 16:02:24 +1100 Subject: [PATCH 9/9] AP_NavEKF3: add logging of time delta between GPS sample timestamps --- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 7 +++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 3 ++- libraries/AP_NavEKF3/LogStructure.h | 4 +++- 4 files changed, 14 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 1f60ae65f932c..ce7e42d83ab59 100644 --- 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 index 9ebe777515a84..996d012d2dd72 100644 --- 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 index 8ba964b7e88fd..9be28bfac3677 100644 --- 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 index 8e124462fcf0d..b77b2f8ef7953 100644 --- 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), \