fixing cherry-picking divergences

ROMFS/px4fmu_common/init.d/rcS

@@ -537,7 +537,7 @@ then
 			# Get FMU driver out of the way
 			set MIXER_AUX none
 			set AUX_MODE none
-			camera_trigger start --pwm
+			camera_trigger start
 		fi
 	fi
 
@@ -606,7 +606,7 @@ then
 		# Try to get an USB console
 		nshterm /dev/ttyACM0 &
 	else
-		mavlink start -r 80000 -d /dev/ttyACM0 -m config -x
+		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
 	fi
 
 	#

src/drivers/camera_trigger/camera_trigger.cpp

@@ -36,7 +36,11 @@
  *
  * External camera-IMU synchronisation and triggering via FMU auxiliary pins.
  *
+ * Support for camera manipulation via PWM signal over servo pins.
+ *
  * @author Mohammed Kabir <mhkabir98@gmail.com>
+ * @author Kelly Steich <kelly.steich@wingtra.com>
+ * @author Andreas Bircher <andreas@wingtra.com>
  */
 
 #include <stdio.h>
@@ -63,10 +67,19 @@
 #include <drivers/drv_hrt.h>
 #include <board_config.h>
 
+#include "interfaces/src/pwm.h"
+#include "interfaces/src/relay.h"
+
 #define TRIGGER_PIN_DEFAULT 1
 
 extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
 
+typedef enum {
+	CAMERA_INTERFACE_MODE_NONE = 0,
+	CAMERA_INTERFACE_MODE_RELAY,
+	CAMERA_INTERFACE_MODE_PWM
+} camera_interface_mode_t;
+
 class CameraTrigger
 {
 public:
@@ -105,8 +118,6 @@ public:
 	 */
 	void		info();
 
-	int			_pins[6];
-
 private:
 
 	struct hrt_call		_engagecall;
@@ -115,7 +126,6 @@ private:
 
 	int 			_gpio_fd;
 
-	int 			_polarity;
 	int				_mode;
 	float 			_activation_time;
 	float  			_interval;
@@ -130,7 +140,6 @@ private:
 
 	orb_advert_t		_trigger_pub;
 
-	param_t _p_polarity;
 	param_t _p_mode;
 	param_t _p_activation_time;
 	param_t _p_interval;
@@ -153,12 +162,9 @@ private:
 	 */
 	static void	disengage(void *arg);
 
-	static void trigger(CameraTrigger *trig, bool trigger);
-
 };
 
 struct work_s CameraTrigger::_work;
