Camera trigger update (#4998)

* updating the camera driver, correct init and keepAlive function

* removing debug output

src/drivers/camera_trigger/camera_trigger.cpp

@@ -103,6 +103,11 @@ public:
 	 */
 	void		shootOnce();
 
+	/**
+	 * Toggle keep camera alive functionality
+	 */
+	void		keepAlive(bool on);
+
 	/**
 	 * Start the task.
 	 */
@@ -127,6 +132,9 @@ private:
 
 	struct hrt_call		_engagecall;
 	struct hrt_call		_disengagecall;
+	struct hrt_call		_keepalivecall_up;
+	struct hrt_call		_keepalivecall_down;
+
 	static struct work_s	_work;
 
 	int 			_gpio_fd;
@@ -167,6 +175,14 @@ private:
 	 * Resets trigger
 	 */
 	static void	disengage(void *arg);
+	/**
+	 * Fires trigger
+	 */
+	static void	keep_alive_up(void *arg);
+	/**
+	 * Resets trigger
+	 */
+	static void	keep_alive_down(void *arg);
 
 };
 
@@ -232,6 +248,7 @@ CameraTrigger::CameraTrigger() :
 	}
 
 	struct camera_trigger_s	report = {};
+
 	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &report);
 }
 
@@ -269,6 +286,26 @@ CameraTrigger::control(bool on)
 	_trigger_enabled = on;
 }
 
+void
+CameraTrigger::keepAlive(bool on)
+{
+	if (on) {
+		// schedule keep-alive up and down calls
+		hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000),
+			       (hrt_callout)&CameraTrigger::keep_alive_up, this);
+
+		// schedule keep-alive up and down calls
+		hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000),
+			       (hrt_callout)&CameraTrigger::keep_alive_down, this);
+
+	} else {
+		// cancel all calls
+		hrt_cancel(&_keepalivecall_up);
+		hrt_cancel(&_keepalivecall_down);
+	}
+
+}
+
 void
 CameraTrigger::shootOnce()
 {
@@ -289,6 +326,14 @@ CameraTrigger::start()
 		control(true);
 	}
 
+	// Prevent camera from sleeping, if triggering is enabled
+	if (_mode > 0) {
+		keepAlive(true);
+
+	} else {
+		keepAlive(false);
+	}
+
 	// start to monitor at high rate for trigger enable command
 	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
 
@@ -300,6 +345,8 @@ CameraTrigger::stop()
 	work_cancel(LPWORK, &_work);
 	hrt_cancel(&_engagecall);
 	hrt_cancel(&_disengagecall);
+	hrt_cancel(&_keepalivecall_up);
+	hrt_cancel(&_keepalivecall_down);
 
 	if (camera_trigger::g_camera_trigger != nullptr) {
 		delete(camera_trigger::g_camera_trigger);
@@ -390,7 +437,14 @@ CameraTrigger::cycle_trampoline(void *arg)
 
 				if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
 
-					// Set trigger to false if the set distance is not positive
+					// Set trigger to disabled if the set distance is not positive
+					if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
+						trig->_camera_interface->powerOn();
+
+					} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
+						trig->_camera_interface->powerOff();
+					}
+
 					trig->_trigger_enabled = cmd.param1 > 0.0f;
 					trig->_distance = cmd.param1;
 				}
@@ -450,6 +504,22 @@ CameraTrigger::disengage(void *arg)
 	trig->_camera_interface->trigger(false);
 }
 
