initial version camera turn on / off

src/drivers/camera_trigger/camera_trigger.cpp

@@ -108,6 +108,11 @@ public:
 	 */
 	void		keepAlive(bool on);
 
+	/**
+	 * Toggle camera on/off functionality
+	 */
+	void        turnOnOff();
+
 	/**
 	 * Start the task.
 	 */
@@ -132,6 +137,8 @@ private:
 
 	struct hrt_call		_engagecall;
 	struct hrt_call		_disengagecall;
+	struct hrt_call     _engage_turn_on_off_call;
+	struct hrt_call     _disengage_turn_on_off_call;
 	struct hrt_call		_keepalivecall_up;
 	struct hrt_call		_keepalivecall_down;
 
@@ -175,6 +182,14 @@ private:
 	 * Resets trigger
 	 */
 	static void	disengage(void *arg);
+	/**
+	 * Fires on/off
+	 */
+	static void engange_turn_on_off(void *arg);
+	/**
+	 * Resets  on/off
+	 */
+	static void disengage_turn_on_off(void *arg);
 	/**
 	 * Fires trigger
 	 */
@@ -197,6 +212,10 @@ CameraTrigger	*g_camera_trigger;
 CameraTrigger::CameraTrigger() :
 	_engagecall {},
 	_disengagecall {},
+	_engage_turn_on_off_call {},
+	_disengage_turn_on_off_call {},
+	_keepalivecall_up {},
+	_keepalivecall_down {},
 	_gpio_fd(-1),
 	_mode(0),
 	_activation_time(0.5f /* ms */),
@@ -306,6 +325,18 @@ CameraTrigger::keepAlive(bool on)
 
 }
 
+void
+CameraTrigger::turnOnOff()
+{
+	// schedule trigger on and off calls
+	hrt_call_after(&_engage_turn_on_off_call, 0,
+		       (hrt_callout)&CameraTrigger::engange_turn_on_off, this);
+
+	// schedule trigger on and off calls
+	hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000),
+		       (hrt_callout)&CameraTrigger::disengage_turn_on_off, this);
+}
+
 void
 CameraTrigger::shootOnce()
 {
@@ -327,7 +358,8 @@ CameraTrigger::start()
 	}
 
 	// Prevent camera from sleeping, if triggering is enabled
-	if (_mode > 0) {
+	if (_mode > 0 && _mode < 4) {
+		turnOnOff();
 		keepAlive(true);
 
 	} else {
@@ -345,6 +377,8 @@ CameraTrigger::stop()
 	work_cancel(LPWORK, &_work);
 	hrt_cancel(&_engagecall);
 	hrt_cancel(&_disengagecall);
+	hrt_cancel(&_engage_turn_on_off_call);
+	hrt_cancel(&_disengage_turn_on_off_call);
 	hrt_cancel(&_keepalivecall_up);
 	hrt_cancel(&_keepalivecall_down);
 
@@ -358,6 +392,7 @@ CameraTrigger::test()
 {
 	struct vehicle_command_s cmd = {};
 	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
+
 	cmd.param5 = 1.0f;
 
 	orb_advert_t pub;
@@ -429,6 +464,8 @@ CameraTrigger::cycle_trampoline(void *arg)
 
 		if (pos.xy_valid) {
 
+			bool turning_on = false;
+
 			if (updated && trig->_mode == 4) {
 
 				// Check update from command
@@ -439,10 +476,16 @@ CameraTrigger::cycle_trampoline(void *arg)
 
 					// Set trigger to disabled if the set distance is not positive
 					if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
-						trig->_camera_interface->powerOn();
+						trig->turnOnOff();
+						trig->keepAlive(true);
+						poll_interval_usec = 2000000;
+						turning_on = true;
 
 					} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
-						trig->_camera_interface->powerOff();
+						hrt_cancel(&(trig->_engagecall));
+						hrt_cancel(&(trig->_disengagecall));
+						trig->keepAlive(false);
+						trig->turnOnOff();
 					}
 
 					trig->_trigger_enabled = cmd.param1 > 0.0f;
@@ -450,7 +493,7 @@ CameraTrigger::cycle_trampoline(void *arg)
 				}
 			}
 
-			if (trig->_trigger_enabled || trig->_mode < 4) {
+			if ((trig->_trigger_enabled || trig->_mode < 4) && !turning_on) {
 
 				// Initialize position if not done yet
 				math::Vector<2> current_position(pos.x, pos.y);
@@ -504,6 +547,23 @@ CameraTrigger::disengage(void *arg)
 	trig->_camera_interface->trigger(false);
 }
 
+void
+CameraTrigger::engange_turn_on_off(void *arg)
+{
+
+	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
+
+	trig->_camera_interface->turn_on_off(true);
+}
+
+void
+CameraTrigger::disengage_turn_on_off(void *arg)
+{
+	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
+
+	trig->_camera_interface->turn_on_off(false);
+}
+
 void
 CameraTrigger::keep_alive_up(void *arg)
 {

src/drivers/camera_trigger/camera_trigger_params.c

@@ -67,7 +67,7 @@ PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
  * @decimal 1
  * @group Camera trigger
  */
-PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f);
+PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 3000.0f);
 
 /**
  * Camera trigger polarity
@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
  * @decimal 1
  * @group Camera trigger
  */
-PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f);
+PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 500.0f);
 
 /**
  * Camera trigger mode
@@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
  * @reboot_required true
  * @group Camera trigger
  */
