Fresh import of the PX4 firmware sources.

1 files changed, 511 insertions, 0 deletions

diff --git a/nuttx/configs/px4fmu/src/drv_tone_alarm.c b/nuttx/configs/px4fmu/src/drv_tone_alarm.c
new file mode 100644
index 000000000..958a18904
--- /dev/null
+++ b/nuttx/configs/px4fmu/src/drv_tone_alarm.c
@@ -0,0 +1,511 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/*
+ * Speaker driver supporting alarm sequences.
+ *
+ * Works with any of the 'generic' STM32 timers that has an output
+ * pin, does not require an interrupt.
+ *
+ * Depends on the HRT timer.
+ */
+
+#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 <arch/board/drv_tone_alarm.h>
+#include <arch/board/up_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+#ifdef CONFIG_TONE_ALARM
+# ifndef CONFIG_HRT_TIMER
+#  error CONFIG_TONE_ALARM requires CONFIG_HRT_TIMER
+# endif
+
+/* Tone alarm configuration */
+#if   TONE_ALARM_TIMER == 2
+# define TONE_ALARM_BASE		STM32_TIM2_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM2_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM2EN
+# ifdef CONFIG_STM32_TIM2
+#  error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2
+# endif
+#elif TONE_ALARM_TIMER == 3
+# define TONE_ALARM_BASE		STM32_TIM3_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM3_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM3EN
+# ifdef CONFIG_STM32_TIM3
+#  error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3
+# endif
+#elif TONE_ALARM_TIMER == 4
+# define TONE_ALARM_BASE		STM32_TIM4_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM4_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM4EN
+# ifdef CONFIG_STM32_TIM4
+#  error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4
+# endif
+#elif TONE_ALARM_TIMER == 5
+# define TONE_ALARM_BASE		STM32_TIM5_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM5_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM5EN
+# ifdef CONFIG_STM32_TIM5
+#  error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
+# endif
+#elif TONE_ALARM_TIMER == 9
+# define TONE_ALARM_BASE		STM32_TIM9_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM9_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM9EN
+# ifdef CONFIG_STM32_TIM9
+#  error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9
+# endif
+#elif TONE_ALARM_TIMER == 10
+# define TONE_ALARM_BASE		STM32_TIM10_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM10_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM10EN
+# ifdef CONFIG_STM32_TIM10
+#  error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10
+# endif
+#elif TONE_ALARM_TIMER == 11
+# define TONE_ALARM_BASE		STM32_TIM11_BASE
+# define TONE_ALARM_CLOCK		STM32_APB1_TIM11_CLKIN
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB1ENR_TIM11EN
+# ifdef CONFIG_STM32_TIM11
+#  error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
+# endif
+#else
+# error Must set TONE_ALARM_TIMER to a generic timer if CONFIG_TONE_ALARM is set
+#endif
+
+#if TONE_ALARM_CHANNEL == 1
+# define TONE_CCMR1	(3 << 4)
+# define TONE_CCMR2	0
+# define TONE_CCER	(1 << 0)
+# define TONE_rCCR	rCCR1
+#elif TONE_ALARM_CHANNEL == 2
+# define TONE_CCMR1	(3 << 12)
+# define TONE_CCMR2	0
+# define TONE_CCER	(1 << 4)
+# define TONE_rCCR	rCCR2
+#elif TONE_ALARM_CHANNEL == 3
+# define TONE_CCMR1	0
+# define TONE_CCMR2	(3 << 4)
+# define TONE_CCER	(1 << 8)
+# define TONE_rCCR	rCCR3
+#elif TONE_ALARM_CHANNEL == 4
+# define TONE_CCMR1	0
+# define TONE_CCMR2	(3 << 12)
+# define TONE_CCER	(1 << 12)
+# define TONE_rCCR	rCCR4
+#else
+# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4
+#endif
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
+
+#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+
+#define TONE_MAX_PATTERN	6
+#define TONE_MAX_PATTERN_LEN	40
+
+/* predefined patterns for alarms 1-TONE_MAX_PATTERN */
+const struct tone_note patterns[TONE_MAX_PATTERN][TONE_MAX_PATTERN_LEN] = {
+	{
+		{TONE_NOTE_A7, 12},
+		{TONE_NOTE_D8, 12},
+		{TONE_NOTE_C8, 12},
+		{TONE_NOTE_A7, 12},
+		{TONE_NOTE_D8, 12},
+		{TONE_NOTE_C8, 12},
+		{TONE_NOTE_D8, 4},
+		{TONE_NOTE_C8, 4},
+		{TONE_NOTE_D8, 4},
+		{TONE_NOTE_C8, 4},
+		{TONE_NOTE_D8, 4},
+		{TONE_NOTE_C8, 4},
+	},
+	{{TONE_NOTE_B6, 100}},
+	{{TONE_NOTE_C7, 100}},
+	{{TONE_NOTE_D7, 100}},
+	{{TONE_NOTE_E7, 100}},
+	{	//This is tetris ;)
+		{TONE_NOTE_C6, 40},
+		{TONE_NOTE_G5, 20},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_A5S, 40},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_G5, 20},
+		{TONE_NOTE_F5, 40},
+		{TONE_NOTE_F5, 20},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_C6, 40},
+		{TONE_NOTE_A5S, 20},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_G5, 60},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_A5S, 40},
+		{TONE_NOTE_C6, 40},
+		{TONE_NOTE_G5S, 40},
+		{TONE_NOTE_F5, 40},
+		{TONE_NOTE_F5, 60},
+
+		{TONE_NOTE_A5S, 40},
+		{TONE_NOTE_C6S, 20},
+		{TONE_NOTE_F6, 40},
+		{TONE_NOTE_D6S, 20},
+		{TONE_NOTE_C6S, 20},
+		{TONE_NOTE_C6, 60},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_C6, 40},
+		{TONE_NOTE_A5S, 20},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_G5, 40},
+		{TONE_NOTE_G5, 20},
+		{TONE_NOTE_G5S, 20},
+		{TONE_NOTE_A5S, 40},
+		{TONE_NOTE_C6, 40},
+		{TONE_NOTE_G5S, 40},
+		{TONE_NOTE_F5, 40},
+		{TONE_NOTE_F5, 60},
+	}
+};
+
+static uint16_t notes[TONE_NOTE_MAX] = {
+	63707, /* E4 */
+	60132, /* F4 */
+	56758, /* F#4/Gb4 */
+	53571, /* G4 */
+	50565, /* G#4/Ab4 */
+	47727, /* A4 */
+	45048, /* A#4/Bb4 */
+	42520, /* B4 */
+	40133, /* C5 */
+	37880, /* C#5/Db5 */
+	35755, /* D5 */
+	33748, /* D#5/Eb5 */
+	31853, /* E5 */
+	30066, /* F5 */
+	28378, /* F#5/Gb5 */
+	26786, /* G5 */
+	25282, /* G#5/Ab5 */
+	23863, /* A5 */
+	22524, /* A#5/Bb5 */
+	21260, /* B5 */
+	20066, /* C6 */
+	18940, /* C#6/Db6 */
+	17877, /* D6 */
+	16874, /* D#6/Eb6 */
+	15927, /* E6 */
+	15033, /* F6 */
+	14189, /* F#6/Gb6 */
+	13393, /* G6 */
+	12641, /* G#6/Ab6 */
+	11931, /* A6 */
+	11262, /* A#6/Bb6 */
+	10630, /* B6 */
+	10033, /* C7 */
+	9470, /* C#7/Db7 */
+	8938, /* D7 */
+	8437, /* D#7/Eb7 */
+	7963, /* E7 */
+	7516, /* F7 */
+	7094, /* F#7/Gb7 */
+	6696, /* G7 */
+	6320, /* G#7/Ab7 */
+	5965, /* A7 */
+	5631, /* A#7/Bb7 */
+	5315, /* B7 */
+	5016, /* C8 */
+	4735, /* C#8/Db8 */
+	4469, /* D8 */
+	4218  /* D#8/Eb8 */
+};
+
+/* current state of the tone driver */
+struct state {
+	int		pattern;				/* current pattern */
+#define PATTERN_NONE	-1
+#define PATTERN_USER	0
+	int		note;					/* next note to play */
+	struct hrt_call	note_end;
+	struct tone_note user_pattern[TONE_MAX_PATTERN_LEN];	/* user-supplied pattern (plays at pattern 0) */
+};
+
+static struct state tone_state;
+
+static int	tone_write(struct file *filp, const char *buffer, size_t len);
+static int	tone_ioctl(struct file *filep, int cmd, unsigned long arg);
+
+static const struct file_operations tone_fops = {
+	.write = tone_write,
+	.ioctl = tone_ioctl
+};
+
+static void	tone_next(void);
+static bool	tone_ok(struct tone_note *pattern);
+int
+tone_alarm_init(void)
+{
+	/* configure the GPIO */
+	stm32_configgpio(GPIO_TONE_ALARM);
+
+	/* clock/power on our timer */
+        modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
+
+	/* default the state */
+	tone_state.pattern = -1;
+
+	/* initialise the timer */
+        rCR1 = 0;
+        rCR2 = 0;
+        rSMCR = 0;
+        rDIER = 0;
+        rCCER = 0;		/* unlock CCMR* registers */
+        rCCMR1 = TONE_CCMR1;
+        rCCMR2 = TONE_CCMR2;
+        rCCER = TONE_CCER;
+        rDCR = 0;
+
+        /* toggle the CC output each time the count passes 1 */
+        TONE_rCCR = 1;
+
+        /* 
+         * Configure the timebase to free-run at half max frequency.
+         * XXX this should be more flexible in order to get a better
+         * frequency range, but for the F4 with the APB1 timers based
+         * at 42MHz, this gets us down to ~320Hz or so.
+         */
+        rPSC = 1;
+
+        tone_state.pattern = PATTERN_NONE;
+        tone_state.note = 0;
+
+	/* play the startup tune */
+    tone_ioctl(NULL, TONE_SET_ALARM, 1);
+
+	/* register the device */
+	return register_driver("/dev/tone_alarm", &tone_fops, 0666, NULL);
+}
+
+static int
+tone_ioctl(struct file *filep, int cmd, unsigned long arg)
+{
+	int result = 0;
+	int new = (int)arg;
+
+	irqstate_t flags = irqsave();
+
+	/* decide whether to increase the alarm level to cmd or leave it alone */
+	switch (cmd) {
+	case TONE_SET_ALARM:
+		if (new == 0) {
+			/* cancel any current alarm */
+			tone_state.pattern = PATTERN_NONE;
+			tone_next();
+
+		} else if (new > TONE_MAX_PATTERN) {
+
+			/* not a legal alarm value */
+			result = -ERANGE;
+
+		} else if (new > tone_state.pattern) {
+
+			/* higher priority than the current alarm */
+			tone_state.pattern = new;
+			tone_state.note = 0;
+
+			/* and start playing it */
+			tone_next();
+		}
+		break;
+	default:
+		result = -EINVAL;
+		break;
+	}
+
+	irqrestore(flags);
+	return result;
+}
+
+static int
+tone_write(struct file *filp, const char *buffer, size_t len)
+{
+	irqstate_t flags;
+
+	/* sanity-check the size of the write */
+	if (len > (TONE_MAX_PATTERN_LEN * sizeof(struct tone_note)))
+		return -EFBIG;
+	if ((len % sizeof(struct tone_note)) || (len == 0))
+		return -EIO;
+	if (!tone_ok((struct tone_note *)buffer))
+		return -EIO;
+
+	flags = irqsave();
+
+	/* if we aren't playing an alarm tone */
+	if (tone_state.pattern <= PATTERN_USER) {
+
+		/* reset the tone state to play the new user pattern */
+		tone_state.pattern = PATTERN_USER;
+		tone_state.note = 0;
+
+		/* copy in the new pattern */
+		memset(tone_state.user_pattern, 0, sizeof(tone_state.user_pattern));
+		memcpy(tone_state.user_pattern, buffer, len);
+
+		/* and start it */
+		tone_next();
+	}
+	irqrestore(flags);
+
+	return len;
+}
+
+static void
+tone_next(void)
+{
+	const struct tone_note *np;
+
+	/* stop the current note */
+	rCR1 = 0;
+
+	/* if we are no longer playing a pattern, we have nothing else to do here */
+	if (tone_state.pattern == PATTERN_NONE) {
+		return;
+	}
+
+	/* if the current pattern has ended, clear the pattern and stop */
+	if (tone_state.note == TONE_NOTE_MAX) {
+		tone_state.pattern = PATTERN_NONE;
+		return;
+	}
+
+	/* find the note to play */
+	if (tone_state.pattern == PATTERN_USER) {
+		np = &tone_state.user_pattern[tone_state.note];
+	} else {
+		np = &patterns[tone_state.pattern - 1][tone_state.note];
+	}
+
+	/* work out which note is next */
+	tone_state.note++;
+	if (tone_state.note >= TONE_NOTE_MAX) {
+		/* hit the end of the pattern, stop */
+		tone_state.pattern = PATTERN_NONE;
+	} else if (np[1].duration == DURATION_END) {
+		/* hit the end of the pattern, stop */
+		tone_state.pattern = PATTERN_NONE;
+	} else if (np[1].duration == DURATION_REPEAT) {
+		/* next note is a repeat, rewind in preparation */
+		tone_state.note = 0;
+	}
+
+	/* set the timer to play the note, if required */
+	if (np->pitch <= TONE_NOTE_SILENCE) {
+
+		/* set reload based on the pitch */
+		rARR = notes[np->pitch];
+
+	        /* force an update, reloads the counter and all registers */
+	        rEGR = GTIM_EGR_UG;
+
+	        /* start the timer */
+	        rCR1 = GTIM_CR1_CEN;
+	}
+
+	/* arrange a callback when the note/rest is done */
+	hrt_call_after(&tone_state.note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)tone_next, NULL);
+
+}
+
+static bool
+tone_ok(struct tone_note *pattern)
+{
+	int	i;
+
+	for (i = 0; i < TONE_NOTE_MAX; i++) {
+
+		/* first note must not be repeat or end */
+		if ((i == 0) &&
+		    ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT)))
+			return false;
+		if (pattern[i].duration == DURATION_END)
+			break;
+
+		/* pitch must be legal */
+		if (pattern[i].pitch >= TONE_NOTE_MAX)
+			return false;
+	}
+	return true;
+}
+
+#endif /* CONFIG_TONE_ALARM */
\ No newline at end of file