-constexpr uint32_t CameraTrigger::_gpios[6];
 
 namespace camera_trigger
 {
@@ -170,7 +176,6 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 	_engagecall {},
 	_disengagecall {},
 	_gpio_fd(-1),
-	_polarity(0),
 	_mode(0),
 	_activation_time(0.5f /* ms */),
 	_interval(100.0f /* ms */),
@@ -185,44 +190,38 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 	_camera_interface_mode(camera_interface_mode),
 	_camera_interface(nullptr)
 {
+	//Initiate Camera interface basedon camera_interface_mode
+	if (_camera_interface != nullptr) {
+		delete(_camera_interface);
+		/* set to zero to ensure parser is not used while not instantiated */
+		_camera_interface = nullptr;
+	}
+
+	switch (_camera_interface_mode) {
+	case CAMERA_INTERFACE_MODE_RELAY:
+		_camera_interface = new CameraInterfaceRelay;
+		break;
+
+	case CAMERA_INTERFACE_MODE_PWM:
+		_camera_interface = new CameraInterfacePWM;
+		break;
+
+	default:
+		break;
+	}
+
 	memset(&_work, 0, sizeof(_work));
 
 	// Parameters
-	_p_polarity = param_find("TRIG_POLARITY");
 	_p_interval = param_find("TRIG_INTERVAL");
 	_p_distance = param_find("TRIG_DISTANCE");
 	_p_activation_time = param_find("TRIG_ACT_TIME");
 	_p_mode = param_find("TRIG_MODE");
-	_p_pin = param_find("TRIG_PINS");
 
-	param_get(_p_polarity, &_polarity);
 	param_get(_p_activation_time, &_activation_time);
 	param_get(_p_interval, &_interval);
 	param_get(_p_distance, &_distance);
 	param_get(_p_mode, &_mode);
-	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++;
-	}
 
 	struct camera_trigger_s	camera_trigger = {};
 
@@ -278,12 +277,6 @@ CameraTrigger::shootOnce()
 void
 CameraTrigger::start()
 {
-
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		px4_arch_configgpio(_gpios[_pins[i]]);
-		px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
-	}
-
 	// enable immediate if configured that way
 	if (_mode == 2) {
 		control(true);
@@ -438,23 +431,10 @@ CameraTrigger::disengage(void *arg)
 	trig->_camera_interface->trigger(false);
 }
 
-void
-CameraTrigger::trigger(CameraTrigger *trig, bool trigger)
-{
-	for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
-		if (trig->_pins[i] >= 0) {
-			// ACTIVE_LOW == 1
-			px4_arch_gpiowrite(trig->_gpios[trig->_pins[i]], trigger);
-		}
-	}
-}
-
 void
 CameraTrigger::info()
 {
 	warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
-	warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
-	      _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
 	warnx("mode : %i", _mode);
 	warnx("interval : %.2f [ms]", (double)_interval);
 	warnx("distance : %.2f [m]", (double)_distance);

src/drivers/camera_trigger/interfaces/src/camera_interface.cpp

@@ -0,0 +1,14 @@
+#include "camera_interface.h"
+
+/**
+ * @file camera_interface.cpp
+ *
+ */
+
+CameraInterface::CameraInterface()
+{
+}
+
+CameraInterface::~CameraInterface()
+{
+}

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

@@ -1,7 +1,8 @@
 #include <px4.h>
 #include <sys/ioctl.h>
-#include <lib/helpfuncs/helpfuncs.h>
+#include <lib/mathlib/mathlib.h>
 
+#include "drivers/drv_pwm_output.h"
 #include "pwm.h"
 
 // PWM levels of the interface to seagull MAP converter to
@@ -41,6 +42,7 @@ CameraInterfacePWM::CameraInterfacePWM():
 		pin_list /= 10;
 		i++;
 	}
+
 	setup();
 }
 
@@ -118,7 +120,7 @@ int CameraInterfacePWM::powerOff()
 	return 0;
 }
 
-void CameraInterfaceRelay::info()
+void CameraInterfacePWM::info()
 {
 	warnx("PWM - camera triggering, pins 1-3 : %d,%d,%d", _pins[0], _pins[1], _pins[2]);
 }

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

@@ -11,20 +11,6 @@
 #include <uORB/topics/vehicle_status.h>
 #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
 {
 public:

src/drivers/camera_trigger/interfaces/src/relay.cpp

@@ -45,8 +45,8 @@ CameraInterfaceRelay::~CameraInterfaceRelay()
 void CameraInterfaceRelay::setup()
 {
 	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		stm32_configgpio(_gpios[_pins[i]]);
+		px4_arch_configgpio(_gpios[_pins[i]]);
-		stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
+		px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
 	}
 }
 
@@ -56,7 +56,7 @@ void CameraInterfaceRelay::trigger(bool enable)
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 			if (_pins[i] >= 0) {
 				// ACTIVE_LOW == 1
-				stm32_gpiowrite(_gpios[_pins[i]], _polarity);
+				px4_arch_gpiowrite(_gpios[_pins[i]], _polarity);
 			}
 		}
 
@@ -64,7 +64,7 @@ void CameraInterfaceRelay::trigger(bool enable)
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 			if (_pins[i] >= 0) {
 				// ACTIVE_LOW == 1
-				stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
+				px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
 			}
 		}
 	}