Fix PPS based UTC timestamp in camera trigger and capture messages

The existing implementation has about 100ms difference to a reference clock. With the changes this error less than 25us.
- Use sensor_gps messages with hrt timestamps as RTC reference and not the system realtime clock. The PPS interrupt can then be aligned with the GPS clock system.
- Keep fallback based on system RTC when no PPS pulse was captured

msg/pps_capture.msg

@@ -1,3 +1,2 @@
-uint64 timestamp			# time since system start (microseconds)
-uint64 rtc_timestamp		# captured time in microseconds from real-time clock at PPS event
-int32 rtc_drift_time		# time drift between RTC and PPS event in microseconds
+uint64 timestamp			  # time since system start (microseconds) at PPS capture event
+uint64 rtc_timestamp		# Corrected GPS UTC timestamp at PPS capture event

src/drivers/camera_capture/camera_capture.cpp

@@ -106,11 +106,6 @@ CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint
 {
 	_trigger.chan_index = chan_index;
 	_trigger.hrt_edge_time = edge_time;
-
-	timespec tv{};
-	px4_clock_gettime(CLOCK_REALTIME, &tv);
-	_trigger.rtc_edge_time = ts_to_abstime(&tv) - hrt_elapsed_time(&edge_time);
-
 	_trigger.edge_state = edge_state;
 	_trigger.overflow = overflow;
 
@@ -124,11 +119,6 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
 
 	dev->_trigger.chan_index = 0;
 	dev->_trigger.hrt_edge_time = hrt_absolute_time();
-
-	timespec tv{};
-	px4_clock_gettime(CLOCK_REALTIME, &tv);
-	dev->_trigger.rtc_edge_time = ts_to_abstime(&tv);
-
 	dev->_trigger.edge_state = 0;
 	dev->_trigger.overflow = 0;
 
@@ -197,10 +187,23 @@ CameraCapture::publish_trigger()
 	pps_capture_s pps_capture;
 
 	if (_pps_capture_sub.update(&pps_capture)) {
-		_rtc_drift_time = pps_capture.rtc_drift_time;
+		_pps_hrt_timestamp = pps_capture.timestamp;
+		_pps_rtc_timestamp = pps_capture.rtc_timestamp;
 	}
 
-	trigger.timestamp_utc = _trigger.rtc_edge_time + _rtc_drift_time;
+
+	if (_pps_hrt_timestamp > 0) {
+		// Current RTC time (RTC time captured by the PPS module + elapsed time since capture)
+		uint64_t gps_utc_time = _pps_rtc_timestamp + hrt_elapsed_time(&_pps_hrt_timestamp);
+		// Current RTC time - elapsed time since capture interrupt event
+		trigger.timestamp_utc = gps_utc_time - hrt_elapsed_time(&trigger.timestamp);
+
+	} else {
+		// No PPS capture received, use RTC clock as fallback
+		timespec tv{};
+		px4_clock_gettime(CLOCK_REALTIME, &tv);
+		trigger.timestamp_utc = ts_to_abstime(&tv) - hrt_elapsed_time(&trigger.timestamp);
+	}
 
 	_trigger_pub.publish(trigger);
 }

src/drivers/camera_capture/camera_capture.hpp

@@ -110,7 +110,6 @@ private:
 	struct _trig_s {
 		uint32_t chan_index;
 		hrt_abstime hrt_edge_time;
-		uint64_t rtc_edge_time;
 		uint32_t edge_state;
 		uint32_t overflow;
 	} _trigger{};
@@ -133,7 +132,8 @@ private:
 	hrt_abstime		_last_trig_time{0};
 	uint32_t 		_capture_overflows{0};
 
-	int32_t			_rtc_drift_time{0};
+	uint64_t    _pps_hrt_timestamp{0};
+	uint64_t    _pps_rtc_timestamp{0};
 
 	// Signal capture callback
 	void			capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);

src/drivers/camera_trigger/camera_trigger.cpp

@@ -174,7 +174,8 @@ private:
 	bool 			_turning_on{false};
 	matrix::Vector2f	_last_shoot_position{0.f, 0.f};
 	bool			_valid_position{false};
-	int32_t 		_rtc_drift_time{0};
+	uint64_t    _pps_hrt_timestamp{0};
+	uint64_t    _pps_rtc_timestamp{0};
 
 	//Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
 	uint32_t		_CAMPOS_num_poses{0};
