diff options
Diffstat (limited to 'src/drivers/stm32')
-rw-r--r-- | src/drivers/stm32/adc/adc.cpp | 387 | ||||
-rw-r--r-- | src/drivers/stm32/adc/module.mk | 42 | ||||
-rw-r--r-- | src/drivers/stm32/drv_hrt.c | 908 | ||||
-rw-r--r-- | src/drivers/stm32/drv_pwm_servo.c | 332 | ||||
-rw-r--r-- | src/drivers/stm32/drv_pwm_servo.h | 66 | ||||
-rw-r--r-- | src/drivers/stm32/module.mk | 43 | ||||
-rw-r--r-- | src/drivers/stm32/tone_alarm/module.mk | 42 | ||||
-rw-r--r-- | src/drivers/stm32/tone_alarm/tone_alarm.cpp | 1018 |
8 files changed, 2838 insertions, 0 deletions
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp new file mode 100644 index 000000000..911def943 --- /dev/null +++ b/src/drivers/stm32/adc/adc.cpp @@ -0,0 +1,387 @@ +/**************************************************************************** + * + * 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 adc.cpp + * + * Driver for the STM32 ADC. + * + * This is a low-rate driver, designed for sampling things like voltages + * and so forth. It avoids the gross complexity of the NuttX ADC driver. + */ + +#include <nuttx/config.h> +#include <drivers/device/device.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <stdio.h> +#include <unistd.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_adc.h> + +#include <arch/stm32/chip.h> +#include <stm32_internal.h> +#include <stm32_gpio.h> + +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> + +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) + +#define rSR REG(STM32_ADC_SR_OFFSET) +#define rCR1 REG(STM32_ADC_CR1_OFFSET) +#define rCR2 REG(STM32_ADC_CR2_OFFSET) +#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) +#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) +#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) +#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) +#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) +#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) +#define rHTR REG(STM32_ADC_HTR_OFFSET) +#define rLTR REG(STM32_ADC_LTR_OFFSET) +#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) +#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) +#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) +#define rJSQR REG(STM32_ADC_JSQR_OFFSET) +#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) +#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) +#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) +#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) +#define rDR REG(STM32_ADC_DR_OFFSET) + +#ifdef STM32_ADC_CCR +# define rCCR REG(STM32_ADC_CCR_OFFSET) +#endif + +class ADC : public device::CDev +{ +public: + ADC(uint32_t channels); + ~ADC(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t read(file *filp, char *buffer, size_t len); + +protected: + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + +private: + static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ + + hrt_call _call; + perf_counter_t _sample_perf; + + unsigned _channel_count; + adc_msg_s *_samples; /**< sample buffer */ + + /** work trampoline */ + static void _tick_trampoline(void *arg); + + /** worker function */ + void _tick(); + + /** + * Sample a single channel and return the measured value. + * + * @param channel The channel to sample. + * @return The sampled value, or 0xffff if + * sampling failed. + */ + uint16_t _sample(unsigned channel); + +}; + +ADC::ADC(uint32_t channels) : + CDev("adc", ADC_DEVICE_PATH), + _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), + _channel_count(0), + _samples(nullptr) +{ + _debug_enabled = true; + + /* always enable the temperature sensor */ + channels |= 1 << 16; + + /* allocate the sample array */ + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _channel_count++; + } + } + _samples = new adc_msg_s[_channel_count]; + + /* prefill the channel numbers in the sample array */ + if (_samples != nullptr) { + unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _samples[index].am_channel = i; + _samples[index].am_data = 0; + index++; + } + } + } +} + +ADC::~ADC() +{ + if (_samples != nullptr) + delete _samples; +} + +int +ADC::init() +{ + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_CAL; + usleep(100); + if (rCR2 & ADC_CR2_CAL) + return -1; +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1 = 0b00000011011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1 = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2 = +#ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ + ADC_CR2_TSVREFE | +#endif + 0; + +#ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR = ADC_CCR_TSVREFE; +#endif + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 0; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2 &= ~ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + rCR2 |= ADC_CR2_SWSTART; + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 500) { + log("sample timeout"); + return -1; + return 0xffff; + } + } + + + debug("init done"); + + /* create the device node */ + return CDev::init(); +} + +int +ADC::ioctl(file *filp, int cmd, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t +ADC::read(file *filp, char *buffer, size_t len) +{ + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) + len = maxsize; + + /* block interrupts while copying samples to avoid racing with an update */ + irqstate_t flags = irqsave(); + memcpy(buffer, _samples, len); + irqrestore(flags); + + return len; +} + +int +ADC::open_first(struct file *filp) +{ + /* get fresh data */ + _tick(); + + /* and schedule regular updates */ + hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + + return 0; +} + +int +ADC::close_last(struct file *filp) +{ + hrt_cancel(&_call); + return 0; +} + +void +ADC::_tick_trampoline(void *arg) +{ + ((ADC *)arg)->_tick(); +} + +void +ADC::_tick() +{ + /* scan the channel set and sample each */ + for (unsigned i = 0; i < _channel_count; i++) + _samples[i].am_data = _sample(_samples[i].am_channel); +} + +uint16_t +ADC::_sample(unsigned channel) +{ + perf_begin(_sample_perf); + + /* clear any previous EOC */ + if (rSR & ADC_SR_EOC) + rSR &= ~ADC_SR_EOC; + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_SWSTART; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 50) { + log("sample timeout"); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + perf_end(_sample_perf); + return result; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int adc_main(int argc, char *argv[]); + +namespace +{ +ADC *g_adc; + +void +test(void) +{ + + int fd = open(ADC_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "can't open ADC device"); + + for (unsigned i = 0; i < 50; i++) { + adc_msg_s data[8]; + ssize_t count = read(fd, data, sizeof(data)); + + if (count < 0) + errx(1, "read error"); + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + printf ("%d: %u ", data[j].am_channel, data[j].am_data); + } + + printf("\n"); + usleep(500000); + } + + exit(0); +} +} + +int +adc_main(int argc, char *argv[]) +{ + if (g_adc == nullptr) { + /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); + + if (g_adc == nullptr) + errx(1, "couldn't allocate the ADC driver"); + + if (g_adc->init() != OK) { + delete g_adc; + errx(1, "ADC init failed"); + } + } + + if (argc > 1) { + if (!strcmp(argv[1], "test")) + test(); + } + + exit(0); +} diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk new file mode 100644 index 000000000..48620feea --- /dev/null +++ b/src/drivers/stm32/adc/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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 ADC driver +# + +MODULE_COMMAND = adc + +SRCS = adc.cpp + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c new file mode 100644 index 000000000..cec0c49ff --- /dev/null +++ b/src/drivers/stm32/drv_hrt.c @@ -0,0 +1,908 @@ +/**************************************************************************** + * + * 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_hrt.c + * + * High-resolution timer callouts and timekeeping. + * + * This can use any general or advanced STM32 timer. + * + * Note that really, this could use systick too, but that's + * monopolised by NuttX and stealing it would just be awkward. + * + * We don't use the NuttX STM32 driver per se; rather, we + * claim the timer and then drive it directly. + */ + +#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 <arch/board/board.h> +#include <drivers/drv_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_HRT_TIMER + +/* HRT configuration */ +#if HRT_TIMER == 1 +# define HRT_TIMER_BASE STM32_TIM1_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1CC +# define HRT_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +# if CONFIG_STM32_TIM1 +# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1 +# endif +#elif HRT_TIMER == 2 +# define HRT_TIMER_BASE STM32_TIM2_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM2 +# define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN +# if CONFIG_STM32_TIM2 +# error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2 +# endif +#elif HRT_TIMER == 3 +# define HRT_TIMER_BASE STM32_TIM3_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM3 +# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN +# if CONFIG_STM32_TIM3 +# error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3 +# endif +#elif HRT_TIMER == 4 +# define HRT_TIMER_BASE STM32_TIM4_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM4EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM4 +# define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN +# if CONFIG_STM32_TIM4 +# error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4 +# endif +#elif HRT_TIMER == 5 +# define HRT_TIMER_BASE STM32_TIM5_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM5 +# define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN +# if CONFIG_STM32_TIM5 +# error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5 +# endif +#elif HRT_TIMER == 8 +# define HRT_TIMER_BASE STM32_TIM8_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC +# define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +# if CONFIG_STM32_TIM8 +# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8 +# endif +#elif HRT_TIMER == 9 +# define HRT_TIMER_BASE STM32_TIM9_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK +# define HRT_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +# if CONFIG_STM32_TIM9 +# error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9 +# endif +#elif HRT_TIMER == 10 +# define HRT_TIMER_BASE STM32_TIM10_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP +# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN +# if CONFIG_STM32_TIM10 +# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 +# endif +#elif HRT_TIMER == 11 +# define HRT_TIMER_BASE STM32_TIM11_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN +# if CONFIG_STM32_TIM11 +# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 +# endif +#else +# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y +#endif + +/* + * HRT clock must be a multiple of 1MHz greater than 1MHz + */ +#if (HRT_TIMER_CLOCK % 1000000) != 0 +# error HRT_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if HRT_TIMER_CLOCK <= 1000000 +# error HRT_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * Minimum/maximum deadlines. + * + * These are suitable for use with a 16-bit timer/counter clocked + * at 1MHz. The high-resolution timer need only guarantee that it + * not wrap more than once in the 50ms period for absolute time to + * be consistently maintained. + * + * The minimum deadline must be such that the time taken between + * reading a time and writing a deadline to the timer cannot + * result in missing the deadline. + */ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* + * Period of the free-running counter, in microseconds. + */ +#define HRT_COUNTER_PERIOD 65536 + +/* + * Scaling factor(s) for the free-running counter; convert an input + * in counts to a time in microseconds. + */ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_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) + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if HRT_TIMER_CHANNEL == 1 +# define rCCR_HRT rCCR1 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC1IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 2 +# define rCCR_HRT rCCR2 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC2IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC2IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 3 +# define rCCR_HRT rCCR3 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC3IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC3IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 4 +# define rCCR_HRT rCCR4 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC4IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC4IF /* interrupt status for HRT */ +#else +# error HRT_TIMER_CHANNEL must be a value between 1 and 4 +#endif + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint16_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint16_t latency_actual; + +/* latency histogram */ +#define LATENCY_BUCKET_COUNT 8 +static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context); +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + +/* + * Specific registers and bits used by PPM sub-functions + */ +#ifdef CONFIG_HRT_PPM +/* + * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. + */ +# ifndef GTIM_CCER_CC1NP +# define GTIM_CCER_CC1NP 0 +# define GTIM_CCER_CC2NP 0 +# define GTIM_CCER_CC3NP 0 +# define GTIM_CCER_CC4NP 0 +# define PPM_EDGE_FLIP +# endif + +# if HRT_PPM_CHANNEL == 1 +# define rCCR_PPM rCCR1 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC1IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC1OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 1 /* not on TI1/TI2 */ +# define CCMR2_PPM 0 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC1P +# elif HRT_PPM_CHANNEL == 2 +# define rCCR_PPM rCCR2 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC2IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC2OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 2 /* not on TI1/TI2 */ +# define CCMR2_PPM 0 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC2P +# elif HRT_PPM_CHANNEL == 3 +# define rCCR_PPM rCCR3 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC3IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC3OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 0 /* not on TI1/TI2 */ +# define CCMR2_PPM 1 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC3P +# elif HRT_PPM_CHANNEL == 4 +# define rCCR_PPM rCCR4 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC4IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC4OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 0 /* not on TI1/TI2 */ +# define CCMR2_PPM 2 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC4P +# else +# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set +# endif + +/* + * PPM decoder tuning parameters + */ +# define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +# define PPM_MIN_START 2500 /* shortest valid start gap */ + +/* decoded PPM buffer */ +#define PPM_MAX_CHANNELS 12 +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT unsigned ppm_decoded_channels = 0; +__EXPORT uint64_t ppm_last_valid_decode = 0; + +/* PPM edge history */ +__EXPORT uint16_t ppm_edge_history[32]; +unsigned ppm_edge_next; + +/* PPM pulse history */ +__EXPORT uint16_t ppm_pulse_history[32]; +unsigned ppm_pulse_next; + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/* PPM decoder state machine */ +struct { + uint16_t last_edge; /* last capture time */ + uint16_t last_mark; /* last significant edge */ + unsigned next_channel; + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + +static void hrt_ppm_decode(uint32_t status); + +#else +/* disable the PPM configuration */ +# define rCCR_PPM 0 +# define DIER_PPM 0 +# define SR_INT_PPM 0 +# define SR_OVF_PPM 0 +# define CCMR1_PPM 0 +# define CCMR2_PPM 0 +# define CCER_PPM 0 +#endif /* CONFIG_HRT_PPM */ + +/* + * Initialise the timer we are going to use. + * + * We expect that we'll own one of the reduced-function STM32 general + * timers, and that we can use channel 1 in compare mode. + */ +static void +hrt_tim_init(void) +{ + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + + /* clock/power on our timer */ + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_HRT | DIER_PPM; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PPM; + rCCMR2 = CCMR2_PPM; + rCCER = CCER_PPM; + rDCR = 0; + + /* configure the timer to free-run at 1MHz */ + rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ + + /* run the full span of the counter */ + rARR = 0xffff; + + /* set an initial capture a little ways off */ + rCCR_HRT = 1000; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); +} + +#ifdef CONFIG_HRT_PPM +/* + * Handle the PPM decoder state machine. + */ +static void +hrt_ppm_decode(uint32_t status) +{ + uint16_t count = rCCR_PPM; + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PPM) + goto error; + + /* how long since the last edge? */ + width = count - ppm.last_edge; + ppm.last_edge = count; + + ppm_edge_history[ppm_edge_next++] = width; + + if (ppm_edge_next >= 32) + ppm_edge_next = 0; + + /* + * if this looks like a start pulse, then push the last set of values + * and reset the state machine + */ + if (width >= PPM_MIN_START) { + + /* export the last set of samples if we got something sensible */ + if (ppm.next_channel > 4) { + for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; + + ppm_decoded_channels = i; + ppm_last_valid_decode = hrt_absolute_time(); + + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + return; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = count; + ppm.phase = INACTIVE; + return; + + case INACTIVE: + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + return; + + case ACTIVE: + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + + ppm_pulse_history[ppm_pulse_next++] = interval; + + if (ppm_pulse_next >= 32) + ppm_pulse_next = 0; + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + goto error; + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) + ppm_temp_buffer[ppm.next_channel++] = interval; + + ppm.phase = INACTIVE; + return; + + } + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; + +} +#endif /* CONFIG_HRT_PPM */ + +/* + * Handle the compare interupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_tim_isr(int irq, void *context) +{ + uint32_t status; + + /* grab the timer for latency tracking purposes */ + latency_actual = rCNT; + + /* copy interrupt status */ + status = rSR; + + /* ack the interrupts we just read */ + rSR = ~status; + +#ifdef CONFIG_HRT_PPM + + /* was this a PPM edge? */ + if (status & (SR_INT_PPM | SR_OVF_PPM)) { + /* if required, flip edge sensitivity */ +# ifdef PPM_EDGE_FLIP + rCCER ^= CCER_PPM_FLIP; +# endif + + hrt_ppm_decode(status); + } +#endif + + /* was this a timer tick? */ + if (status & SR_INT_HRT) { + + /* do latency calculations */ + hrt_latency_update(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + } + + return OK; +} + +/* + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ + hrt_abstime abstime; + uint32_t count; + irqstate_t flags; + + /* + * Counter state. Marked volatile as they may change + * inside this routine but outside the irqsave/restore + * pair. Discourage the compiler from moving loads/stores + * to these outside of the protected range. + */ + static volatile hrt_abstime base_time; + static volatile uint32_t last_count; + + /* prevent re-entry */ + flags = irqsave(); + + /* get the current counter value */ + count = rCNT; + + /* + * Determine whether the counter has wrapped since the + * last time we're called. + * + * This simple test is sufficient due to the guarantee that + * we are always called at least once per counter period. + */ + if (count < last_count) + base_time += HRT_COUNTER_PERIOD; + + /* save the count for next time */ + last_count = count; + + /* compute the current time */ + abstime = HRT_COUNTER_SCALE(base_time + count); + + irqrestore(flags); + + return abstime; +} + +/* + * Convert a timespec to absolute time + */ +hrt_abstime +ts_to_abstime(struct timespec *ts) +{ + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; +} + +/* + * Convert absolute time to a timespec. + */ +void +abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +{ + ts->tv_sec = abstime / 1000000; + abstime -= ts->tv_sec * 1000000; + ts->tv_nsec = abstime * 1000; +} + +/* + * Compare a time value with the current time. + */ +hrt_abstime +hrt_elapsed_time(const volatile hrt_abstime *then) +{ + irqstate_t flags = irqsave(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + irqrestore(flags); + + return delta; +} + +/* + * Store the absolute time in an interrupt-safe fashion + */ +hrt_abstime +hrt_store_absolute_time(volatile hrt_abstime *now) +{ + irqstate_t flags = irqsave(); + + hrt_abstime ts = hrt_absolute_time(); + + irqrestore(flags); + + return ts; +} + +/* + * Initalise the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); + +#ifdef CONFIG_HRT_PPM + /* configure the PPM input pin */ + stm32_configgpio(GPIO_PPM_IN); +#endif +} + +/* + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/* + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/* + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = irqsave(); + + /* if the entry is currently queued, remove it */ + if (entry->deadline != 0) + sq_rem(&entry->link, &callout_queue); + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + irqrestore(flags); +} + +/* + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/* + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = irqsave(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + irqrestore(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + //lldbg("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + //lldbg("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) + break; + + if (call->deadline > now) + break; + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + call->deadline = deadline + call->period; + hrt_call_enter(call); + } + } +} + +/* + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + //lldbg("entry in queue\n"); + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + //lldbg("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + deadline = next->deadline; + } + } + + //lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + rCCR_HRT = latency_baseline = deadline & 0xffff; +} + +static void +hrt_latency_update(void) +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + + +#endif /* CONFIG_HRT_TIMER */ diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c new file mode 100644 index 000000000..c1efb8515 --- /dev/null +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -0,0 +1,332 @@ +/**************************************************************************** + * + * 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 "drv_pwm_servo.h" + +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> + +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +#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); +static void pwm_timer_set_rate(unsigned timer, unsigned rate); +static void pwm_channel_init(unsigned channel); + +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; + + /* default to updating at 50Hz */ + pwm_timer_set_rate(timer, 50); + + /* 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) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; + rCCR1(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC1E; + break; + + case 2: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; + rCCR2(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC2E; + break; + + case 3: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; + rCCR3(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC3E; + break; + + case 4: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; + rCCR4(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC4E; + break; + } +} + +int +up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) + 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) + 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].timer_channel == 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 + 1; +} + +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].timer_channel != 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_group_update(unsigned group, unsigned rate) +{ + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + if (rate < 1) + return -ERANGE; + if (rate > 10000) + return -ERANGE; + + if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) + return ERROR; + + pwm_timer_set_rate(group, rate); + + return OK; +} + +int +up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) + up_pwm_servo_set_rate_group_update(i, rate); + + return 0; +} + +uint32_t +up_pwm_servo_get_rate_group(unsigned group) +{ + unsigned channels = 0; + + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) + channels |= (1 << i); + } + return channels; +} + +void +up_pwm_servo_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + // XXX This leads to FMU PWM being still active + // but uncontrollable. Just disable the timer + // and risk a runt. + ///* on disarm, just stop auto-reload so we don't generate runts */ + //rCR1(i) &= ~GTIM_CR1_ARPE; + rCR1(i) = 0; + } + } + } +} diff --git a/src/drivers/stm32/drv_pwm_servo.h b/src/drivers/stm32/drv_pwm_servo.h new file mode 100644 index 000000000..5dd4cf70c --- /dev/null +++ b/src/drivers/stm32/drv_pwm_servo.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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.h + * + * stm32-specific PWM output data. + */ + +#pragma once + +#include <drivers/drv_pwm_output.h> + +/* configuration limits */ +#define PWM_SERVO_MAX_TIMERS 4 +#define PWM_SERVO_MAX_CHANNELS 8 + +/* array of timers dedicated to PWM servo use */ +struct pwm_servo_timer { + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; +}; + +/* array of channels in logical order */ +struct pwm_servo_channel { + uint32_t gpio; + uint8_t timer_index; + uint8_t timer_channel; + servo_position_t default_value; +}; + +/* supplied by board-specific code */ +__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS]; +__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS]; diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk new file mode 100644 index 000000000..bb751c7f6 --- /dev/null +++ b/src/drivers/stm32/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# + +SRCS = drv_hrt.c \ + drv_pwm_servo.c + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk new file mode 100644 index 000000000..827cf30b2 --- /dev/null +++ b/src/drivers/stm32/tone_alarm/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Tone alarm driver +# + +MODULE_COMMAND = tone_alarm + +SRCS = tone_alarm.cpp + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp new file mode 100644 index 000000000..ac5511e60 --- /dev/null +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -0,0 +1,1018 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Driver for the PX4 audio alarm port, /dev/tone_alarm. + * + * The tone_alarm driver supports a set of predefined "alarm" + * tunes and one user-supplied tune. + * + * The TONE_SET_ALARM ioctl can be used to select a predefined + * alarm tune, from 1 - <TBD>. Selecting tune zero silences + * the alarm. + * + * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY + * statement, with some exceptions and extensions. + * + * From Wikibooks: + * + * PLAY "[string expression]" + * + * Used to play notes and a score ... The tones are indicated by letters A through G. + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * immediately after the note letter. See this example: + * + * PLAY "C C# C C#" + * + * Whitespaces are ignored inside the string expression. There are also codes that + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators + * that change the properties are effective for the notes following that indicator. + * + * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration + * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. + * (L8, L16, L32, L64, ...). By default, n = 4. + * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. + * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" + * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. + * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. + * Remember that C- is equivalent to B. + * < > Changes the current octave respectively down or up one level. + * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) + * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. + * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. + * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. + * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. + * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. + * It can be used for a pause as well. + * + * Extensions/variations: + * + * MB MF The MF command causes the tune to play once and then stop. The MB command causes the + * tune to repeat when it ends. + * + */ + +#include <nuttx/config.h> +#include <debug.h> + +#include <drivers/device/device.h> +#include <drivers/drv_tone_alarm.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <stdio.h> +#include <unistd.h> +#include <math.h> +#include <ctype.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> + +#include <arch/stm32/chip.h> +#include <up_internal.h> +#include <up_arch.h> + +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +#include <systemlib/err.h> + +#ifndef CONFIG_HRT_TIMER +# error This driver 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 in order to use this driver. +#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 to use this driver. +#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) + +class ToneAlarm : public device::CDev +{ +public: + ToneAlarm(); + ~ToneAlarm(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + +private: + static const unsigned _tune_max = 1024; // be reasonable about user tunes + static const char * const _default_tunes[]; + static const unsigned _default_ntunes; + static const uint8_t _note_tab[]; + + const char *_user_tune; + + const char *_tune; // current tune string + const char *_next; // next note in the string + + unsigned _tempo; + unsigned _note_length; + enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; + unsigned _octave; + unsigned _silence_length; // if nonzero, silence before next note + bool _repeat; // if true, tune restarts at end + + hrt_call _note_call; // HRT callout for note completion + + // Convert a note value in the range C1 to B7 into a divisor for + // the configured timer's clock. + // + unsigned note_to_divisor(unsigned note); + + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of + // dots following in the play string. + // + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + + // Calculate the duration in microseconds of a rest corresponding to + // a given note length. + // + unsigned rest_duration(unsigned rest_length, unsigned dots); + + // Start playing the note + // + void start_note(unsigned note); + + // Stop playing the current note and make the player 'safe' + // + void stop_note(); + + // Start playing the tune + // + void start_tune(const char *tune); + + // Parse the next note out of the string and play it + // + void next_note(); + + // Find the next character in the string, discard any whitespace and + // return the canonical (uppercase) version. + // + int next_char(); + + // Extract a number from the string, consuming all the digit characters. + // + unsigned next_number(); + + // Consume dot characters from the string, returning the number consumed. + // + unsigned next_dots(); + + // hrt_call trampoline for next_note + // + static void next_trampoline(void *arg); + +}; + +// predefined tune array +const char * const ToneAlarm::_default_tunes[] = { + "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune + "MBT200a8a8a8PaaaP", // ERROR tone + "MFT200e8a8a", // NotifyPositive tone + "MFT200e8e", // NotifyNeutral tone + "MFT200e8c8e8c8e8c8", // NotifyNegative tone + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! + "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie + "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha + "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee + "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy + "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell + "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" + "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" + "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" + "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" + "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" + "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" + "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" + "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" + "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" + "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" + "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" + "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" + "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" + "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" + "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" + "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" + "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" + "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" + "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" + "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" + "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" + "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" + "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" + "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" + "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" + "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" + "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" + "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" + "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" + "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" + "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" + "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" + "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" + "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" + "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" + "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" + "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" + "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" + "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" + "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" + "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" + "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" + "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" + "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" + "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" + "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" + "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" + "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" + "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" + "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" + "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" + "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." + "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" + "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" + "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" + "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" + "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" + "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" + "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" + "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" + "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" + "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" + "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" + "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" + "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" + "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" + "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" + "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" + "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" + "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" + "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" + "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" + "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" + "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" + "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" + "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" + "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" + "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" + "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" + "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" + "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" + "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" + "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" + "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" + "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" + "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" + "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" + "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" + "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" + "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" + "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" + "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" + "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" + "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" + "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" + "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" + "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" + "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" + "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" + "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" + "O2E2P64", +}; + +const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); + +// semitone offsets from C for the characters 'A'-'G' +const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); + + +ToneAlarm::ToneAlarm() : + CDev("tone_alarm", "/dev/tone_alarm"), + _user_tune(nullptr), + _tune(nullptr), + _next(nullptr) +{ + // enable debug() calls + //_debug_enabled = true; +} + +ToneAlarm::~ToneAlarm() +{ +} + +int +ToneAlarm::init() +{ + int ret; + + ret = CDev::init(); + + if (ret != OK) + return ret; + + /* configure the GPIO to the idle state */ + stm32_configgpio(GPIO_TONE_ALARM_IDLE); + + /* clock/power on our timer */ + modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); + + /* initialise the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = 0; + rCCER &= TONE_CCER; /* 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; + + /* default the timer to a prescale value of 1; playing notes will change this */ + rPSC = 0; + + /* make sure the timer is running */ + rCR1 = GTIM_CR1_CEN; + + debug("ready"); + return OK; +} + +unsigned +ToneAlarm::note_to_divisor(unsigned note) +{ + // compute the frequency first (Hz) + float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); + + float period = 0.5f / freq; + + // and the divisor, rounded to the nearest integer + unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f; + + return divisor; +} + +unsigned +ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) +{ + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; + + if (note_length == 0) + note_length = 1; + unsigned note_period = whole_note_period / note_length; + + switch (_note_mode) { + case MODE_NORMAL: + silence = note_period / 8; + break; + case MODE_STACCATO: + silence = note_period / 4; + break; + default: + case MODE_LEGATO: + silence = 0; + break; + } + note_period -= silence; + + unsigned dot_extension = note_period / 2; + while (dots--) { + note_period += dot_extension; + dot_extension /= 2; + } + + return note_period; +} + +unsigned +ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) +{ + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; + + if (rest_length == 0) + rest_length = 1; + + unsigned rest_period = whole_note_period / rest_length; + + unsigned dot_extension = rest_period / 2; + while (dots--) { + rest_period += dot_extension; + dot_extension /= 2; + } + + return rest_period; +} + +void +ToneAlarm::start_note(unsigned note) +{ + // compute the divisor + unsigned divisor = note_to_divisor(note); + + // pick the lowest prescaler value that we can use + // (note that the effective prescale value is 1 greater) + unsigned prescale = divisor / 65536; + + // calculate the timer period for the selected prescaler value + unsigned period = (divisor / (prescale + 1)) - 1; + + rPSC = prescale; // load new prescaler + rARR = period; // load new toggle period + rEGR = GTIM_EGR_UG; // force a reload of the period + rCCER |= TONE_CCER; // enable the output + + // configure the GPIO to enable timer output + stm32_configgpio(GPIO_TONE_ALARM); +} + +void +ToneAlarm::stop_note() +{ + /* stop the current note */ + rCCER &= ~TONE_CCER; + + /* + * Make sure the GPIO is not driving the speaker. + */ + stm32_configgpio(GPIO_TONE_ALARM_IDLE); +} + +void +ToneAlarm::start_tune(const char *tune) +{ + // kill any current playback + hrt_cancel(&_note_call); + + // record the tune + _tune = tune; + _next = tune; + + // initialise player state + _tempo = 120; + _note_length = 4; + _note_mode = MODE_NORMAL; + _octave = 4; + _silence_length = 0; + _repeat = false; // otherwise command-line tunes repeat forever... + + // schedule a callback to start playing + hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this); +} + +void +ToneAlarm::next_note() +{ + // do we have an inter-note gap to wait for? + if (_silence_length > 0) { + stop_note(); + hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + _silence_length = 0; + return; + } + + // make sure we still have a tune - may be removed by the write / ioctl handler + if ((_next == nullptr) || (_tune == nullptr)) { + stop_note(); + return; + } + + // parse characters out of the string until we have resolved a note + unsigned note = 0; + unsigned note_length = _note_length; + unsigned duration; + + while (note == 0) { + // we always need at least one character from the string + int c = next_char(); + if (c == 0) + goto tune_end; + _next++; + + switch (c) { + case 'L': // select note length + _note_length = next_number(); + if (_note_length < 1) + goto tune_error; + break; + + case 'O': // select octave + _octave = next_number(); + if (_octave > 6) + _octave = 6; + break; + + case '<': // decrease octave + if (_octave > 0) + _octave--; + break; + + case '>': // increase octave + if (_octave < 6) + _octave++; + break; + + case 'M': // select inter-note gap + c = next_char(); + if (c == 0) + goto tune_error; + _next++; + switch (c) { + case 'N': + _note_mode = MODE_NORMAL; + break; + case 'L': + _note_mode = MODE_LEGATO; + break; + case 'S': + _note_mode = MODE_STACCATO; + break; + case 'F': + _repeat = false; + break; + case 'B': + _repeat = true; + break; + default: + goto tune_error; + } + break; + + case 'P': // pause for a note length + stop_note(); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); + return; + + case 'T': { // change tempo + unsigned nt = next_number(); + + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + } else { + goto tune_error; + } + break; + } + + case 'N': // play an arbitrary note + note = next_number(); + if (note > 84) + goto tune_error; + if (note == 0) { + // this is a rest - pause for the current note length + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; + } + break; + + case 'A'...'G': // play a note in the current octave + note = _note_tab[c - 'A'] + (_octave * 12) + 1; + c = next_char(); + switch (c) { + case '#': // up a semitone + case '+': + if (note < 84) + note++; + _next++; + break; + case '-': // down a semitone + if (note > 1) + note--; + _next++; + break; + default: + // 0 / no next char here is OK + break; + } + // shorthand length notation + note_length = next_number(); + if (note_length == 0) + note_length = _note_length; + break; + + default: + goto tune_error; + } + } + + // compute the duration of the note and the following silence (if any) + duration = note_duration(_silence_length, note_length, next_dots()); + + // start playing the note + start_note(note); + + // and arrange a callback when the note should stop + hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); + return; + + // tune looks bad (unexpected EOF, bad character, etc.) +tune_error: + lowsyslog("tune error\n"); + _repeat = false; // don't loop on error + + // stop (and potentially restart) the tune +tune_end: + stop_note(); + if (_repeat) + start_tune(_tune); + return; +} + +int +ToneAlarm::next_char() +{ + while (isspace(*_next)) { + _next++; + } + return toupper(*_next); +} + +unsigned +ToneAlarm::next_number() +{ + unsigned number = 0; + int c; + + for (;;) { + c = next_char(); + if (!isdigit(c)) + return number; + _next++; + number = (number * 10) + (c - '0'); + } +} + +unsigned +ToneAlarm::next_dots() +{ + unsigned dots = 0; + + while (next_char() == '.') { + _next++; + dots++; + } + return dots; +} + +void +ToneAlarm::next_trampoline(void *arg) +{ + ToneAlarm *ta = (ToneAlarm *)arg; + + ta->next_note(); +} + + +int +ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + debug("ioctl %i %u", cmd, arg); + +// irqstate_t flags = irqsave(); + + /* decide whether to increase the alarm level to cmd or leave it alone */ + switch (cmd) { + case TONE_SET_ALARM: + debug("TONE_SET_ALARM %u", arg); + + if (arg <= _default_ntunes) { + if (arg == 0) { + // stop the tune + _tune = nullptr; + _next = nullptr; + } else { + // play the selected tune + start_tune(_default_tunes[arg - 1]); + } + } else { + result = -EINVAL; + } + + break; + + default: + result = -ENOTTY; + break; + } + +// irqrestore(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) + result = CDev::ioctl(filp, cmd, arg); + + return result; +} + +int +ToneAlarm::write(file *filp, const char *buffer, size_t len) +{ + // sanity-check the buffer for length and nul-termination + if (len > _tune_max) + return -EFBIG; + + // if we have an existing user tune, free it + if (_user_tune != nullptr) { + + // if we are playing the user tune, stop + if (_tune == _user_tune) { + _tune = nullptr; + _next = nullptr; + } + + // free the old user tune + free((void *)_user_tune); + _user_tune = nullptr; + } + + // if the new tune is empty, we're done + if (buffer[0] == '\0') + return OK; + + // allocate a copy of the new tune + _user_tune = strndup(buffer, len); + if (_user_tune == nullptr) + return -ENOMEM; + + // and play it + start_tune(_user_tune); + + return len; +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +ToneAlarm *g_dev; + +int +play_tune(unsigned tune) +{ + int fd, ret; + + fd = open("/dev/tone_alarm", 0); + + if (fd < 0) + err(1, "/dev/tone_alarm"); + + ret = ioctl(fd, TONE_SET_ALARM, tune); + close(fd); + + if (ret != 0) + err(1, "TONE_SET_ALARM"); + + exit(0); +} + +int +play_string(const char *str) +{ + int fd, ret; + + fd = open("/dev/tone_alarm", O_WRONLY); + + if (fd < 0) + err(1, "/dev/tone_alarm"); + + ret = write(fd, str, strlen(str) + 1); + close(fd); + + if (ret < 0) + err(1, "play tune"); + exit(0); +} + +} // namespace + +int +tone_alarm_main(int argc, char *argv[]) +{ + unsigned tune; + + /* start the driver lazily */ + if (g_dev == nullptr) { + g_dev = new ToneAlarm; + + if (g_dev == nullptr) + errx(1, "couldn't allocate the ToneAlarm driver"); + + if (g_dev->init() != OK) { + delete g_dev; + errx(1, "ToneAlarm init failed"); + } + } + + if ((argc > 1) && !strcmp(argv[1], "start")) + play_tune(1); + + if ((argc > 1) && !strcmp(argv[1], "stop")) + play_tune(0); + + if ((tune = strtol(argv[1], nullptr, 10)) != 0) + play_tune(tune); + + /* if it looks like a PLAY string... */ + if (strlen(argv[1]) > 2) { + const char *str = argv[1]; + if (str[0] == 'M') { + play_string(str); + } + } + + errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); +} |