Tease the PWM driver out and fix some build issues after cleaning up behind the cpuload pieces.

5 files changed, 455 insertions, 4 deletions

diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c
index c35290589..bb30087e0 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_init.c
+++ b/apps/drivers/boards/px4fmu/px4fmu_init.c
@@ -63,7 +63,6 @@
 #include "px4fmu_internal.h"
 #include "stm32_uart.h"
 
-#include <arch/board/up_cpuload.h>
 #include <arch/board/up_adc.h>
 #include <arch/board/board.h>
 #include <arch/board/drv_led.h>
@@ -71,6 +70,8 @@
 
 #include <drivers/drv_hrt.h>
 
+#include <systemlib/cpuload.h>
+
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index 340175a4b..fc458e9b0 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -41,14 +41,15 @@
  * channel.
  */
 
-#ifndef _DRV_PWM_OUTPUT_H
-#define _DRV_PWM_OUTPUT_H
+#pragma once
 
 #include <stdint.h>
 #include <sys/ioctl.h>
 
 #include "drv_orb_dev.h"
 
+__BEGIN_DECLS
+
 /**
  * Path for the default PWM output device.
  *
@@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_GET(_servo)	_IOC(_PWM_SERVO_BASE, 0x40 + _servo)
 
 
-#endif /* _DRV_PWM_OUTPUT_H */
+/*
+ * Low-level PWM output interface.
+ *
+ * This is the low-level API to the platform-specific PWM driver.
+ */
+
+/**
+ * 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_servo_init(uint32_t channel_mask);
+
+/**
+ * De-initialise the PWM servo outputs.
+ */
+__EXPORT extern void	up_pwm_servo_deinit(void);
+
+/**
+ * Arm or disarm servo outputs.
+ *
+ * When disarmed, servos output no pulse.
+ *
+ * @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_servo_arm(bool armed);
+
+/**
+ * Set the servo update rate
+ *
+ * @param rate		The update rate in Hz to set.
+ * @return		OK on success, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate(unsigned rate);
+
+/**
+ * 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_servo_set(unsigned channel, servo_position_t value);
+
+/**
+ * Get the current output value for a channel.
+ *
+ * @param channel	The channel to read.
+ * @return		The output pulse width in microseconds, or zero if
+ *			outputs are not armed or not configured.
+ */
+__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
+
+__END_DECLS
diff --git a/apps/drivers/stm32/Makefile b/apps/drivers/stm32/Makefile
new file mode 100644
index 000000000..4ad57f413
--- /dev/null
+++ b/apps/drivers/stm32/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# STM32 driver support code
+#
+# Modules in this directory are compiled for all STM32 targets.
+#
+
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/boards/px4fmu/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c
index 11b98fd1b..11b98fd1b 100644
--- a/apps/drivers/boards/px4fmu/drv_hrt.c
+++ b/apps/drivers/stm32/drv_hrt.c
diff --git a/apps/drivers/stm32/up_pwm_servo.c b/apps/drivers/stm32/up_pwm_servo.c
new file mode 100644
index 000000000..708aa8884
--- /dev/null
+++ b/apps/drivers/stm32/up_pwm_servo.c
@@ -0,0 +1,348 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_servo.c
+ *
+ * Servo driver supporting PWM servos connected to STM32 timer blocks.
+ *
+ * Works with any of the 'generic' or 'advanced' STM32 timers that
+ * have output pins, does not require an interrupt.
+ */
+
+#include <nuttx/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_output.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+/* configuration limits */
+#define PWM_SERVO_MAX_TIMERS	2
+#define PWM_SERVO_MAX_CHANNELS	8
+
+/* default rate (in Hz) of PWM updates */
+static uint32_t	pwm_update_rate = 50;
+
+/*
+ * Servo configuration for all of the pins that can be used as
+ * PWM outputs on FMU.
+ */
+
+/* array of timers dedicated to PWM servo use */
+static const struct pwm_servo_timer {
+	uint32_t	base;
+	uint32_t	clock_register;
+	uint32_t	clock_bit;
+	uint32_t	clock_freq;
+} pwm_timers[] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	}
+};
+
+/* array of channels in logical order */
+static const struct pwm_servo_channel {
+	uint32_t	gpio;
+	uint8_t		timer_index;
+	uint8_t		timer_channel;
+	servo_position_t default_value;
+} pwm_channels[] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
+
+
+#define REG(_tmr, _reg)	(*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
+
+#define rCR1(_tmr)    	REG(_tmr, STM32_GTIM_CR1_OFFSET)
+#define rCR2(_tmr)    	REG(_tmr, STM32_GTIM_CR2_OFFSET)
+#define rSMCR(_tmr)   	REG(_tmr, STM32_GTIM_SMCR_OFFSET)
+#define rDIER(_tmr)   	REG(_tmr, STM32_GTIM_DIER_OFFSET)
+#define rSR(_tmr)     	REG(_tmr, STM32_GTIM_SR_OFFSET)
+#define rEGR(_tmr)    	REG(_tmr, STM32_GTIM_EGR_OFFSET)
+#define rCCMR1(_tmr)  	REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2(_tmr)  	REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
+#define rCCER(_tmr)   	REG(_tmr, STM32_GTIM_CCER_OFFSET)
+#define rCNT(_tmr)    	REG(_tmr, STM32_GTIM_CNT_OFFSET)
+#define rPSC(_tmr)    	REG(_tmr, STM32_GTIM_PSC_OFFSET)
+#define rARR(_tmr)    	REG(_tmr, STM32_GTIM_ARR_OFFSET)
+#define rCCR1(_tmr)   	REG(_tmr, STM32_GTIM_CCR1_OFFSET)
+#define rCCR2(_tmr)   	REG(_tmr, STM32_GTIM_CCR2_OFFSET)
+#define rCCR3(_tmr)   	REG(_tmr, STM32_GTIM_CCR3_OFFSET)
+#define rCCR4(_tmr)   	REG(_tmr, STM32_GTIM_CCR4_OFFSET)
+#define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
+#define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+
+static void
+pwm_timer_init(unsigned timer)
+{
+	/* enable the timer clock before we try to talk to it */
+	modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit);
+
+	/* disable and configure the timer */
+	rCR1(timer) = 0;
+	rCR2(timer) = 0;
+	rSMCR(timer) = 0;
+	rDIER(timer) = 0;
+	rCCER(timer) = 0;
+	rCCMR1(timer) = 0;
+	rCCMR2(timer) = 0;
+	rCCER(timer) = 0;
+	rDCR(timer) = 0;
+
+	/* configure the timer to free-run at 1MHz */
+	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
+
+	/* and update at the desired rate */
+	rARR(timer) = (1000000 / pwm_update_rate) - 1;
+
+	/* generate an update event; reloads the counter and all registers */
+	rEGR(timer) = GTIM_EGR_UG;
+
+	/* note that the timer is left disabled - arming is performed separately */
+}
+
+static void
+pwm_timer_set_rate(unsigned timer, unsigned rate)
+{
+	/* configure the timer to update at the desired rate */
+	rARR(timer) = 1000000 / rate;
+
+	/* generate an update event; reloads the counter and all registers */
+	rEGR(timer) = GTIM_EGR_UG;
+}
+
+static void
+pwm_channel_init(unsigned channel)
+{
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* configure the GPIO first */
+	stm32_configgpio(pwm_channels[channel].gpio);
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+		case 1:
+			rCCMR1(timer) |= (6 << 4);
+			rCCR1(timer) = pwm_channels[channel].default_value;
+			rCCER(timer) |= (1 << 0);
+			break;
+		case 2:
+			rCCMR1(timer) |= (6 << 12);
+			rCCR2(timer) = pwm_channels[channel].default_value;
+			rCCER(timer) |= (1 << 4);
+			break;
+		case 3:
+			rCCMR2(timer) |= (6 << 4);
+			rCCR3(timer) = pwm_channels[channel].default_value;
+			rCCER(timer) |= (1 << 8);
+			break;
+		case 4:
+			rCCMR2(timer) |= (6 << 12);
+			rCCR4(timer) = pwm_channels[channel].default_value;
+			rCCER(timer) |= (1 << 12);
+			break;
+	}
+}
+
+int
+up_pwm_servo_set(unsigned channel, servo_position_t value)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS) {
+		lldbg("pwm_channel_set: bogus channel %u\n", channel);
+		return -1;
+	}
+
+	unsigned timer = pwm_channels[channel].timer_index;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].gpio == 0))
+		return -1;
+
+	/* configure the channel */
+	if (value > 0)
+		value--;
+	switch (pwm_channels[channel].timer_channel) {
+		case 1:
+			rCCR1(timer) = value;
+			break;
+		case 2:
+			rCCR2(timer) = value;
+			break;
+		case 3:
+			rCCR3(timer) = value;
+			break;
+		case 4:
+			rCCR4(timer) = value;
+			break;
+		default:
+			return -1;
+	}
+	return 0;
+}
+
+servo_position_t
+up_pwm_servo_get(unsigned channel)
+{
+	if (channel >= PWM_SERVO_MAX_CHANNELS) {
+		lldbg("pwm_channel_get: bogus channel %u\n", channel);
+		return 0;
+	}
+
+	unsigned timer = pwm_channels[channel].timer_index;
+	servo_position_t value = 0;
+
+	/* test timer for validity */
+	if ((pwm_timers[timer].base == 0) ||
+	    (pwm_channels[channel].gpio == 0))
+		return 0;
+
+	/* configure the channel */
+	switch (pwm_channels[channel].timer_channel) {
+		case 1:
+			value = rCCR1(timer);
+			break;
+		case 2:
+			value = rCCR2(timer);
+			break;
+		case 3:
+			value = rCCR3(timer);
+			break;
+		case 4:
+			value = rCCR4(timer);
+			break;
+	}
+	return value;
+}
+
+int
+up_pwm_servo_init(uint32_t channel_mask)
+{
+	/* do basic timer initialisation first */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			pwm_timer_init(i);
+	}
+
+	/* now init channels */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
+		/* don't do init for disabled channels; this leaves the pin configs alone */
+		if (((1<<i) & channel_mask) && (pwm_channels[i].gpio != 0))
+			pwm_channel_init(i);
+	}
+	return OK;
+}
+
+void
+up_pwm_servo_deinit(void)
+{
+	/* disable the timers */
+	up_pwm_servo_arm(false);
+}
+
+int
+up_pwm_servo_set_rate(unsigned rate)
+{
+	if ((rate < 50) || (rate > 400))
+		return -ERANGE;
+
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			pwm_timer_set_rate(i, rate);
+	}
+	return OK;
+}
+
+void
+up_pwm_servo_arm(bool armed)
+{
+	/* 
+	 * XXX this is inelgant and in particular will either jam outputs at whatever level
+	 * they happen to be at at the time the timers stop or generate runts.
+	 * The right thing is almost certainly to kill auto-reload on the timers so that 
+	 * they just stop at the end of their count for disable, and to reset/restart them 
+	 * for enable.
+	 */
+
+	/* iterate timers and arm/disarm appropriately */
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
+		if (pwm_timers[i].base != 0)
+			rCR1(i) = armed ? GTIM_CR1_CEN : 0;
+	}
+}