camera_capture: update to use PPS correction for UTC time

src/drivers/camera_capture/camera_capture.cpp

@@ -105,7 +105,7 @@ void
 CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
 {
 	_trigger.chan_index = chan_index;
-	_trigger.edge_time = edge_time;
+	_trigger.hrt_edge_time = edge_time;
 	_trigger.edge_state = edge_state;
 	_trigger.overflow = overflow;
 
@@ -118,7 +118,8 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
 	CameraCapture *dev = static_cast<CameraCapture *>(arg);
 
 	dev->_trigger.chan_index = 0;
-	dev->_trigger.edge_time = hrt_absolute_time();
+	dev->_trigger.hrt_edge_time = hrt_absolute_time();
+	px4_clock_gettime(CLOCK_REALTIME, &dev->_trigger.rtc_edge_time);
 	dev->_trigger.edge_state = 0;
 	dev->_trigger.overflow = 0;
 
@@ -144,19 +145,20 @@ CameraCapture::publish_trigger()
 
 	// MODES 1 and 2 are not fully tested
 	if (_camera_capture_mode == 0 || _gpio_capture) {
-		trigger.timestamp = _trigger.edge_time - uint64_t(1000 * _strobe_delay);
+		trigger.timestamp = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
 		trigger.seq = _capture_seq++;
 		_last_trig_time = trigger.timestamp;
+
 		publish = true;
 
 	} else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high)
 		if (_trigger.edge_state == 1) {
-			_last_trig_begin_time = _trigger.edge_time - uint64_t(1000 * _strobe_delay);
+			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
 
 		} else if (_trigger.edge_state == 0 && _last_trig_begin_time > 0) {
-			trigger.timestamp = _trigger.edge_time - ((_trigger.edge_time - _last_trig_begin_time) / 2);
+			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
 			trigger.seq = _capture_seq++;
-			_last_exposure_time = _trigger.edge_time - _last_trig_begin_time;
+			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
 			_last_trig_time = trigger.timestamp;
 			publish = true;
 			_capture_seq++;
@@ -164,12 +166,12 @@ CameraCapture::publish_trigger()
 
 	} else { // Get timestamp of mid-exposure (active low)
 		if (_trigger.edge_state == 0) {
-			_last_trig_begin_time = _trigger.edge_time - uint64_t(1000 * _strobe_delay);
+			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
 
 		} else if (_trigger.edge_state == 1 && _last_trig_begin_time > 0) {
-			trigger.timestamp = _trigger.edge_time - ((_trigger.edge_time - _last_trig_begin_time) / 2);
+			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
 			trigger.seq = _capture_seq++;
-			_last_exposure_time = _trigger.edge_time - _last_trig_begin_time;
+			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
 			_last_trig_time = trigger.timestamp;
 			publish = true;
 		}
@@ -183,6 +185,14 @@ CameraCapture::publish_trigger()
 		return;
 	}
 
+	pps_capture_s pps_capture;
+
+	if (_pps_capture_sub.update(&pps_capture)) {
+		_rtc_drift_time = pps_capture.rtc_drift_time;
+	}
+
+	trigger.timestamp_utc = ts_to_abstime(&_trigger.rtc_edge_time) + _rtc_drift_time;
+
 	_trigger_pub.publish(trigger);
 }
 

src/drivers/camera_capture/camera_capture.hpp

@@ -51,6 +51,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/camera_trigger.h>
+#include <uORB/topics/pps_capture.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
@@ -103,11 +104,13 @@ private:
 
 	// Subscribers
 	uORB::Subscription				_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription				_pps_capture_sub{ORB_ID(pps_capture)};
 
 	// Trigger Buffer
 	struct _trig_s {
 		uint32_t chan_index;
-		hrt_abstime edge_time;
+		hrt_abstime hrt_edge_time;
+		timespec rtc_edge_time;
 		uint32_t edge_state;
 		uint32_t overflow;
 	} _trigger{};
@@ -130,6 +133,8 @@ private:
 	hrt_abstime		_last_trig_time{0};
 	uint32_t 		_capture_overflows{0};
 
+	int32_t			_rtc_drift_time{0};
+
 	// Signal capture callback
 	void			capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);