feat(sensors/gps): map PVT UTC to HRT for EKF time alignment#27387
Draft
dakejahl wants to merge 1 commit into
Draft
feat(sensors/gps): map PVT UTC to HRT for EKF time alignment#27387dakejahl wants to merge 1 commit into
dakejahl wants to merge 1 commit into
Conversation
github-actions
Bot
added
kind:feature
Contributor
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 304 byte (0.02 %)]px4_fmu-v6x [Total VM Diff: 304 byte (0.02 %)]Updated: 2026-05-19T03:07:30 |
Replaces the constant-latency SENS_GPS*_DELAY guess (110 ms) with an asymmetric leaky-max filter on (UTC_PVT - HRT_recv) per receiver, reducing residual GPS-vs-IMU time-alignment bias from ~110 ms to the minimum observed latency (a few ms on typical CAN or serial GPS). For each cand = UTC_PVT - HRT_recv = offset - latency (latency >= 0), the true offset is the supremum of cand. The filter snaps up to a new max immediately (tracks low-latency observations and growing UTC-HRT delta) and decays slowly between observations at 100 ppm so the offset can also track a shrinking delta when HRT runs faster than UTC. Without the decay, long-flight clock drift in that direction would accumulate unbounded bias. UtcToHrtMapper lives in VehicleGPSPosition so every sensor_gps source benefits, not just DroneCAN. Per-receiver state prevents one receiver's latency from biasing another's. Hard reset on a backward cand jump larger than 100 ms (leap-second insertion, receiver recovery). Strict cross-sample monotonicity clamp on the emitted timestamp so snap-ups during the convergence transient do not get rejected by the EKF GPS buffer's _min_obs_interval_us gate. Falls through to the SENS_GPS_DELAY path when UTC is unavailable (e.g., before first fix). PpsTimeSync still overrides post-blending when PPS hardware is wired. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
dakejahl
force-pushed
the
feat/uavcan-gnss-pvt-timestamp
branch
from
67db04c to
b997fd6
Compare
github-actions
Bot
added
risk:safety-critical
dakejahl
changed the title
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Map the GNSS PVT UTC timestamp to the FC's HRT timebase in
VehicleGPSPositionwith an asymmetric leaky-max filter on(UTC_PVT - HRT_recv), so EKF GPS samples align with IMU samples to within minimum observed latency (a few ms) instead of the constant-latencySENS_GPS*_DELAY110 ms guess.Problem
VehicleGPSPositionfalls back totimestamp - SENS_GPS_DELAY(default 110 ms) when the driver leavestimestamp_sampleunset. The DroneCAN GNSS driver parsesFix2.gnss_timestampintosensor_gps.time_utc_usecbut only uses it for setting the system clock and the PPS path — the precise PVT instant is discarded for EKF fusion. On common no-PPS DroneCAN setups the EKF therefore aligns GPS against IMU with a fixed-latency assumption that ignores actual receiver-internal processing plus CAN-bus latency (which varies with bus load). Serial GPS drivers that populatetime_utc_usechit the same fallback.Solution
New
UtcToHrtMapperhelper insensors/vehicle_gps_positionruns above theSENS_GPS_DELAYfallback. For eachsensor_gpsmessage with validtime_utc_usec, it estimatesoffset := UTC - HRTfromas the supremum of
candover recent observations — one-sided noise (latency cannot be negative) makes max-of-candthe right estimator, the same technique used by NTP/PTP min-RTT filters. Implemented as an asymmetric leaky-max: snap up immediately on lower-latency observations orUTC > HRTdrift, decay at 100 ppm between observations so the offset can also trackUTC < HRTdrift over long flights. Residualtimestamp_samplebias is approximatelymin(latency)— three orders of magnitude better than the 110 ms fallback.Lives in
VehicleGPSPositionso everysensor_gpssource benefits, not just DroneCAN. Per-receiver state (_utc_to_hrt_mapper[GPS_MAX_RECEIVERS]) so different bus/cable latencies between receivers do not bias each other. Hard reset on a backwardcandjump > 100 ms (leap-second insertion, receiver recovery) — bounded re-bootstrap instead of hours-of-decay recovery. Strict cross-sample monotonicity clamp on the emitted timestamp so snap-ups during the convergence transient do not get rejected by the EKF GPS-buffer's_min_obs_interval_usgate.Falls through to
SENS_GPS_DELAYwhen UTC is unavailable (e.g., before first fix).PpsTimeSyncstill overrides post-blending when PPS hardware is wired; this only improves the no-PPS path.Tested by building
px4_fmu-v6x_default,px4_sitl, and runninggps_blendingunit tests. Needs flight-log validation on hardware with a DroneCAN GNSS before un-drafting.