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>

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -159,7 +159,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS)
-void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
+void EstimatorInterface::setGpsData(const gnssSample &gnss_sample, const bool pps_compensation)
 {
 	if (!_initialised) {
 		return;
@@ -177,8 +177,10 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 		}
 	}
 
+	const int64_t delay = pps_compensation ? 0 : static_cast<int64_t>(_params.ekf2_gps_delay * 1000);
+
 	const int64_t time_us = gnss_sample.time_us
-				- static_cast<int64_t>(_params.ekf2_gps_delay * 1000)
+				- delay
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -89,7 +89,7 @@ public:
 	void setIMUData(const imuSample &imu_sample);
 
 #if defined(CONFIG_EKF2_GNSS)
-	void setGpsData(const gnssSample &gnss_sample);
+	void setGpsData(const gnssSample &gnss_sample, const bool pps_compensation = false);
 
 	const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
 

src/modules/ekf2/EKF2.cpp

@@ -2452,8 +2452,12 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 		const float altitude_amsl = static_cast<float>(vehicle_gps_position.altitude_msl_m);
 		const float altitude_ellipsoid = static_cast<float>(vehicle_gps_position.altitude_ellipsoid_m);
 
+		// if pps_compensation is active but not valid, the timestamp_sample will be equal to timestamp
+		const bool pps_compensation = vehicle_gps_position.timestamp_sample > 0
+					      && vehicle_gps_position.timestamp_sample != vehicle_gps_position.timestamp;
+
 		gnssSample gnss_sample{
-			.time_us = vehicle_gps_position.timestamp,
+			.time_us = pps_compensation ? vehicle_gps_position.timestamp_sample : vehicle_gps_position.timestamp,
 			.lat = vehicle_gps_position.latitude_deg,
 			.lon = vehicle_gps_position.longitude_deg,
 			.alt = altitude_amsl,
@@ -2471,7 +2475,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			.spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED,
 		};
 
-		_ekf.setGpsData(gnss_sample);
+		_ekf.setGpsData(gnss_sample, pps_compensation);
 
 		const float geoid_height = altitude_ellipsoid - altitude_amsl;
 

src/modules/ekf2/params_gnss.yaml

@@ -29,7 +29,9 @@ parameters:
       default: 0
     EKF2_GPS_DELAY:
       description:
-        short: GPS measurement delay relative to IMU measurements
+        short: GPS measurement delay relative to IMU measurement
+        long: GPS measurement delay relative to IMU measurement if PPS time correction
+          is not available/enabled (PPS_CAP_ENABLE).
       type: float
       default: 110
       min: 0

src/modules/sensors/vehicle_gps_position/CMakeLists.txt

@@ -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

src/modules/sensors/vehicle_gps_position/PpsTimeSync.cpp

@@ -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;
+}

src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp

@@ -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};
+};

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -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);
 		}
 	}

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -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,