Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: add support for correcting GPS times while using DroneCAN GPS

Browse Source
This commit is contained in:
bugobliterator 2022-01-18 16:13:42 +05:30 committed by Andrew Tridgell
parent 49bf4fa27b
commit 49b767b24e
2 changed files with 42 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -39,6 +39,8 @@
#include <AP_BoardConfig/AP_BoardConfig.h>
#define GPS_PPS_EMULATION 0
extern const AP_HAL::HAL& hal;
#define GPS_UAVCAN_DEBUGGING 0
@ -68,6 +70,11 @@ UC_REGISTRY_BINDER(MovingBaselineDataCb, ardupilot::gnss::MovingBaselineData);
UC_REGISTRY_BINDER(RelPosHeadingCb, ardupilot::gnss::RelPosHeading);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::native_micros64())
#else
#define NATIVE_TIME_OFFSET 0
#endif
AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
HAL_Semaphore AP_GPS_UAVCAN::_sem_registry;
@ -578,8 +585,41 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
// hdop from pdop. Some GPS modules don't provide the Aux message
interim_state.hdop = interim_state.vdop = cb.msg->pdop * 100.0;
}
if ((cb.msg->timestamp.usec > cb.msg->gnss_timestamp.usec) && (cb.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(cb.msg->timestamp.usec, (cb.msg->getUtcTimestamp().toUSec() + NATIVE_TIME_OFFSET));
interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
interim_state.last_corrected_gps_time_us -= cb.msg->timestamp.usec - cb.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(cb.msg->timestamp.usec, cb.msg->getUtcTimestamp().toUSec() + 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)
{
(void)arg;
//we are called from ISR context
chSysLockFromISR();
hal.gpio->toggle(51);
chSysUnlockFromISR();
};
static uint64_t next_toggle, last_toggle;
interim_state.last_gps_time_ms = AP_HAL::millis();
next_toggle = (cb.msg->timestamp.usec) + (1000000ULL - ((cb.msg->timestamp.usec) % 1000000ULL));
next_toggle += jitter_correction.get_link_offset_usec();
if (next_toggle != last_toggle) {
chVTSet(&timeout_vt, chTimeUS2I(next_toggle - AP_HAL::micros64()), handle_timeout, nullptr);
last_toggle = next_toggle;
}
#endif
_new_data = true;
if (!seen_message) {

4
libraries/AP_GPS/GPS_Backend.h
View File

@ -157,6 +157,7 @@ protected:
protected:
uint64_t _last_pps_time_us;
JitterCorrection jitter_correction;
private:
// itow from previous message
@ -167,9 +168,6 @@ private:
uint32_t _rate_ms;
uint32_t _last_rate_ms;
uint16_t _rate_counter;
JitterCorrection jitter_correction;
#if AP_GPS_DEBUG_LOGGING_ENABLED
struct {
int fd = -1;