diff --git a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt index c82a6361187b..e77774962934 100644 --- a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt +++ b/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 diff --git a/src/modules/sensors/vehicle_gps_position/UtcToHrtMapper.cpp b/src/modules/sensors/vehicle_gps_position/UtcToHrtMapper.cpp new file mode 100644 index 000000000000..c8c84af206b8 --- /dev/null +++ b/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(utc_us) - static_cast(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(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(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(hrt_recv_us)) { + pvt_hrt_us = static_cast(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(_last_emitted_us)) { + pvt_hrt_us = static_cast(_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(hrt_recv_us)) { + return 0; + } + + _last_emitted_us = static_cast(pvt_hrt_us); + return _last_emitted_us; +} diff --git a/src/modules/sensors/vehicle_gps_position/UtcToHrtMapper.hpp b/src/modules/sensors/vehicle_gps_position/UtcToHrtMapper.hpp new file mode 100644 index 000000000000..587937984f29 --- /dev/null +++ b/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 +#include + +/** + * 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}; +}; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 306451984777..6152155b4638 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/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; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index bf3cc6635f85..9fa65bf6b2a6 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/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};