camera_capture: use up_input_capture_set directly

It reserves the channel and pwm_out will not use it

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

@@ -106,11 +106,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer,
 	uint16_t capture = _REG32(timer->base, chan->ccr_offset);
 	channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in);
 
-	if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
-		channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
+	if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) {
+		channel_stats[chan_index].latency = (isrs_rcnt - capture);
 	}
 
-	channel_stats[chan_index].chan_in_edges_out++;
+	channel_stats[chan_index].edges++;
 	channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
 	uint32_t overflow = _REG32(timer->base, STM32_GTIM_SR_OFFSET) & chan->masks & GTIM_SR_CCOF;
 

src/drivers/camera_capture/CMakeLists.txt

@@ -36,5 +36,7 @@ px4_add_module(
 	COMPILE_FLAGS
 	SRCS
 		camera_capture.cpp
+	DEPENDS
+		arch_io_pins
 	)
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/camera_capture/camera_capture.cpp

@@ -43,6 +43,8 @@
 
 #define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
 
+static constexpr int capture_channel = 5; ///< FMU output 6
+
 namespace camera_capture
 {
 CameraCapture *g_camera_capture{nullptr};
@@ -216,63 +218,27 @@ CameraCapture::set_capture_control(bool enabled)
 
 #else
 
-	int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
-
-	if (fd < 0) {
-		PX4_ERR("open fail");
-		return;
-	}
-
-	input_capture_config_t conf;
-	conf.channel = 5; // FMU chan 6
-	conf.filter = 0;
-
-	if (_camera_capture_mode == 0) {
-		conf.edge = _camera_capture_edge ? Rising : Falling;
-
-	} else {
-		conf.edge = Both;
-	}
-
-	conf.callback = nullptr;
-	conf.context = nullptr;
+	capture_callback_t callback = nullptr;
+	void *context = nullptr;
 
 	if (enabled) {
-
-		conf.callback = &CameraCapture::capture_trampoline;
-		conf.context = this;
-
-		unsigned int capture_count = 0;
-
-		if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
-			PX4_INFO("Not in a capture mode");
-
-			unsigned long mode = PWM_SERVO_MODE_4PWM2CAP;
-
-			if (::ioctl(fd, PWM_SERVO_SET_MODE, mode) == 0) {
-				PX4_INFO("Mode changed to 4PWM2CAP");
-
-			} else {
-				PX4_ERR("Mode NOT changed to 4PWM2CAP!");
-				goto err_out;
-			}
-		}
+		callback = &CameraCapture::capture_trampoline;
+		context = this;
 	}
 
-	if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
+	int ret = up_input_capture_set_callback(capture_channel, callback, context);
+
+	if (ret == 0) {
 		_capture_enabled = enabled;
 		_gpio_capture = false;
 
 	} else {
-		PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", conf.channel);
+		PX4_ERR("Unable to set capture callback for chan %" PRIu8 " (%i)", capture_channel, ret);
 		_capture_enabled = false;
-		goto err_out;
 	}
 
 	reset_statistics(false);
 
-err_out:
-	::close(fd);
 #endif
 }
 
@@ -292,6 +258,22 @@ CameraCapture::reset_statistics(bool reset_seq)
 int
 CameraCapture::start()
 {
+#if !defined(BOARD_CAPTURE_GPIO)
+	input_capture_edge edge = Both;
+
+	if (_camera_capture_mode == 0) {
+		edge = _camera_capture_edge ? Rising : Falling;
+	}
+
+	int ret = up_input_capture_set(capture_channel, edge, 0, nullptr, nullptr);
+
+	if (ret != 0) {
+		PX4_ERR("up_input_capture_set failed (%i)", ret);
+		return ret;
+	}
+
+#endif
+
 	// run every 100 ms (10 Hz)
 	ScheduleOnInterval(100000, 10000);
 
@@ -329,6 +311,26 @@ CameraCapture::status()
 	}
 
 	PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows);
+
+#if !defined(BOARD_CAPTURE_GPIO)
+	input_capture_stats_t stats;
+	int ret =  up_input_capture_get_stats(capture_channel, &stats, false);
+
+	if (ret != 0) {
+		PX4_ERR("Unable to get stats for chan %" PRIu8 " (%i)", capture_channel, ret);
+
+	} else {
+		PX4_INFO("Status chan: %" PRIu8 " edges: %" PRIu32 " last time: %" PRIu64 " last state: %" PRIu32
+			 " overflows: %" PRIu32 " latency: %" PRIu16,
+			 capture_channel,
+			 stats.edges,
+			 stats.last_time,
+			 stats.last_edge,
+			 stats.overflows,
+			 stats.latency);
+	}
+
+#endif
 }
 
 static int usage()

