From c97226b9da794df167e166707a3f779886887532 Mon Sep 17 00:00:00 2001
From: Mohammed Kabir <kabir@uasys.io>
Date: Tue, 11 Apr 2017 23:29:32 +0200
Subject: [PATCH] camera_trigger : add generic pwm interface for servo-like
 trigger systems (e.g IR trigger)

---
 src/drivers/camera_trigger/CMakeLists.txt     |  1 +
 .../camera_trigger/interfaces/src/pwm.cpp     | 90 +++++++++++++++++++
 .../camera_trigger/interfaces/src/pwm.h       | 35 ++++++++
 .../interfaces/src/seagull_map2.cpp           |  3 +-
 4 files changed, 128 insertions(+), 1 deletion(-)
 create mode 100644 src/drivers/camera_trigger/interfaces/src/pwm.cpp
 create mode 100644 src/drivers/camera_trigger/interfaces/src/pwm.h

diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt
index e72ebba712..4a886d5663 100644
--- a/src/drivers/camera_trigger/CMakeLists.txt
+++ b/src/drivers/camera_trigger/CMakeLists.txt
@@ -38,6 +38,7 @@ px4_add_module(
 	SRCS
 		camera_trigger.cpp
                 interfaces/src/camera_interface.cpp
+                interfaces/src/pwm.cpp
                 interfaces/src/seagull_map2.cpp
                 interfaces/src/gpio.cpp
 	DEPENDS
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
new file mode 100644
index 0000000000..e60b419347
--- /dev/null
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -0,0 +1,90 @@
+#ifdef __PX4_NUTTX
+
+#include <sys/ioctl.h>
+#include <lib/mathlib/mathlib.h>
+
+#include "drivers/drv_pwm_trigger.h"
+#include "pwm.h"
+
+// TODO : make these parameters later
+#define PWM_CAMERA_SHOOT 1900
+#define PWM_CAMERA_NEUTRAL 1500
+
+CameraInterfacePWM::CameraInterfacePWM():
+	CameraInterface()
+{
+	_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] = -1;
+		}
+
+		pin_list /= 10;
+		i++;
+	}
+
+	setup();
+}
+
+CameraInterfacePWM::~CameraInterfacePWM()
+{
+	// Deinitialise trigger channels
+	up_pwm_trigger_deinit();
+}
+
+void CameraInterfacePWM::setup()
+{
+	// Precompute the bitmask for enabled channels
+	uint8_t pin_bitmask = 0;
+
+	for (unsigned i = 0; i < arraySize(_pins); i++) {
+		if (_pins[i] >= 0) {
+			pin_bitmask |= (1 << _pins[i]);
+		}
+	}
+
+	// Initialize and arm channels
+	up_pwm_trigger_init(pin_bitmask);
+
+	// Set neutral pulsewidths
+	for (unsigned i = 0; i < arraySize(_pins); i++) {
+		if (_pins[i] >= 0) {
+			up_pwm_trigger_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, PWM_CAMERA_NEUTRAL, 2000));
+		}
+	}
+
+}
+
+void CameraInterfacePWM::trigger(bool enable)
+{
+	for (unsigned i = 0; i < arraySize(_pins); i++) {
+		if (_pins[i] >= 0) {
+			// Set all valid pins to shoot or neutral levels
+			up_pwm_trigger_set(_pins[i], math::constrain(enable ? PWM_CAMERA_SHOOT : PWM_CAMERA_NEUTRAL, 1000, 2000));
+		}
+	}
+}
+
+void CameraInterfacePWM::info()
+{
+	// TODO : cleanup this output for all trigger types
+	PX4_INFO("PWM trigger mode (generic), pins enabled : [%d][%d][%d][%d][%d][%d]",
+		 _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]);
+}
+
+#endif /* ifdef __PX4_NUTTX */
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
new file mode 100644
index 0000000000..3ad24d701f
--- /dev/null
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -0,0 +1,35 @@
+/**
+ * @file pwm.h
+ *
+ * Interface with cameras via pwm.
+ *
+ */
+#pragma once
+
+#ifdef __PX4_NUTTX
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/param/param.h>
+#include <px4_log.h>
+
+#include "camera_interface.h"
+
+class CameraInterfacePWM : public CameraInterface
+{
+public:
+	CameraInterfacePWM();
+	virtual ~CameraInterfacePWM();
+
+	void trigger(bool enable);
+
+	void info();
+
+	int _pins[6];
+private:
+	void setup();
+
+	param_t _p_pin;
+
+};
+
+#endif /* ifdef __PX4_NUTTX */
diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp
index 3837129f76..b6f58a90a6 100644
--- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp
@@ -17,6 +17,8 @@
 #define PWM_2_CAMERA_KEEP_ALIVE		1700
 #define PWM_2_CAMERA_ON_OFF			1900
 
+// TODO : cleanup using arraySize() macro
+
 CameraInterfaceSeagull::CameraInterfaceSeagull():
 	CameraInterface(),
 	_camera_is_on(false)
@@ -69,7 +71,6 @@ void CameraInterfaceSeagull::setup()
 
 void CameraInterfaceSeagull::trigger(bool enable)
 {
-	// This only starts working upon prearming
 
 	if (!_camera_is_on) {
 		return;