camera_capture:Use IOCTL to set capture mode.

src/drivers/camera_capture/camera_capture.cpp

@@ -232,24 +232,55 @@ CameraCapture::cycle()
 void
 CameraCapture::set_capture_control(bool enabled)
 {
-	if (enabled) {
+	int fd = -1;
-		// register callbacks
+	fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
-		if (!_camera_capture_edge) {
-			up_input_capture_set(5, Falling, 0, &CameraCapture::capture_trampoline, this);
-
-		} else {
-			up_input_capture_set(5, Rising, 0, &CameraCapture::capture_trampoline, this);
 
+	if (fd < 0) {
+		PX4_ERR("open fail");
+		return;
 	}
 
-		_capture_enabled = true;
+	input_capture_config_t conf;
+	conf.channel = 5; // FMU chan 6
+	conf.filter = 0;
+	conf.edge = _camera_capture_edge ? Rising : Falling;
+	conf.callback = NULL;
+	conf.context = NULL;
+
+	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_5PWM1CAP;
+
+			if (::ioctl(fd, PWM_SERVO_SET_MODE, mode) == 0) {
+				PX4_INFO("Mode changed to 5PWM1CAP");
 
 			} else {
-		up_input_capture_set(5, Disabled, 0, NULL, NULL);
+				PX4_ERR("Mode NOT changed to 4PWM2CAP");
+				goto err_out;
+			}
+		}
+	}
+
+	if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
+		_capture_enabled = enabled;
+
+	} else {
+		PX4_ERR("Unable to set capture callback for chan %u\n", conf.channel);
 		_capture_enabled = false;
+		goto err_out;
 	}
 
 	reset_statistics(false);
+err_out:
+	::close(fd);
+	return;
 }
 
 void

src/drivers/camera_capture/camera_capture.hpp

@@ -55,6 +55,8 @@
 #include <px4_workqueue.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/drv_pwm_output.h>
 #include <drivers/drv_input_capture.h>
 #include <drivers/device/ringbuffer.h>