feat(sensors/gps): map PVT UTC to HRT for EKF time alignment

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>

Author: dakejahl

src/modules/sensors/vehicle_gps_position/CMakeLists.txt

@@ -38,6 +38,8 @@ px4_add_library(vehicle_gps_position
 	gps_blending.hpp
 	PpsTimeSync.cpp
 	PpsTimeSync.hpp
+	UtcToHrtMapper.cpp
+	UtcToHrtMapper.hpp
 )
 target_link_libraries(vehicle_gps_position
 	PRIVATE

src/modules/sensors/vehicle_gps_position/UtcToHrtMapper.cpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "UtcToHrtMapper.hpp"
+
+hrt_abstime UtcToHrtMapper::map(uint64_t utc_us, hrt_abstime hrt_recv_us)
+{
+	if (utc_us == 0) {
+		return 0;
+	}
+
+	const int64_t cand_offset = static_cast<int64_t>(utc_us) - static_cast<int64_t>(hrt_recv_us);
+
+	// Hard reset on a UTC discontinuity: a backward jump in cand larger than
+	// any plausible latency spike means the previous offset estimate is stale.
+	if (_valid && cand_offset < _offset_us - kUtcDiscontinuityResetUs) {
+		_valid = false;
+		_last_emitted_us = 0;
+	}
+
+	if (!_valid) {
+		_offset_us = cand_offset;
+		_valid = true;
+
+	} else {
+		const hrt_abstime dt_us = (hrt_recv_us > _last_update_us) ? (hrt_recv_us - _last_update_us) : 0;
+		const int64_t decay_us = static_cast<int64_t>(dt_us) * kMaxClockDriftPpm / 1'000'000;
+		_offset_us -= decay_us;
+
+		if (cand_offset > _offset_us) {
+			_offset_us = cand_offset;
+		}
+	}
+
+	_last_update_us = hrt_recv_us;
+
+	int64_t pvt_hrt_us = static_cast<int64_t>(utc_us) - _offset_us;
+
+	// Strict-less-than hrt_recv_us: equality is misread upstream as
+	// "driver didn't set timestamp_sample" and replaced with SENS_GPS_DELAY.
+	if (pvt_hrt_us >= static_cast<int64_t>(hrt_recv_us)) {
+		pvt_hrt_us = static_cast<int64_t>(hrt_recv_us) - 1;
+	}
+
+	// Strict-greater-than _last_emitted_us: snap-ups during the convergence
+	// transient can produce a backward step that the EKF GPS buffer rejects.
+	if (_last_emitted_us > 0 && pvt_hrt_us <= static_cast<int64_t>(_last_emitted_us)) {
+		pvt_hrt_us = static_cast<int64_t>(_last_emitted_us) + 1;
+	}
+
+	// Both clamps applied; bail out if the window (_last_emitted_us, hrt_recv_us)
+	// was too narrow to satisfy them.
+	if (pvt_hrt_us <= 0 || pvt_hrt_us >= static_cast<int64_t>(hrt_recv_us)) {
+		return 0;
+	}
+
+	_last_emitted_us = static_cast<hrt_abstime>(pvt_hrt_us);
+	return _last_emitted_us;
+}

src/modules/sensors/vehicle_gps_position/UtcToHrtMapper.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <drivers/drv_hrt.h>
+
+/**
+ * Maps a GNSS PVT UTC timestamp to the FC's HRT timebase so that the EKF can
+ * align GPS samples with IMU samples without relying on the SENS_GPS*_DELAY
+ * constant-latency guess.
+ *
+ * Each observation gives cand = UTC_PVT - HRT_recv = (UTC - HRT) - latency,
+ * where latency >= 0. The true offset (UTC - HRT) is therefore the supremum
+ * of cand over recent observations. Estimated by an asymmetric leaky-max:
+ *
+ *   - snap up whenever cand > current_offset (tracks lower latency and any
+ *     UTC-faster-than-HRT drift),
+ *   - decay slowly between observations at the worst-case FC crystal drift
+ *     rate so the offset can also follow UTC-slower-than-HRT drift without
+ *     accumulating bias over long flights.
+ *
+ * Residual bias of the mapped timestamp is approximately min(latency) over
+ * the recent window -- a few ms at most on typical CAN/serial GPS, three
+ * orders of magnitude better than the 110 ms SENS_GPS_DELAY fallback.
+ *
+ * Output is clamped to be strictly less than hrt_recv_us (so downstream
+ * "did the driver set it" checks don't misfire) and strictly greater than
+ * the previously emitted value (so the EKF GPS-buffer push doesn't reject
+ * non-monotonic samples during the convergence transient).
+ */
+class UtcToHrtMapper
+{
+public:
+	UtcToHrtMapper() = default;
+	~UtcToHrtMapper() = default;
+
+	/**
+	 * @param utc_us       PVT UTC timestamp [us since Unix epoch]; 0 if unavailable.
+	 * @param hrt_recv_us  HRT timestamp captured when the GNSS message was received.
+	 * @return mapped HRT timestamp, or 0 if mapping not available for this sample.
+	 */
+	hrt_abstime map(uint64_t utc_us, hrt_abstime hrt_recv_us);
+
+private:
+	// Bound on FC HSE crystal drift vs GNSS atomic time. 100 ppm safely envelopes
+	// raw crystals over temperature; TCXOs are at most a few ppm.
+	static constexpr int64_t kMaxClockDriftPpm{100};
+
+	// A negative jump in cand larger than this can't be FC drift -- treat as a
+	// UTC discontinuity (leap-second insertion, receiver recovery) and reset.
+	static constexpr int64_t kUtcDiscontinuityResetUs{100'000};
+
+	int64_t     _offset_us{0};       // estimate of UTC - HRT
+	hrt_abstime _last_update_us{0};
+	hrt_abstime _last_emitted_us{0};
+	bool        _valid{false};
+};

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -163,7 +163,18 @@ void VehicleGPSPosition::Run()
 				delay_us = _gps_param_slots[i].delay_us;
 			}
 
-			// Apply delay to timestamp_sample if the driver didn't set one
+			// Prefer PVT-UTC-to-HRT mapping when the driver didn't set timestamp_sample but
+			// did provide a valid UTC PVT time. Residual bias is ~min(latency) (a few ms),
+			// vs the ~110 ms SENS_GPS_DELAY fallback below.
+			if (gps_data.timestamp_sample == 0 || gps_data.timestamp_sample == gps_data.timestamp) {
+				const hrt_abstime mapped = _utc_to_hrt_mapper[i].map(gps_data.time_utc_usec, gps_data.timestamp);
+
+				if (mapped > 0) {
+					gps_data.timestamp_sample = mapped;
+				}
+			}
+
+			// Fallback for drivers that don't provide UTC (e.g., no fix yet).
 			if (gps_data.timestamp_sample == 0 || gps_data.timestamp_sample == gps_data.timestamp) {
 				if (delay_us > 0 && gps_data.timestamp > delay_us) {
 					gps_data.timestamp_sample = gps_data.timestamp - delay_us;

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -49,6 +49,7 @@
 
 #include "gps_blending.hpp"
 #include "PpsTimeSync.hpp"
+#include "UtcToHrtMapper.hpp"
 
 using namespace time_literals;
 
@@ -96,6 +97,7 @@ class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem
 
 	GpsBlending _gps_blending;
 	PpsTimeSync _pps_time_sync;
+	UtcToHrtMapper _utc_to_hrt_mapper[GPS_MAX_RECEIVERS];
 
 	struct GpsParamSlot {
 		uint32_t device_id{0};