feat(drivers/uavcan): use GNSS PVT timestamp for EKF time alignment

Previously the DroneCAN GNSS driver left sensor_gps.timestamp_sample at 0,
which caused VehicleGPSPosition to fall back to timestamp - SENS_GPS_DELAY
(a fixed 110 ms guess). The actual PVT solution timestamp from Fix2.gnss_timestamp
was only consumed for system clock setting and the PPS path.

Map the UTC PVT timestamp to HRT directly using an asymmetric filter on
(UTC_PVT - HRT_recv): snap up to a new max immediately (tracks low-latency
observations and growing UTC-HRT delta), and decay slowly between observations
at 100 us/s (~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 the
HRT-faster-than-UTC direction would accumulate unbounded bias.

Clamp the resulting timestamp_sample to strictly less than timestamp so that
VehicleGPSPosition does not mistake a snap-up event for an unset value.

PpsTimeSync still overrides this when PPS hardware is wired.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

Author: dakejahl

src/drivers/uavcan/sensors/gnss.cpp

@@ -505,6 +505,46 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 		break;
 	}
 
+	// Map the GNSS PVT UTC timestamp to HRT so the EKF can align GPS with IMU. cand_offset =
+	// UTC_PVT - HRT_recv = delta - latency. Snap up immediately to track lower latency or
+	// growing delta (HRT slower than UTC); decay slowly so the offset can also follow a
+	// shrinking delta (HRT faster than UTC) without accumulating bias over long flights.
+	// PpsTimeSync in VehicleGPSPosition overrides this when PPS hardware is wired.
+	if (sensor_gps.time_utc_usec != 0) {
+		const int64_t cand_offset = (int64_t)sensor_gps.time_utc_usec - (int64_t)sensor_gps.timestamp;
+
+		if (!_utc_minus_hrt_offset_valid) {
+			_utc_minus_hrt_offset_us = cand_offset;
+			_utc_minus_hrt_offset_valid = true;
+
+		} else {
+			const hrt_abstime dt_us = (sensor_gps.timestamp > _utc_minus_hrt_offset_last_update)
+						  ? (sensor_gps.timestamp - _utc_minus_hrt_offset_last_update) : 0;
+			const int64_t decay_us = static_cast<int64_t>(dt_us) * kMaxClockDriftPpm / 1'000'000;
+			_utc_minus_hrt_offset_us -= decay_us;
+
+			if (cand_offset > _utc_minus_hrt_offset_us) {
+				_utc_minus_hrt_offset_us = cand_offset;
+			}
+		}
+
+		_utc_minus_hrt_offset_last_update = sensor_gps.timestamp;
+
+		int64_t pvt_hrt_us = (int64_t)sensor_gps.time_utc_usec - _utc_minus_hrt_offset_us;
+
+		// On snap-up events pvt_hrt_us == timestamp, which VehicleGPSPosition mistakes for
+		// "driver didn't set timestamp_sample" and replaces with SENS_GPS_DELAY (110 ms back).
+		// That intermittent backward jump trips the EKF monotonicity gate and drops samples.
+		// Bias by 1 us so the value is always strictly less than timestamp.
+		if (pvt_hrt_us >= (int64_t)sensor_gps.timestamp) {
+			pvt_hrt_us = (int64_t)sensor_gps.timestamp - 1;
+		}
+
+		if (pvt_hrt_us > 0) {
+			sensor_gps.timestamp_sample = (uint64_t)pvt_hrt_us;
+		}
+	}
+
 	// If we haven't already done so, set the system clock using GPS data
 	if (sensor_gps.time_utc_usec != 0 && (fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
 		timespec ts{};

src/drivers/uavcan/sensors/gnss.hpp

@@ -151,6 +151,14 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase
 
 	bool _system_clock_set{false};  ///< Have we set the system clock at least once from GNSS data?
 
+	// Asymmetric filter on (UTC_PVT - HRT_recv) used to map GNSS PVT UTC to HRT for EKF
+	// alignment. Snap up to a new max (low-latency or UTC>HRT drift), decay slowly between
+	// observations at the worst-case clock-drift rate so the offset can also track UTC<HRT.
+	static constexpr int64_t kMaxClockDriftPpm{100};   // ~100 us/s; covers cheap-crystal drift
+	int64_t     _utc_minus_hrt_offset_us{0};
+	hrt_abstime _utc_minus_hrt_offset_last_update{0};
+	bool        _utc_minus_hrt_offset_valid{false};
+
 	bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg
 
 	bool _publish_rtcm_stream{false};