PPS capture driver (not PPS clock synchronization)

ROMFS/px4fmu_common/init.d/rcS

@@ -477,6 +477,12 @@ else
 		rc_input start $RC_INPUT_ARGS
 	fi
 
+	# PPS capture driver (before pwm_out)
+	if param greater -s PPS_CAP_ENABLE 0
+	then
+		pps_capture start
+	fi
+
 	# Camera capture driver (before pwm_out)
 	if param greater -s CAM_CAP_FBACK 0
 	then

Tools/kconfig/cmake_kconfig_lut.txt

@@ -92,6 +92,7 @@ pca9685,CONFIG_DRIVERS_PCA9685=y
 pca9685_pwm_out,CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 power_monitor/ina226,CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 power_monitor/voxlpm,CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
+pps_capture,CONFIG_DRIVERS_PPS_CAPTURE=y
 protocol_splitter,CONFIG_DRIVERS_PROTOCOL_SPLITTER=y
 pwm_input,CONFIG_DRIVERS_PWM_INPUT=y
 pwm_out_sim,CONFIG_DRIVERS_PWM_OUT_SIM=y

msg/CMakeLists.txt

@@ -118,6 +118,7 @@ set(msg_files
 	orbit_status.msg
 	parameter_update.msg
 	ping.msg
+	pps_capture.msg
 	position_controller_landing_status.msg
 	position_controller_status.msg
 	position_setpoint.msg

msg/camera_trigger.msg

@@ -4,4 +4,6 @@ uint64 timestamp_utc # UTC timestamp
 uint32 seq		# Image sequence number
 bool feedback	# Trigger feedback from camera
 
+uint32 ORB_QUEUE_LENGTH = 2
+
 # TOPICS camera_trigger

msg/pps_capture.msg

@@ -0,0 +1,2 @@
+uint64 timestamp			  # time since system start (microseconds) at PPS capture event
+uint64 rtc_timestamp		# Corrected GPS UTC timestamp at PPS capture event

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h

@@ -62,7 +62,8 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_Trigger = 5,
 	IOTimerChanMode_Dshot   = 6,
 	IOTimerChanMode_LED     = 7,
-	IOTimerChanMode_Other   = 8,
+	IOTimerChanMode_PPS     = 8,
+	IOTimerChanMode_Other   = 9,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 

platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c

@@ -149,8 +149,8 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 #define rFCTRL20(_tim)         REG(_tim,  0, IMXRT_FLEXPWM_FCTRL20_OFFSET)       /* Fault Control 2 Register */
 
 
-//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED Other
-io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0 };
+//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED PPS Other
+io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0, 0 };
 
 typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
 

platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h

@@ -109,6 +109,7 @@ int kinetis_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, boo
 
 #define _PX4_MAKE_GPIO(pin_ftmx, io)    ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io))
 #define PX4_MAKE_GPIO_INPUT(gpio) _PX4_MAKE_GPIO(gpio, GPIO_PULLUP)
+#define PX4_MAKE_GPIO_EXTI(gpio) _PX4_MAKE_GPIO(gpio, PIN_INT_BOTH | GPIO_PULLUP)
 #define PX4_MAKE_GPIO_OUTPUT_SET(gpio) _PX4_MAKE_GPIO(gpio, GPIO_HIGHDRIVE)
 #define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) _PX4_MAKE_GPIO(gpio, GPIO_LOWDRIVE)
 

platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h

@@ -66,7 +66,8 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_Trigger = 5,
 	IOTimerChanMode_Dshot   = 6,
 	IOTimerChanMode_LED     = 7,
-	IOTimerChanMode_Other   = 8,
+	IOTimerChanMode_PPS     = 8,
+	IOTimerChanMode_Other   = 9,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 

platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c

@@ -152,8 +152,8 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 
 #define CnSC_PWMIN_INIT 0 // TBD
 
-//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED Other
-io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0 };
+//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED PPS Other
+io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0, 0 };
 
 typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
 

platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h

@@ -62,7 +62,8 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_Trigger = 5,
 	IOTimerChanMode_Dshot   = 6,
 	IOTimerChanMode_LED     = 7,
-	IOTimerChanMode_Other   = 8,
+	IOTimerChanMode_PPS     = 8,
+	IOTimerChanMode_Other   = 9,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 

platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c

@@ -150,8 +150,8 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 
 #define CnSC_PWMIN_INIT 0 // TBD
 
-//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED Other
-io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0 };
+//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED PPS Other
+io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0, 0 };
 
 typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
 

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h

@@ -75,7 +75,8 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_Trigger = 5,
 	IOTimerChanMode_Dshot   = 6,
 	IOTimerChanMode_LED     = 7,
-	IOTimerChanMode_Other   = 8,
+	IOTimerChanMode_PPS     = 8,
+	IOTimerChanMode_Other   = 9,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h

@@ -103,6 +103,7 @@ __BEGIN_DECLS
 #define px4_arch_gpiosetevent(pinset,r,f,e,fp,a)  stm32_gpiosetevent(pinset,r,f,e,fp,a)
 
 #define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP))
