PPS time corection capability for GNSS measurements (#25838)

** add PPS time corection capability for GNSS measurements * * add documentation * add comment for 'future check' * replace url with relative link * Update docs/en/advanced/pps_time_sync.md Co-authored-by: Silvan Fuhrer <silvan@auterion.com> * Subedit and add to sidebar * Apply suggestions from code review * Update docs/en/SUMMARY.md * remove offset jump check, clean up * add comment pps_compensation activation condition. move documentation to hardware integration --------- Co-authored-by: Hamish Willee <hamishwillee@gmail.com> Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
2026-10-20 04:33:35 +08:00 · 2025-11-11 20:56:56 +01:00
parent 98d3a2141a
commit 5eab16c17c
12 changed files with 335 additions and 6 deletions
@@ -36,6 +36,8 @@ px4_add_library(vehicle_gps_position
 	VehicleGPSPosition.hpp
 	gps_blending.cpp
 	gps_blending.hpp
+	PpsTimeSync.cpp
+	PpsTimeSync.hpp
 )
 target_link_libraries(vehicle_gps_position
 	PRIVATE
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PpsTimeSync.cpp
+ *
+ * PPS-based time synchronization implementation
+ */
+
+#include "PpsTimeSync.hpp"
+#include <px4_platform_common/log.h>
+#include <mathlib/mathlib.h>
+
+void PpsTimeSync::process_pps(const pps_capture_s &pps)
+{
+	if (pps.timestamp == 0 || pps.rtc_timestamp == 0) {
+		return;
+	}
+
+	_pps_hrt_timestamp = pps.timestamp;
+	_pps_rtc_timestamp = pps.rtc_timestamp;
+	_time_offset = (int64_t)pps.rtc_timestamp - (int64_t)pps.timestamp;
+	_initialized = true;
+	_updated = true;
+}
+
+uint64_t PpsTimeSync::correct_gps_timestamp(uint64_t gps_fc_timestamp, uint64_t gps_utc_timestamp)
+{
+	if (!is_valid()) {
+		return gps_fc_timestamp;
+	}
+
+	const int64_t corrected_fc_timestamp = (int64_t)gps_utc_timestamp - _time_offset;
+
+	if (_updated) {
+		const int64_t correction_amount = corrected_fc_timestamp - (int64_t)gps_fc_timestamp;
+
+		if (math::abs_t(correction_amount) > kPpsMaxCorrectionUs) {
+			PX4_DEBUG("PPS: Correction too large: %" PRId64 " us (%.1f ms), rejecting",
+				  correction_amount, (double)correction_amount / 1000.0);
+			return gps_fc_timestamp;
+		}
+
+		// Additional sanity check: corrected timestamp should not be too far in the future (0.1s)
+		const uint64_t now = hrt_absolute_time();
+
+		if ((uint64_t)corrected_fc_timestamp > now + 100000) {
+			return gps_fc_timestamp;
+		}
+
+		_updated = false;
+	}
+
+	return (uint64_t)corrected_fc_timestamp;
+}
+
+bool PpsTimeSync::is_valid() const
+{
+	if (!_initialized) {
+		return false;
+	}
+
+	uint64_t now = hrt_absolute_time();
+
+	if (now < _pps_hrt_timestamp) {
+		now = UINT64_MAX;
+	}
+
+	if (now - _pps_hrt_timestamp > kPpsStaleTimeoutUs) {
+		return false;
+	}
+
+	return true;
+}
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PpsTimeSync.hpp
+ *
+ * PPS-based time synchronization for GPS timestamp correction
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <uORB/topics/pps_capture.h>
+#include <drivers/drv_hrt.h>
+
+class PpsTimeSync
+{
+public:
+	PpsTimeSync() = default;
+	~PpsTimeSync() = default;
+
+	void process_pps(const pps_capture_s &pps);
+	uint64_t correct_gps_timestamp(uint64_t gps_fc_timestamp, uint64_t gps_utc_timestamp);
+
+private:
+	bool is_valid() const;
+
+	uint64_t _pps_hrt_timestamp{0};	// FC time when PPS pulse arrived (usec since boot)
+	uint64_t _pps_rtc_timestamp{0};	// GPS UTC time at PPS pulse (usec since Unix epoch)
+	int64_t _time_offset{0};
+
+	static constexpr uint64_t kPpsStaleTimeoutUs = 5'000'000;
+	static constexpr int64_t kPpsMaxCorrectionUs = 300'000;	// max delay (max of EKF2_GPS_DELAY)
+
+	bool _initialized{false};
+	bool _updated{false};
+};
@@ -104,6 +104,12 @@ void VehicleGPSPosition::Run()
 	perf_begin(_cycle_perf);
 	ParametersUpdate();

+	pps_capture_s pps_capture;
+
+	if (_pps_capture_sub.update(&pps_capture)) {
+		_pps_time_sync.process_pps(pps_capture);
+	}
+
 	// Check all GPS instance
 	bool any_gps_updated = false;
 	bool gps_updated = false;
@@ -136,6 +142,7 @@ void VehicleGPSPosition::Run()
 				gps_output.device_id = 0;
 			}

+			gps_output.timestamp_sample = _pps_time_sync.correct_gps_timestamp(gps_output.timestamp, gps_output.time_utc_usec);
 			_vehicle_gps_position_pub.publish(gps_output);
 		}
 	}
@@ -45,8 +45,10 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/pps_capture.h>

 #include "gps_blending.hpp"
+#include "PpsTimeSync.hpp"

 using namespace time_literals;

@@ -88,9 +90,12 @@ private:
 		{this, ORB_ID(sensor_gps), 1},
 	};

+	uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)};
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};

 	GpsBlending _gps_blending;
+	PpsTimeSync _pps_time_sync;

 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,