@@ -828,16 +829,24 @@ CameraTrigger::engage(void *arg)
 	pps_capture_s pps_capture;
 
 	if (trig->_pps_capture_sub.update(&pps_capture)) {
-		trig->_rtc_drift_time = pps_capture.rtc_drift_time;
+		trig->_pps_hrt_timestamp = pps_capture.timestamp;
+		trig->_pps_rtc_timestamp = pps_capture.rtc_timestamp;
 	}
 
 	// Send camera trigger message. This messages indicates that we sent
 	// the camera trigger request. Does not guarantee capture.
 	camera_trigger_s trigger{};
 
-	timespec tv{};
-	px4_clock_gettime(CLOCK_REALTIME, &tv);
-	trigger.timestamp_utc = ts_to_abstime(&tv) + trig->_rtc_drift_time;
+	if (trig->_pps_hrt_timestamp > 0) {
+		// Current RTC time (RTC time captured by the PPS module + elapsed time since capture)
+		trigger.timestamp_utc = trig->_pps_rtc_timestamp + hrt_elapsed_time(&trig->_pps_hrt_timestamp);
+
+	} else {
+		// No PPS capture received, use RTC clock as fallback
+		timespec tv{};
+		px4_clock_gettime(CLOCK_REALTIME, &tv);
+		trigger.timestamp_utc = ts_to_abstime(&tv);
+	}
 
 	trigger.seq = trig->_trigger_seq;
 	trigger.feedback = false;

src/drivers/pps_capture/PPSCapture.cpp

@@ -89,14 +89,27 @@ void PPSCapture::Run()
 		return;
 	}
 
+	sensor_gps_s sensor_gps;
+
+	if (_sensor_gps_sub.update(&sensor_gps)) {
+		_last_gps_utc_timestamp = sensor_gps.time_utc_usec;
+		_last_gps_timestamp = sensor_gps.timestamp;
+	}
+
 	pps_capture_s pps_capture;
-	pps_capture.timestamp = hrt_absolute_time();
-	pps_capture.rtc_timestamp = ts_to_abstime(&_rtc_system_time);
-	uint32_t microseconds_part = pps_capture.rtc_timestamp % USEC_IN_1_SEC;
+	pps_capture.timestamp = _hrt_timestamp;
+	// GPS UTC time when the GPIO interrupt was triggered
+	// UTC time + elapsed time since the UTC time was received - eleapsed time since the PPS interrupt
+	// event
+	uint64_t gps_utc_time = _last_gps_utc_timestamp + hrt_elapsed_time(&_last_gps_timestamp)
+				- hrt_elapsed_time(&_hrt_timestamp);
+
+	// (For ubx F9P) The rising edge of the PPS pulse is aligned to the top of second GPS time base.
+	// So, remove the fraction of second and shift to the next second. The interrupt is triggered
+	// before the matching timestamp tranfered, which means the last received GPS time is always
+	// behind.
+	pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC;
 
-	/* max drift time to detect with PPS is in the range between -0.5 s to +0.5 s */
-	pps_capture.rtc_drift_time = (microseconds_part >= MAX_POSITIVE_DRIFT) ? ((-1 * USEC_IN_1_SEC) + microseconds_part)
-				     : microseconds_part;
 	_pps_capture_pub.publish(pps_capture);
 }
 
@@ -104,7 +117,7 @@ int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg)
 {
 	PPSCapture *instance = static_cast<PPSCapture *>(arg);
 
-	px4_clock_gettime(CLOCK_REALTIME, &(instance->_rtc_system_time));
+	instance->_hrt_timestamp = hrt_absolute_time();
 	instance->ScheduleNow(); // schedule work queue to publish PPS captured time
 
 	return PX4_OK;

src/drivers/pps_capture/PPSCapture.hpp

@@ -38,7 +38,9 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/pps_capture.h>
+#include <uORB/topics/sensor_gps.h>
 
 using namespace time_literals;
 
@@ -69,10 +71,14 @@ private:
 
 	uint32_t _pps_capture_gpio{0};
 	uORB::Publication<pps_capture_s>	_pps_capture_pub{ORB_ID(pps_capture)};
+	uORB::Subscription								_sensor_gps_sub{ORB_ID(sensor_gps)};
 
 	static constexpr unsigned USEC_IN_1_SEC{1000000}; // microseconds in 1 second
 	static constexpr unsigned MAX_POSITIVE_DRIFT{USEC_IN_1_SEC / 2}; // microseconds
 
-	timespec _rtc_system_time{0};
+	hrt_abstime _hrt_timestamp{0};
+
+	uint64_t _last_gps_timestamp{0};
+	uint64_t _last_gps_utc_timestamp{0};
 
 };