+#define PX4_MAKE_GPIO_EXTI(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_EXTI|GPIO_INPUT|GPIO_PULLUP))
 #define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR))
 #define PX4_MAKE_GPIO_OUTPUT_SET(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET))
 

platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c

@@ -149,8 +149,8 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 /* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA  */
 #define TIM_DMABURSTLENGTH_4TRANSFERS	0x00000300U
 
-//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED Other
-io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0 };
+//												 				  NotUsed   PWMOut  PWMIn Capture OneShot Trigger Dshot LED PPS Other
+io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX,   0,  0,  0, 0, 0, 0, 0, 0, 0 };
 
 typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
 

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,7 @@ 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();
 	dev->_trigger.edge_state = 0;
 	dev->_trigger.overflow = 0;
 
@@ -144,32 +144,33 @@ 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++;
 		}
 
 	} 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 +184,25 @@ CameraCapture::publish_trigger()
 		return;
 	}
 
+	pps_capture_s pps_capture;
+
+	if (_pps_capture_sub.update(&pps_capture)) {
+		_pps_hrt_timestamp = pps_capture.timestamp;
+		_pps_rtc_timestamp = pps_capture.rtc_timestamp;
+	}
+
+
+	if (_pps_hrt_timestamp > 0) {
+		// Last PPS RTC time + elapsed time to the camera capture interrupt
+		trigger.timestamp_utc = _pps_rtc_timestamp + (trigger.timestamp - _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) - hrt_elapsed_time(&trigger.timestamp);
+	}
+
 	_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,12 @@ class CameraCapture : public px4::ScheduledWorkItem
 
 	// 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;
 		uint32_t edge_state;
 		uint32_t overflow;
 	} _trigger{};
@@ -130,6 +132,9 @@ class CameraCapture : public px4::ScheduledWorkItem
 	hrt_abstime		_last_trig_time{0};
 	uint32_t 		_capture_overflows{0};
 
+	hrt_abstime	_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

@@ -59,6 +59,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>
 #include <uORB/topics/vehicle_local_position.h>
@@ -173,6 +174,8 @@ class CameraTrigger : public px4::ScheduledWorkItem
 	bool 			_turning_on{false};
 	matrix::Vector2f	_last_shoot_position{0.f, 0.f};
 	bool			_valid_position{false};
+	hrt_abstime	_pps_hrt_timestamp{0};
+	uint64_t		_pps_rtc_timestamp{0};
 
 	//Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
 	uint32_t		_CAMPOS_num_poses{0};
@@ -186,6 +189,7 @@ class CameraTrigger : public px4::ScheduledWorkItem
 
 	uORB::Subscription	_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_lpos_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription	_pps_capture_sub{ORB_ID(pps_capture)};
 
 	orb_advert_t		_trigger_pub{nullptr};
 
@@ -197,10 +201,8 @@ class CameraTrigger : public px4::ScheduledWorkItem
 	param_t			_p_min_interval;
 	param_t			_p_distance;
 	param_t			_p_interface;
-	param_t 		_p_cam_cap_fback;
 
 	trigger_mode_t		_trigger_mode{TRIGGER_MODE_NONE};
-	int32_t _cam_cap_fback{0};
 
 	camera_interface_mode_t	_camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO};
 	CameraInterface		*_camera_interface{nullptr};  ///< instance of camera interface
@@ -263,7 +265,6 @@ CameraTrigger::CameraTrigger() :
 	_p_activation_time = param_find("TRIG_ACT_TIME");
 	_p_mode = param_find("TRIG_MODE");
 	_p_interface = param_find("TRIG_INTERFACE");
-	_p_cam_cap_fback = param_find("CAM_CAP_FBACK");
 
 	param_get(_p_activation_time, &_activation_time);
 	param_get(_p_interval, &_interval);
@@ -272,10 +273,6 @@ CameraTrigger::CameraTrigger() :
 	param_get(_p_mode, (int32_t *)&_trigger_mode);
 	param_get(_p_interface, (int32_t *)&_camera_interface_mode);
 
-	if (_p_cam_cap_fback != PARAM_INVALID) {
-		param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
-	}
-
 	switch (_camera_interface_mode) {
 #ifdef __PX4_NUTTX
 
@@ -317,10 +314,7 @@ CameraTrigger::CameraTrigger() :
 	// Advertise critical publishers here, because we cannot advertise in interrupt context
 	camera_trigger_s trigger{};
 
-	if (!_cam_cap_fback) {
-		_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
-
-	}
+	_trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH);
 }
 
 CameraTrigger::~CameraTrigger()
@@ -832,23 +826,33 @@ CameraTrigger::engage(void *arg)
 		return;
 	}
 
+	pps_capture_s pps_capture;
+
+	if (trig->_pps_capture_sub.update(&pps_capture)) {
+		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 = (uint64_t) tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
+	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;
 	trigger.timestamp = hrt_absolute_time();
 
-	// Publish only if  _cam_cap_fback is disabled, otherwise, it is published over camera_capture driver
-	if (!trig->_cam_cap_fback) {
-		orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
-
-	}
+	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
 
 	// increment frame count
 	trig->_trigger_seq++;