stm32 : add new interface for PWM-based camera_trigger

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

@@ -3,7 +3,7 @@
 #include <sys/ioctl.h>
 #include <lib/mathlib/mathlib.h>
 
-#include "drivers/drv_pwm_output.h"
+#include "drivers/drv_pwm_trigger.h"
 #include "pwm.h"
 
 // PWM levels of the interface to seagull MAP converter to
@@ -52,8 +52,7 @@ CameraInterfacePWM::CameraInterfacePWM():
 CameraInterfacePWM::~CameraInterfacePWM()
 {
 	// Deinitialise pwm channels
-	// Can currently not be used, because up_pwm_servo_deinit() will deinitialise all pwm channels
-	// up_pwm_servo_deinit();
+	up_pwm_trigger_deinit();
 }
 
 void CameraInterfacePWM::setup()
@@ -61,9 +60,9 @@ void CameraInterfacePWM::setup()
 	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
 		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
 			uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
-			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));
+			up_pwm_trigger_init(pin_bitmask);
+			up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
+			up_pwm_trigger_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
 		}
 	}
 }
@@ -80,7 +79,7 @@ void CameraInterfacePWM::trigger(bool 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));
+				up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
 			}
 		}
 
@@ -88,7 +87,7 @@ void CameraInterfacePWM::trigger(bool enable)
 		// Set all valid pins back to neutral 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_NEUTRAL, 1000, 2000));
+				up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 			}
 		}
 	}
@@ -106,7 +105,7 @@ void CameraInterfacePWM::keep_alive(bool 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));
+				up_pwm_trigger_set(_pins[i], math::constrain(PWM_2_CAMERA_KEEP_ALIVE, 1000, 2000));
 			}
 		}
 
@@ -114,7 +113,7 @@ void CameraInterfacePWM::keep_alive(bool signal_on)
 		// Set channel 2 pin to neutral 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_CAMERA_NEUTRAL, 1000, 2000));
+				up_pwm_trigger_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 			}
 		}
 	}
@@ -126,8 +125,8 @@ void CameraInterfacePWM::turn_on_off(bool enable)
 		// 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_2_CAMERA_ON_OFF, 1000, 2000));
+				up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+				up_pwm_trigger_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
 			}
 		}
 
@@ -135,8 +134,8 @@ void CameraInterfacePWM::turn_on_off(bool enable)
 		// 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));
+				up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+				up_pwm_trigger_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 			}
 		}
 
@@ -146,7 +145,7 @@ void CameraInterfacePWM::turn_on_off(bool enable)
 
 void CameraInterfacePWM::info()
 {
-	warnx("PWM - interface, pin config: %d,%d,%d", _pins[0] + 1, _pins[1] + 1, _pins[2] + 1);
+	warnx("PWM trigger mode - pin config: %d,%d,%d", _pins[0] + 1, _pins[1] + 1, _pins[2] + 1);
 }
 
 #endif /* ifdef __PX4_NUTTX */

src/drivers/drv_pwm_trigger.h

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PWM trigger output interface.
+ *
+ */
+
+#pragma once
+
+#include <px4_defines.h>
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/**
+ * Intialise the PWM servo outputs using the specified configuration.
+ *
+ * @param channel_mask	Bitmask of channels (LSB = channel 0) to enable.
+ *			This allows some of the channels to remain configured
+ *			as GPIOs or as another function.
+ * @return		OK on success.
+ */
+__EXPORT extern int	up_pwm_trigger_init(uint32_t channel_mask);
+
+/**
+ * De-initialise the PWM trigger outputs.
+ */
+__EXPORT extern void	up_pwm_trigger_deinit(void);
+
+/**
+ * Arm or disarm trigger outputs.
+ *
+ * @bug This function should, but does not, guarantee that any pulse
+ *      currently in progress is cleanly completed.
+ *
+ * @param armed		If true, outputs are armed; if false they
+ *			are disarmed.
+ */
+__EXPORT extern void	up_pwm_trigger_arm(bool armed);
+
+/**
+ * Set the current output value for a channel.
+ *
+ * @param channel	The channel to set.
+ * @param value		The output pulse width in microseconds.
+ */
+__EXPORT extern int	up_pwm_trigger_set(unsigned channel, uint16_t value);
+
+__END_DECLS

src/drivers/stm32/CMakeLists.txt

@@ -37,6 +37,7 @@ px4_add_module(
 		drv_hrt.c
 		drv_io_timer.c
 		drv_pwm_servo.c
+		drv_pwm_trigger.c
 		drv_input_capture.c
 		drv_led_pwm.cpp
 	DEPENDS

src/drivers/stm32/drv_io_timer.c

@@ -578,6 +578,7 @@ int io_timer_set_rate(unsigned timer, unsigned rate)
 	/* Check that all channels are either in PWM or Oneshot */
 
 	if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] |