src/drivers/camera_capture/camera_capture.hpp

@@ -54,9 +54,6 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
-#define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
-
-
 class CameraCapture : public px4::ScheduledWorkItem
 {
 public:

src/drivers/camera_trigger/interfaces/src/gpio.cpp

@@ -56,7 +56,7 @@ CameraInterfaceGPIO::~CameraInterfaceGPIO()
 	unsigned channel = 0;
 
 	while (_allocated_channels != 0) {
-		if (((1 << channel) & _allocated_channels)) {
+		if (((1u << channel) & _allocated_channels)) {
 			io_timer_unallocate_channel(channel);
 			_allocated_channels &= ~(1u << channel);
 		}

src/drivers/drv_input_capture.h

@@ -83,11 +83,11 @@ typedef struct input_capture_element_t {
 } input_capture_element_t;
 
 typedef struct input_capture_stats_t {
-	uint32_t 		   chan_in_edges_out;
+	uint32_t 		   edges;
 	uint32_t 		   overflows;
 	uint32_t		   last_edge;
 	hrt_abstime		   last_time;
-	uint16_t		   latnecy;
+	uint16_t		   latency;
 } input_capture_stats_t;
 
 /**

src/drivers/pwm_out/PWMOut.cpp

@@ -1850,16 +1850,10 @@ int PWMOut::test(const char *dev)
 {
 	int	 fd;
 	unsigned servo_count = 0;
-	unsigned capture_count = 0;
 	unsigned pwm_value = 1000;
 	int	 direction = 1;
 	int  ret;
 	int   rv = -1;
-	uint32_t rate_limit = 0;
-	struct input_capture_t {
-		bool valid;
-		input_capture_config_t  chan;
-	} capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
 
 	fd = ::open(dev, O_RDWR);
 
@@ -1883,39 +1877,7 @@ int PWMOut::test(const char *dev)
 		goto err_out;
 	}
 
-	if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
-		PX4_INFO("Not in a capture mode");
-	}
-
-	PX4_INFO("Testing %u servos and %u input captures", servo_count, capture_count);
-	memset(capture_conf, 0, sizeof(capture_conf));
-
-	if (capture_count != 0) {
-		for (unsigned i = 0; i < capture_count; i++) {
-			// Map to channel number
-			capture_conf[i].chan.channel = i + servo_count;
-
-			/* Save handler */
-			if (::ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) {
-				PX4_ERR("Unable to get capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
-				goto err_out;
-
-			} else {
-				input_capture_config_t conf = capture_conf[i].chan;
-				conf.callback = &PWMOut::capture_trampoline;
-				conf.context = _objects[0].load(); // TODO PWMOut::get_instance();
-
-				if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
-					capture_conf[i].valid = true;
-
-				} else {
-					PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
-					goto err_out;
-				}
-			}
-
-		}
-	}
+	PX4_INFO("Testing %u servos", servo_count);
 
 	struct pollfd fds;
 
@@ -1972,31 +1934,6 @@ int PWMOut::test(const char *dev)
 			}
 		}
 
-		if (capture_count != 0 && (++rate_limit % 500 == 0)) {
-			for (unsigned i = 0; i < capture_count; i++) {
-				if (capture_conf[i].valid) {
-					input_capture_stats_t stats;
-					stats.chan_in_edges_out = capture_conf[i].chan.channel;
-
-					if (::ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) {
-						PX4_ERR("Unable to get stats for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
-						goto err_out;
-
-					} else {
-						fprintf(stdout, "FMU: Status chan:%" PRIu8 " edges: %" PRIu32 " last time:%" PRIu64 " last state:%" PRIu32
-							" overflows:%" PRIu32 " lantency:%" PRIu16 "\n",
-							capture_conf[i].chan.channel,
-							stats.chan_in_edges_out,
-							stats.last_time,
-							stats.last_edge,
-							stats.overflows,
-							stats.latnecy);
-					}
-				}
-			}
-
-		}
-
 		/* Check if user wants to quit */
 		char c;
 		ret = ::poll(&fds, 1, 0);
@@ -2012,19 +1949,6 @@ int PWMOut::test(const char *dev)
 		}
 	}
 
-	if (capture_count != 0) {
-		for (unsigned i = 0; i < capture_count; i++) {
-			// Map to channel number
-			if (capture_conf[i].valid) {
-				/* Save handler */
-				if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) {
-					PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel);
-					goto err_out;
-				}
-			}
-		}
-	}
-
 	rv = 0;
 
 err_out: