adding the initial pwm trigger logic

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

@@ -1,8 +1,33 @@
 #include "pwm.h"
 
 CameraInterfacePWM::CameraInterfacePWM():
-	CameraInterface()
+	CameraInterface(),
+	_camera_is_on(false)
 {
+	_p_pin = param_find("TRIG_PINS");
+	int pin_list;
+	param_get(_p_pin, &pin_list);
+
+	// Set all pins as invalid
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		_pins[i] = -1;
+	}
+
+	// Convert number to individual channels
+	unsigned i = 0;
+	int single_pin;
+
+	while ((single_pin = pin_list % 10)) {
+
+		_pins[i] = single_pin - 1;
+
+		if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
+			_pins[i] = -1;
+		}
+
+		pin_list /= 10;
+		i++;
+	}
 }
 
 CameraInterfacePWM::~CameraInterfacePWM()
@@ -11,20 +36,100 @@ CameraInterfacePWM::~CameraInterfacePWM()
 
 void CameraInterfacePWM::setup()
 {
+	_pwm_dev = PWM_DEVICE_PATH;  // Used for direct pwm output without mixer
+	_pwm_fd = open(_pwm_dev, 0);  // open pwm device
 
+	if (_pwm_fd < 0) {
+		err(1, "can't open %s", _pwm_dev);
+	}
+
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		if (_pin[i] >= 0) {
+			int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+		}
+	}
 }
 
 void CameraInterfacePWM::trigger(bool enable)
 {
+	// Check if armed, otherwise don't send high PWM values
+	if (_disarmed) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			}
+		}
 
+	} else {
+		if (!_camera_is_on) {
+			// Turn camera on and give time to start up
+			powerOn();
+			return;
+		}
+
+		if (trig) {
+			// Set all valid pins to shoot level
+			for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+				if (_pin[i] >= 0) {
+					int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), 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 (_pin[i] >= 0) {
+					int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+				}
+			}
+		}
+	}
 }
 
 int CameraInterfacePWM::powerOn()
 {
+	// Check if armed, otherwise don't send high PWM values
+	if (_disarmed) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			}
+		}
+
+	} else {
+		// Set all valid pins to turn on level
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000));
+			}
+		}
+
+		_camera_is_on = true;
+	}
+
 	return 0;
 }
 
 int CameraInterfacePWM::powerOff()
 {
+	// Check if armed, otherwise don't send high PWM values
+	if (_disarmed) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			}
+		}
+
+	} else {
+		// Set all valid pins to turn off level
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+			}
+		}
+
+		_camera_is_on = false;
+	}
+
 	return 0;
 }

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

@@ -8,6 +8,19 @@
 
 #include "camera_interface.h"
 
+// TODO(birchera): check if this is the right device and addresses
+#define PWM_DEVICE_PATH /dev/pwm_output0
+#define PWM_CAMERA_BASE		0x2a00
+#define PWM_CAMERA_SET(_pin)	_PX4_IOC(_PWM_CAMERA_BASE, 0x30 + _pin)
+// 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_ON 1100
+#define PWM_CAMERA_AUTOFOCUS_SHOOT 1300
+#define PWM_CAMERA_NEUTRAL 1500
+#define PWM_CAMERA_INSTANT_SHOOT 1700
+#define PWM_CAMERA_OFF 1900
+
 
 class CameraInterfacePWM : public CameraInterface
 {
@@ -22,6 +35,12 @@ public:
 	int powerOn();
 	int powerOff();
 
+	int _pins[6];
 private:
 
+	param_t _p_pin;
+	const char *_pwm_dev;
+	int _pwm_fd;
+	bool _camera_is_on;
+
 };