+			 channel_allocations[IOTimerChanMode_Trigger] |
 			 channel_allocations[IOTimerChanMode_OneShot] |
 			 channel_allocations[IOTimerChanMode_NotUsed])) ==
 	    channels) {
@@ -637,6 +638,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 	// intentional fallthrough
 	case IOTimerChanMode_OneShot:
 	case IOTimerChanMode_PWMOut:
+	case IOTimerChanMode_Trigger:
 		ccer_setbits = 0;
 		dier_setbits = 0;
 		setbits = CCMR_C1_PWMOUT_INIT;
@@ -744,8 +746,9 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 
 	switch (mode) {
 	case IOTimerChanMode_NotUsed:
-	case IOTimerChanMode_PWMOut:
 	case IOTimerChanMode_OneShot:
+	case IOTimerChanMode_PWMOut:
+	case IOTimerChanMode_Trigger:
 		dier_bit = 0;
 		break;
 
@@ -788,7 +791,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 			action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE  << shifts;
 			action_cache[timer].dier_setbits   |= dier_bit << shifts;
 
-			if ((state && (mode == IOTimerChanMode_PWMOut || mode == IOTimerChanMode_OneShot))) {
+			if ((state && (mode == IOTimerChanMode_PWMOut || mode == IOTimerChanMode_OneShot || mode == IOTimerChanMode_Trigger))) {
 				action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
 			}
 		}
@@ -846,7 +849,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value)
 	int mode = io_timer_get_channel_mode(channel);
 
 	if (rv == 0) {
-		if ((mode != IOTimerChanMode_PWMOut) && (mode != IOTimerChanMode_OneShot)) {
+		if ((mode != IOTimerChanMode_PWMOut) && (mode != IOTimerChanMode_OneShot) && (mode != IOTimerChanMode_Trigger)) {
 
 			rv = -EIO;
 
@@ -868,7 +871,7 @@ uint16_t io_channel_get_ccr(unsigned channel)
 	if (io_timer_validate_channel_index(channel) == 0) {
 		int mode = io_timer_get_channel_mode(channel);
 
-		if ((mode == IOTimerChanMode_PWMOut) || (mode == IOTimerChanMode_OneShot)) {
+		if ((mode == IOTimerChanMode_PWMOut) || (mode == IOTimerChanMode_OneShot) || (mode == IOTimerChanMode_Trigger)) {
 			value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset);
 		}
 	}

src/drivers/stm32/drv_io_timer.h

@@ -59,6 +59,7 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_PWMIn   = 2,
 	IOTimerChanMode_Capture = 3,
 	IOTimerChanMode_OneShot = 4,
+	IOTimerChanMode_Trigger = 5,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 

src/drivers/stm32/drv_pwm_servo.c

@@ -87,6 +87,7 @@ int up_pwm_servo_init(uint32_t channel_mask)
 		}
 	}
 
+
 	// Now allocate the new set
 
 	for (unsigned channel = 0; channel_mask != 0 &&  channel < MAX_TIMER_IO_CHANNELS; channel++) {

src/drivers/stm32/drv_pwm_trigger.c

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file drv_pwm_trigger.c
+ *
+ */
+
+#include <px4_config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_trigger.h>
+
+#include "drv_io_timer.h"
+#include "drv_pwm_trigger.h"
+
+#include <stm32_tim.h>
+
+int up_pwm_trigger_set(unsigned channel, uint16_t value)
+{
+	return io_timer_set_ccr(channel, value);
+}
+
+int up_pwm_trigger_init(uint32_t channel_mask)
+{
+	/* Init channels */
+	for (unsigned channel = 0; channel_mask != 0 &&  channel < MAX_TIMER_IO_CHANNELS; channel++) {
+		if (channel_mask & (1 << channel)) {
+
+			// First free any that were not trigger mode before
+			if (-EBUSY == io_timer_is_channel_free(channel)) {
+				io_timer_free_channel(channel);
+				PX4_INFO("FREE PIN");
+			}
+
+			io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL);
+			channel_mask &= ~(1 << channel);
+			PX4_INFO("SET PIN TO TRIGGER");
+		}
+	}
+
+	/* enable the timers */
+	up_pwm_trigger_arm(true);
+
+	return OK;
+}
+
+void up_pwm_trigger_deinit(void)
+{
+	/* disable the timers */
+	up_pwm_trigger_arm(false);
+}
+
+void
+up_pwm_trigger_arm(bool armed)
+{
+	io_timer_set_enable(armed, IOTimerChanMode_Trigger, IO_TIMER_ALL_MODES_CHANNELS);
+	PX4_INFO("ENABLE PIN : %d", (int)armed);
+}

src/drivers/stm32/drv_pwm_trigger.h

@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_pwm_trigger.h
+ *
+ * stm32-specific PWM output data.
+ */
+
+#pragma once
+
+#include <drivers/drv_pwm_trigger.h>