From 4e5e0c69219043faef97e25dfdb71fcab0350835 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Thu, 14 Jun 2018 08:43:44 -1000
Subject: [PATCH] camera_trigger:Refacter GPIO camera triggering

   Refactored for efficiency and simplicity.
---
 .../camera_trigger/camera_trigger_params.c    |  3 ++
 .../camera_trigger/interfaces/src/gpio.cpp    | 47 ++++++++++---------
 .../camera_trigger/interfaces/src/gpio.h      |  3 +-
 3 files changed, 30 insertions(+), 23 deletions(-)

diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c
index 69a158ff86..61d7d240f7 100644
--- a/src/drivers/camera_trigger/camera_trigger_params.c
+++ b/src/drivers/camera_trigger/camera_trigger_params.c
@@ -115,6 +115,9 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
  * 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 56 would trigger on pins 5 and 6.
+ * For GPIO mode Pin 6 will be triggered followed by 5. With a value of 65 pin 5 will
+ * be triggered followed by 6. Pins may be non contiguous. I.E. 16 or 61.
+ * for GPIO mode the delay pin to pin is < .2 uS.
  *
  * @min 1
  * @max 123456
diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp
index 8231fad181..8cdb2528af 100644
--- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/gpio.cpp
@@ -1,15 +1,20 @@
 #ifdef __PX4_NUTTX
 
 #include "gpio.h"
+#include <cstring>
 
 constexpr uint32_t CameraInterfaceGPIO::_gpios[6];
 
 CameraInterfaceGPIO::CameraInterfaceGPIO():
 	CameraInterface(),
-	_polarity(0)
+	_trigger_invert(false)
 {
 	_p_polarity = param_find("TRIG_POLARITY");
-	param_get(_p_polarity, &_polarity);
+
+	// polarity of the trigger (0 = active low, 1 = active high )
+	int polarity;
+	param_get(_p_polarity, &polarity);
+	_trigger_invert = (polarity == 0);
 
 	get_pins();
 	setup();
@@ -21,32 +26,30 @@ CameraInterfaceGPIO::~CameraInterfaceGPIO()
 
 void CameraInterfaceGPIO::setup()
 {
-	for (unsigned i = 0; i < arraySize(_pins); i++) {
-		// Pin range is from 0 to 5
+	memset(_triggers, 0, sizeof(_triggers));
+
+	for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) {
+
+		// Pin range is from 1 to 6, indexes are 0 to 5
+
 		if (_pins[i] >= 0 && _pins[i] < (int)arraySize(_gpios)) {
-			px4_arch_configgpio(_gpios[_pins[i]]);
-			px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
+			uint32_t gpio = _gpios[_pins[i]];
+			_triggers[t++] = gpio;
+			px4_arch_configgpio(gpio);
+			px4_arch_gpiowrite(gpio, false ^ _trigger_invert);
 		}
 	}
 }
 
-void CameraInterfaceGPIO::trigger(bool enable)
+void CameraInterfaceGPIO::trigger(bool trigger_on_true)
 {
-	if (enable) {
-		for (unsigned i = 0; i < arraySize(_pins); i++) {
-			if (_pins[i] >= 0 && _pins[i] < (int)arraySize(_gpios)) {
-				// ACTIVE_LOW == 1
-				px4_arch_gpiowrite(_gpios[_pins[i]], _polarity);
-			}
-		}
+	bool trigger_state = trigger_on_true ^ _trigger_invert;
 
-	} else {
-		for (unsigned i = 0; i < arraySize(_pins); i++) {
-			if (_pins[i] >= 0 && _pins[i] < (int)arraySize(_gpios)) {
-				// ACTIVE_LOW == 1
-				px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
-			}
-		}
+	for (unsigned i = 0; i < arraySize(_triggers); i++) {
+
+		if (_triggers[i] == 0) { break; }
+
+		px4_arch_gpiowrite(_triggers[i], trigger_state);
 	}
 }
 
@@ -54,7 +57,7 @@ void CameraInterfaceGPIO::info()
 {
 	PX4_INFO("GPIO trigger mode, pins enabled : [%d][%d][%d][%d][%d][%d], polarity : %s",
 		 _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0],
-		 _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
+		 _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH");
 }
 
 #endif /* ifdef __PX4_NUTTX */
diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.h b/src/drivers/camera_trigger/interfaces/src/gpio.h
index eb2cc3a32e..d4d9cbf005 100644
--- a/src/drivers/camera_trigger/interfaces/src/gpio.h
+++ b/src/drivers/camera_trigger/interfaces/src/gpio.h
@@ -28,7 +28,7 @@ private:
 
 	param_t _p_polarity;
 
-	int _polarity;
+	bool _trigger_invert;
 
 	static constexpr uint32_t _gpios[6] = {
 		GPIO_GPIO0_OUTPUT,
@@ -39,6 +39,7 @@ private:
 		GPIO_GPIO5_OUTPUT
 	};
 
+	uint32_t _triggers[arraySize(_gpios) + 1];
 };
 
 #endif /* ifdef __PX4_NUTTX */