+void
+CameraTrigger::keep_alive_up(void *arg)
+{
+	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
+
+	trig->_camera_interface->keep_alive(true);
+}
+
+void
+CameraTrigger::keep_alive_down(void *arg)
+{
+	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
+
+	trig->_camera_interface->keep_alive(false);
+}
+
 void
 CameraTrigger::info()
 {

src/drivers/camera_trigger/camera_trigger_params.c

@@ -36,25 +36,12 @@
  * Camera trigger parameters
  *
  * @author Mohammed Kabir <mhkabir98@gmail.com>
+ * @author Andreas Bircher <andreas@wingtra.com>
  */
 
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
- /**
- * Camera trigger Interface
- *
- * Selects the trigger interface
- *
- * @value 1 GPIO
- * @value 2 Seagull MAP2 (PWM)
- *
- * @reboot_required true
- *
- * @group Camera trigger
- */
-PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
-
 /**
 * Camera trigger Interface
 *
@@ -102,7 +89,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
  *
  * @unit ms
  * @min 0.1
- * @max 3
+ * @max 3000
  * @decimal 1
  * @group Camera trigger
  */
@@ -126,7 +113,9 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
 /**
  * Camera trigger pin
  *
- * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6)
+ * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4fmu-v2 and the rail
+ * pins on px4fmu-v4). The PWM interface takes two pins per camera, while relay
+ * triggers on every pin individually. Example: Value 34 would trigger on pins 3 and 4.
  *
  * @min 1
  * @max 123456

src/drivers/camera_trigger/interfaces/src/camera_interface.h

@@ -24,6 +24,11 @@ public:
 	 */
 	virtual void trigger(bool enable) {};
 
+	/**
+	 * prevent the camera from sleeping
+	 * @param keep alive signal:
+	 */
+	virtual void keep_alive(bool signal_on) {};
 
 	/**
 	 * Display info.

src/drivers/camera_trigger/interfaces/src/pwm.cpp

@@ -7,12 +7,14 @@
 
 // PWM levels of the interface to seagull MAP converter to
 // Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf)
-#define PWM_CAMERA_DISARMED			90 // TODO(birchera): check here value
+#define PWM_CAMERA_DISARMED			900
 #define PWM_CAMERA_ON				1100
 #define PWM_CAMERA_AUTOFOCUS_SHOOT	1300
 #define PWM_CAMERA_NEUTRAL			1500
 #define PWM_CAMERA_INSTANT_SHOOT	1700
 #define PWM_CAMERA_OFF				1900
+#define PWM_2_CAMERA_KEEP_ALIVE		1700
+#define PWM_2_CAMERA_ON_OFF			1900
 
 CameraInterfacePWM::CameraInterfacePWM():
 	CameraInterface(),
@@ -48,13 +50,18 @@ CameraInterfacePWM::CameraInterfacePWM():
 
 CameraInterfacePWM::~CameraInterfacePWM()
 {
+	// Deinitialise pwm channels
+	// Can currently not be used, because up_pwm_servo_deinit() will deinitialise all pwm channels
+	// up_pwm_servo_deinit();
 }
 
 void CameraInterfacePWM::setup()
 {
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		if (_pins[i] >= 0) {
-			// TODO(birchera): use return value to make sure everything is fine
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+			uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
+			up_pwm_servo_init(pin_bitmask);
+			up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
 			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
 		}
 	}
@@ -65,23 +72,52 @@ void CameraInterfacePWM::trigger(bool enable)
 	// This only starts working upon prearming
 
 	if (!_camera_is_on) {
+		// (TODO: powerOn does not work yet)
 		// Turn camera on and give time to start up
-		powerOn();
+		// powerOn();
 		return;
 	}
 
 	if (enable) {
 		// Set all valid pins to shoot level
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
-				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+				up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
 			}
 		}
 
 	} else {
 		// Set all valid pins back to neutral level
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+				up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+			}
+		}
+	}
+}
+
+void CameraInterfacePWM::keep_alive(bool signal_on)
+{
+	// This should alternate between signal_on and !signal_on to keep the camera alive
+
+	if (!_camera_is_on) {
+		// (TODO: powerOn does not work yet)
+		// Turn camera on and give time to start up
+		powerOn();
+	}
+
+	if (signal_on) {
+		// Set channel 2 pin to keep_alive signal
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+				up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_KEEP_ALIVE, 1000, 2000));
+			}
+		}
+
+	} else {
+		// Set channel 2 pin to neutral signal
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
 				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 			}
 		}
@@ -93,9 +129,17 @@ int CameraInterfacePWM::powerOn()
 	// This only starts working upon prearming
 
 	// Set all valid pins to turn on level
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		if (_pins[i] >= 0) {
-			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_ON, 1000, 2000));
+	// for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+	// 	if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+	// 		up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_ON, 1000, 2000));
+	// 		up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
+	// 	}
+	// }
+
+	// For now, set channel one on neutral upon startup.
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+			up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 		}
 	}
 
@@ -109,9 +153,10 @@ int CameraInterfacePWM::powerOff()
 	// This only starts working upon prearming
 
 	// Set all valid pins to turn off level
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		if (_pins[i] >= 0) {
-			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+			up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+			up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
 		}
 	}
 

src/drivers/camera_trigger/interfaces/src/pwm.h

@@ -18,6 +18,7 @@ public:
 	virtual ~CameraInterfacePWM();
 
 	void trigger(bool enable);
+	void keep_alive(bool signal_on);
 
 	int powerOn();
 	int powerOff();