-PARAM_DEFINE_INT32(TRIG_PINS, 6);
+PARAM_DEFINE_INT32(TRIG_PINS, 56);
 
 /**
  * Camera trigger distance

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

@@ -24,6 +24,12 @@ public:
 	 */
 	virtual void trigger(bool enable) {};
 
+	/**
+	 * turn on/off the camera
+	 * @param enable:
+	 */
+	virtual void turn_on_off(bool enable) {};
+
 	/**
 	 * prevent the camera from sleeping
 	 * @param keep alive signal:

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

@@ -2,6 +2,7 @@
 #include <sys/ioctl.h>
 #include <lib/mathlib/mathlib.h>
 
+#include <uORB/topics/trigger_pwm.h>
 #include "drivers/drv_pwm_output.h"
 #include "pwm.h"
 
@@ -18,6 +19,7 @@
 
 CameraInterfacePWM::CameraInterfacePWM():
 	CameraInterface(),
+	_pwm_pub {nullptr, nullptr},
 	_camera_is_on(false)
 {
 	_p_pin = param_find("TRIG_PINS");
@@ -34,17 +36,24 @@ CameraInterfacePWM::CameraInterfacePWM():
 	int single_pin;
 
 	while ((single_pin = pin_list % 10)) {
-
 		_pins[i] = single_pin - 1;
 
 		if (_pins[i] < 0) {
 			_pins[i] = -1;
 		}
 
+		// Wingtra safety check, pins can only be on 5 or 6!!!
+		if (_pins[i] != 4 && _pins[i] != 5) {
+			_pins[i] = -1;
+		}
+
 		pin_list /= 10;
 		i++;
 	}
 
+	_orb_id[0] = 0;
+	_orb_id[1] = 1;
+
 	setup();
 }
 
@@ -63,6 +72,13 @@ void CameraInterfacePWM::setup()
 			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));
+
+			trigger_pwm_s pwm;
+			pwm.timestamp = hrt_absolute_time();
+			pwm.pwm = PWM_CAMERA_DISARMED;
+			orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
+			pwm.pwm = PWM_CAMERA_DISARMED;
+			orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
 		}
 	}
 }
@@ -72,17 +88,21 @@ 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();
 		return;
 	}
 
+
+	trigger_pwm_s pwm;
+	pwm.timestamp = hrt_absolute_time();
+
 	if (enable) {
 		// Set all valid pins to shoot level
 		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));
+
+				pwm.pwm = PWM_CAMERA_INSTANT_SHOOT;
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
 			}
 		}
 
@@ -91,6 +111,9 @@ void CameraInterfacePWM::trigger(bool enable)
 		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));
+
+				pwm.pwm = PWM_CAMERA_NEUTRAL;
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
 			}
 		}
 	}
@@ -101,16 +124,20 @@ 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)
+		return;
-		// Turn camera on and give time to start up
-		powerOn();
 	}
 
+	trigger_pwm_s pwm;
+	pwm.timestamp = hrt_absolute_time();
+
 	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));
+
+				pwm.pwm = PWM_2_CAMERA_KEEP_ALIVE;
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
 			}
 		}
 
@@ -119,50 +146,50 @@ void CameraInterfacePWM::keep_alive(bool signal_on)
 		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));
+
+				pwm.pwm = PWM_CAMERA_NEUTRAL;
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
 			}
 		}
 	}
 }
 
-int CameraInterfacePWM::powerOn()
+void CameraInterfacePWM::turn_on_off(bool enable)
 {
-	// This only starts working upon prearming
 
-	// Set all valid pins to turn on level
+	trigger_pwm_s pwm;
-	// for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+	pwm.timestamp = hrt_absolute_time();
-	// 	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.
+	if (enable) {
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+		// For now, set channel one on neutral upon startup.
-		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
-			up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+				up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+				up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
+
+				pwm.pwm = PWM_CAMERA_NEUTRAL;
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
+				pwm.pwm = PWM_2_CAMERA_ON_OFF;
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
+			}
 		}
-	}
 
-	_camera_is_on = true;
+	} else {
+		// 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));
+				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 
-	return 0;
+				pwm.pwm = PWM_CAMERA_NEUTRAL;
-}
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
-
+				pwm.pwm = PWM_CAMERA_NEUTRAL;
-int CameraInterfacePWM::powerOff()
+				orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
-{
+			}
-	// 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 = 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));
 		}
+
+		_camera_is_on = !_camera_is_on;
 	}
-
-	_camera_is_on = false;
-
-	return 0;
 }
 
 void CameraInterfacePWM::info()

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

@@ -6,6 +6,7 @@
  */
 #pragma once
 
+#include <drivers/drv_hrt.h>
 #include <systemlib/param/param.h>
 
 #include <uORB/topics/vehicle_status.h>
@@ -20,8 +21,7 @@ public:
 	void trigger(bool enable);
 	void keep_alive(bool signal_on);
 
-	int powerOn();
+	void turn_on_off(bool enable);
-	int powerOff();
 
 	void info();
 
@@ -29,7 +29,10 @@ public:
 private:
 	void setup();
 
-	param_t _p_pin;
+	param_t				_p_pin;
-	bool _camera_is_on;
+	orb_advert_t		_pwm_pub[2];
+	int					_orb_id[2];
+
+	bool				_camera_is_on;
 
 };