diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/boards/px4io/Makefile | 41 | ||||
-rw-r--r-- | apps/drivers/boards/px4io/px4io_init.c | 98 | ||||
-rw-r--r-- | apps/drivers/boards/px4io/px4io_internal.h | 83 | ||||
-rw-r--r-- | apps/drivers/boards/px4io/px4io_pwm_servo.c | 123 | ||||
-rw-r--r-- | apps/drivers/stm32/Makefile | 42 | ||||
-rw-r--r-- | apps/drivers/stm32/adc/Makefile | 43 | ||||
-rw-r--r-- | apps/drivers/stm32/adc/adc.cpp | 387 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_hrt.c | 908 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_pwm_servo.c | 330 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_pwm_servo.h | 66 | ||||
-rw-r--r-- | apps/drivers/stm32/tone_alarm/Makefile | 43 | ||||
-rw-r--r-- | apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 1018 | ||||
-rw-r--r-- | apps/px4io/Makefile | 58 | ||||
-rw-r--r-- | apps/px4io/adc.c | 168 | ||||
-rw-r--r-- | apps/px4io/controls.c | 332 | ||||
-rw-r--r-- | apps/px4io/dsm.c | 356 | ||||
-rw-r--r-- | apps/px4io/i2c.c | 340 | ||||
-rw-r--r-- | apps/px4io/mixer.cpp | 311 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 204 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 231 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 188 | ||||
-rw-r--r-- | apps/px4io/registers.c | 644 | ||||
-rw-r--r-- | apps/px4io/safety.c | 195 | ||||
-rw-r--r-- | apps/px4io/sbus.c | 255 |
24 files changed, 0 insertions, 6464 deletions
diff --git a/apps/drivers/boards/px4io/Makefile b/apps/drivers/boards/px4io/Makefile deleted file mode 100644 index 85806fe6f..000000000 --- a/apps/drivers/boards/px4io/Makefile +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Board-specific startup code for the PX4IO -# - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common -LIBNAME = brd_px4io - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c deleted file mode 100644 index 14e8dc13a..000000000 --- a/apps/drivers/boards/px4io/px4io_init.c +++ /dev/null @@ -1,98 +0,0 @@ -/**************************************************************************** - * - * 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 px4io_init.c - * - * PX4IO-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include <nuttx/config.h> - -#include <stdbool.h> -#include <stdio.h> -#include <debug.h> -#include <errno.h> - -#include <nuttx/arch.h> - -#include "stm32_internal.h" -#include "px4io_internal.h" -#include "stm32_uart.h" - -#include <arch/board/board.h> - -#include <drivers/drv_hrt.h> -#include <drivers/drv_led.h> -#include <drivers/drv_pwm_output.h> - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - /* configure GPIOs */ - stm32_configgpio(GPIO_ACC1_PWR_EN); - stm32_configgpio(GPIO_ACC2_PWR_EN); - stm32_configgpio(GPIO_SERVO_PWR_EN); - stm32_configgpio(GPIO_RELAY1_EN); - stm32_configgpio(GPIO_RELAY2_EN); - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); - stm32_configgpio(GPIO_LED3); - stm32_configgpio(GPIO_ACC_OC_DETECT); - stm32_configgpio(GPIO_SERVO_OC_DETECT); - stm32_configgpio(GPIO_BTN_SAFETY); - - stm32_configgpio(GPIO_ADC_VBATT); - stm32_configgpio(GPIO_ADC_IN5); -} diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h deleted file mode 100644 index 44bb98513..000000000 --- a/apps/drivers/boards/px4io/px4io_internal.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * 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 PX4IO hardware definitions. - */ - -#pragma once - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include <nuttx/config.h> -#include <nuttx/compiler.h> -#include <stdint.h> - -#include <stm32_internal.h> - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/* PX4IO GPIOs **********************************************************************/ -/* LEDs */ - -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) - -/* Safety switch button *************************************************************/ - -#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) - -/* Power switch controls ************************************************************/ - -#define GPIO_ACC1_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) -#define GPIO_ACC2_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14) -#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) - -#define GPIO_ACC_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12) -#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) - -#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) -#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) - -/* Analog inputs ********************************************************************/ - -#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) -#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) diff --git a/apps/drivers/boards/px4io/px4io_pwm_servo.c b/apps/drivers/boards/px4io/px4io_pwm_servo.c deleted file mode 100644 index a2f73c429..000000000 --- a/apps/drivers/boards/px4io/px4io_pwm_servo.c +++ /dev/null @@ -1,123 +0,0 @@ -/**************************************************************************** - * - * 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 px4fmu_pwm_servo.c - * - * Configuration data for the stm32 pwm_servo driver. - * - * Note that these arrays must always be fully-sized. - */ - -#include <stdint.h> - -#include <drivers/stm32/drv_pwm_servo.h> - -#include <arch/board/board.h> -#include <drivers/drv_pwm_output.h> - -#include <stm32_internal.h> -#include <stm32_gpio.h> -#include <stm32_tim.h> - -__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { - { - .base = STM32_TIM2_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM2EN, - .clock_freq = STM32_APB1_TIM2_CLKIN - }, - { - .base = STM32_TIM3_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM3EN, - .clock_freq = STM32_APB1_TIM3_CLKIN - }, - { - .base = STM32_TIM4_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM4EN, - .clock_freq = STM32_APB1_TIM4_CLKIN - } -}; - -__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { - { - .gpio = GPIO_TIM2_CH1OUT, - .timer_index = 0, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM2_CH2OUT, - .timer_index = 0, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH3OUT, - .timer_index = 2, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH4OUT, - .timer_index = 2, - .timer_channel = 4, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH1OUT, - .timer_index = 1, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH2OUT, - .timer_index = 1, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH3OUT, - .timer_index = 1, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH4OUT, - .timer_index = 1, - .timer_channel = 4, - .default_value = 1000, - } -}; diff --git a/apps/drivers/stm32/Makefile b/apps/drivers/stm32/Makefile deleted file mode 100644 index 4ad57f413..000000000 --- a/apps/drivers/stm32/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# STM32 driver support code -# -# Modules in this directory are compiled for all STM32 targets. -# - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/stm32/adc/Makefile b/apps/drivers/stm32/adc/Makefile deleted file mode 100644 index 443bc0623..000000000 --- a/apps/drivers/stm32/adc/Makefile +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# STM32 ADC driver -# - -APPNAME = adc -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp deleted file mode 100644 index 911def943..000000000 --- a/apps/drivers/stm32/adc/adc.cpp +++ /dev/null @@ -1,387 +0,0 @@ -/**************************************************************************** - * - * 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/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c deleted file mode 100644 index cec0c49ff..000000000 --- a/apps/drivers/stm32/drv_hrt.c +++ /dev/null @@ -1,908 +0,0 @@ -/**************************************************************************** - * - * 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/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c deleted file mode 100644 index d7316e1f7..000000000 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ /dev/null @@ -1,330 +0,0 @@ -/**************************************************************************** - * - * 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); -} - -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/apps/drivers/stm32/drv_pwm_servo.h b/apps/drivers/stm32/drv_pwm_servo.h deleted file mode 100644 index 5dd4cf70c..000000000 --- a/apps/drivers/stm32/drv_pwm_servo.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * 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/apps/drivers/stm32/tone_alarm/Makefile b/apps/drivers/stm32/tone_alarm/Makefile deleted file mode 100644 index d2ddf9534..000000000 --- a/apps/drivers/stm32/tone_alarm/Makefile +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Tone alarm driver -# - -APPNAME = tone_alarm -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp deleted file mode 100644 index ac5511e60..000000000 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ /dev/null @@ -1,1018 +0,0 @@ -/**************************************************************************** - * - * 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"); -} diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile deleted file mode 100644 index 0eb3ffcf7..000000000 --- a/apps/px4io/Makefile +++ /dev/null @@ -1,58 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Build the px4io application. -# - -# -# Note that we pull a couple of specific files from the systemlib, since -# we can't support it all. -# -CSRCS = adc.c \ - controls.c \ - dsm.c \ - i2c.c \ - px4io.c \ - registers.c \ - safety.c \ - sbus.c \ - ../systemlib/hx_stream.c \ - ../systemlib/perf_counter.c \ - ../systemlib/up_cxxinitialize.c - -CXXSRCS = mixer.cpp - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common - -include $(APPDIR)/mk/app.mk diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c deleted file mode 100644 index 670b8d635..000000000 --- a/apps/px4io/adc.c +++ /dev/null @@ -1,168 +0,0 @@ -/**************************************************************************** - * - * 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.c - * - * Simple ADC support for PX4IO on STM32. - */ -#include <nuttx/config.h> -#include <stdint.h> - -#include <nuttx/arch.h> -#include <arch/stm32/chip.h> -#include <stm32_internal.h> - -#include <drivers/drv_hrt.h> -#include <systemlib/perf_counter.h> - -#define DEBUG -#include "px4io.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) - -perf_counter_t adc_perf; - -int -adc_init(void) -{ - adc_perf = perf_alloc(PC_ELAPSED, "adc"); - - /* do calibration if supported */ -#ifdef ADC_CR2_CAL - rCR2 |= ADC_CR2_RSTCAL; - up_udelay(1); - - if (rCR2 & ADC_CR2_RSTCAL) - return -1; - - rCR2 |= ADC_CR2_CAL; - up_udelay(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; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - - return 0; -} - -uint16_t -adc_measure(unsigned channel) -{ - perf_begin(adc_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_ADON; - - /* wait for the conversion to complete */ - hrt_abstime now = hrt_absolute_time(); - - while (!(rSR & ADC_SR_EOC)) { - - /* never spin forever - this will give a bogus result though */ - if (hrt_elapsed_time(&now) > 1000) { - debug("adc timeout"); - break; - } - } - - /* read the result and clear EOC */ - uint16_t result = rDR; - - perf_end(adc_perf); - return result; -}
\ No newline at end of file diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c deleted file mode 100644 index dc36f6c93..000000000 --- a/apps/px4io/controls.c +++ /dev/null @@ -1,332 +0,0 @@ -/**************************************************************************** - * - * 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 controls.c - * - * R/C inputs and servo outputs. - */ - -#include <nuttx/config.h> -#include <stdbool.h> - -#include <drivers/drv_hrt.h> -#include <systemlib/perf_counter.h> -#include <systemlib/ppm_decode.h> - -#include "px4io.h" - -#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 5000 -#define RC_CHANNEL_LOW_THRESH -5000 - -static bool ppm_input(uint16_t *values, uint16_t *num_values); - -static perf_counter_t c_gather_dsm; -static perf_counter_t c_gather_sbus; -static perf_counter_t c_gather_ppm; - -void -controls_init(void) -{ - /* DSM input */ - dsm_init("/dev/ttyS0"); - - /* S.bus input */ - sbus_init("/dev/ttyS2"); - - /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - } - - c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); - c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); - c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); -} - -void -controls_tick() { - - /* - * Gather R/C control inputs from supported sources. - * - * Note that if you're silly enough to connect more than - * one control input source, they're going to fight each - * other. Don't do that. - */ - - perf_begin(c_gather_dsm); - bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); - if (dsm_updated) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; - perf_end(c_gather_dsm); - - perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); - if (sbus_updated) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - perf_end(c_gather_sbus); - - /* - * XXX each S.bus frame will cause a PPM decoder interrupt - * storm (lots of edges). It might be sensible to actually - * disable the PPM decoder completely if we have S.bus signal. - */ - perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); - if (ppm_updated) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; - perf_end(c_gather_ppm); - - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - - /* - * In some cases we may have received a frame, but input has still - * been lost. - */ - bool rc_input_lost = false; - - /* - * If we received a new frame from any of the RC sources, process it. - */ - if (dsm_updated || sbus_updated || ppm_updated) { - - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); - - /* record a bitmask of channels assigned */ - unsigned assigned_channels = 0; - - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } - - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; - - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < MAX_CONTROL_CHANNELS); - - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; - - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - } - } - - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } - - /* - * If we got an update with zero channels, treat it as - * a loss of input. - * - * This might happen if a protocol-based receiver returns an update - * that contains no channels that we have mapped. - */ - if (assigned_channels == 0) { - rc_input_lost = true; - } else { - /* set RC OK flag and clear RC lost alarm */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; - } - - /* - * Export the valid channel bitmap - */ - r_rc_valid = assigned_channels; - } - - /* - * If we haven't seen any new control data in 200ms, assume we - * have lost input. - */ - if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { - rc_input_lost = true; - - /* clear the input-kind flags here */ - r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS); - } - - /* - * Handle losing RC input - */ - if (rc_input_lost) { - - /* Clear the RC input status flag, clear manual override flag */ - r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK); - - /* Set the RC_LOST alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - - /* Mark the arrays as empty */ - r_raw_rc_count = 0; - r_rc_valid = 0; - } - - /* - * Check for manual override. - * - * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. - * Override is enabled if either the hardcoded channel / value combination - * is selected, or the AP has requested it. - */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { - - bool override = false; - - /* - * Check mapped channel 5 (can be any remote channel, - * depends on RC_MAP_OVER parameter); - * If the value is 'high' then the pilot has - * requested override. - * - */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) - override = true; - - if (override) { - - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; - - /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated) - mixer_tick(); - - } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; - } - } -} - -static bool -ppm_input(uint16_t *values, uint16_t *num_values) -{ - bool result = false; - - /* avoid racing with PPM updates */ - irqstate_t state = irqsave(); - - /* - * If we have received a new PPM frame within the last 200ms, accept it - * and then invalidate it. - */ - if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) { - - /* PPM data exists, copy it */ - *num_values = ppm_decoded_channels; - if (*num_values > MAX_CONTROL_CHANNELS) - *num_values = MAX_CONTROL_CHANNELS; - - for (unsigned i = 0; i < *num_values; i++) - values[i] = ppm_buffer[i]; - - /* clear validity */ - ppm_last_valid_decode = 0; - - /* good if we got any channels */ - result = (*num_values > 0); - } - - irqrestore(state); - - return result; -} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c deleted file mode 100644 index ea35e5513..000000000 --- a/apps/px4io/dsm.c +++ /dev/null @@ -1,356 +0,0 @@ -/**************************************************************************** - * - * 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 dsm.c - * - * Serial protocol decoder for the Spektrum DSM* family of protocols. - * - * Decodes into the global PPM buffer and updates accordingly. - */ - -#include <nuttx/config.h> - -#include <fcntl.h> -#include <unistd.h> -#include <termios.h> - -#include <drivers/drv_hrt.h> - -#define DEBUG - -#include "px4io.h" - -#define DSM_FRAME_SIZE 16 -#define DSM_FRAME_CHANNELS 7 - -static int dsm_fd = -1; - -static hrt_abstime last_rx_time; -static hrt_abstime last_frame_time; - -static uint8_t frame[DSM_FRAME_SIZE]; - -static unsigned partial_frame_count; -static unsigned channel_shift; - -unsigned dsm_frame_drops; - -static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); -static void dsm_guess_format(bool reset); -static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values); - -int -dsm_init(const char *device) -{ - if (dsm_fd < 0) - dsm_fd = open(device, O_RDONLY | O_NONBLOCK); - - if (dsm_fd >= 0) { - struct termios t; - - /* 115200bps, no parity, one stop bit */ - tcgetattr(dsm_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(dsm_fd, TCSANOW, &t); - - /* initialise the decoder */ - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); - - /* reset the format detector */ - dsm_guess_format(true); - - debug("DSM: ready"); - - } else { - debug("DSM: open failed"); - } - - return dsm_fd; -} - -bool -dsm_input(uint16_t *values, uint16_t *num_values) -{ - ssize_t ret; - hrt_abstime now; - - /* - * The DSM* protocol doesn't provide any explicit framing, - * so we detect frame boundaries by the inter-frame delay. - * - * The minimum frame spacing is 11ms; with 16 bytes at 115200bps - * frame transmission time is ~1.4ms. - * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 5ms passes between calls, - * the first byte we read will be the first byte of a frame. - * - * In the case where byte(s) are dropped from a frame, this also - * provides a degree of protection. Of course, it would be better - * if we didn't drop bytes... - */ - now = hrt_absolute_time(); - - if ((now - last_rx_time) > 5000) { - if (partial_frame_count > 0) { - dsm_frame_drops++; - partial_frame_count = 0; - } - } - - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count); - - /* if the read failed for any reason, just give up here */ - if (ret < 1) - return false; - - last_rx_time = now; - - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < DSM_FRAME_SIZE) - return false; - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - return dsm_decode(now, values, num_values); -} - -static bool -dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) -{ - - if (raw == 0xffff) - return false; - - *channel = (raw >> shift) & 0xf; - - uint16_t data_mask = (1 << shift) - 1; - *value = raw & data_mask; - - //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); - - return true; -} - -static void -dsm_guess_format(bool reset) -{ - static uint32_t cs10; - static uint32_t cs11; - static unsigned samples; - - /* reset the 10/11 bit sniffed channel masks */ - if (reset) { - cs10 = 0; - cs11 = 0; - samples = 0; - channel_shift = 0; - return; - } - - /* scan the channels in the current frame in both 10- and 11-bit mode */ - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - - uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; - - /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) - cs10 |= (1 << channel); - - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) - cs11 |= (1 << channel); - - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */ - } - - /* wait until we have seen plenty of frames - 2 should normally be enough */ - if (samples++ < 5) - return; - - /* - * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognise. - * - * XXX Note that due to what seem to be bugs in the DSM2 high-resolution - * stream, we may want to sniff for longer in some cases when we think we - * are talking to a DSM2 receiver in high-resolution mode (so that we can - * reject it, ideally). - * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion - * of this issue. - */ - static uint32_t masks[] = { - 0x3f, /* 6 channels (DX6) */ - 0x7f, /* 7 channels (DX7) */ - 0xff, /* 8 channels (DX8) */ - 0x1ff, /* 9 channels (DX9, etc.) */ - 0x3ff, /* 10 channels (DX10) */ - 0x3fff /* 18 channels (DX10) */ - }; - unsigned votes10 = 0; - unsigned votes11 = 0; - - for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) { - - if (cs10 == masks[i]) - votes10++; - - if (cs11 == masks[i]) - votes11++; - } - - if ((votes11 == 1) && (votes10 == 0)) { - channel_shift = 11; - debug("DSM: 11-bit format"); - return; - } - - if ((votes10 == 1) && (votes11 == 0)) { - channel_shift = 10; - debug("DSM: 10-bit format"); - return; - } - - /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); - dsm_guess_format(true); -} - -static bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) -{ - - /* - debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], - frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); - */ - /* - * If we have lost signal for at least a second, reset the - * format guessing heuristic. - */ - if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) - dsm_guess_format(true); - - /* we have received something we think is a frame */ - last_frame_time = frame_time; - - /* if we don't know the frame format, update the guessing state machine */ - if (channel_shift == 0) { - dsm_guess_format(false); - return false; - } - - /* - * The encoding of the first two bytes is uncertain, so we're - * going to ignore them for now. - * - * Each channel is a 16-bit unsigned value containing either a 10- - * or 11-bit channel value and a 4-bit channel number, shifted - * either 10 or 11 bits. The MSB may also be set to indicate the - * second frame in variants of the protocol where more than - * seven channels are being transmitted. - */ - - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - - uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; - - if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) - continue; - - /* ignore channels out of range */ - if (channel >= PX4IO_INPUT_CHANNELS) - continue; - - /* update the decoded channel count */ - if (channel >= *num_values) - *num_values = channel + 1; - - /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - if (channel_shift == 11) - value /= 2; - - value += 998; - - /* - * Store the decoded channel into the R/C input buffer, taking into - * account the different ideas about channel assignement that we have. - * - * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, - * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. - */ - switch (channel) { - case 0: - channel = 2; - break; - - case 1: - channel = 0; - break; - - case 2: - channel = 1; - - default: - break; - } - - values[channel] = value; - } - - /* - * XXX Note that we may be in failsafe here; we need to work out how to detect that. - */ - return true; -} diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c deleted file mode 100644 index 4485daa5b..000000000 --- a/apps/px4io/i2c.c +++ /dev/null @@ -1,340 +0,0 @@ -/**************************************************************************** - * - * 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 i2c.c - * - * I2C communication for the PX4IO module. - */ - -#include <stdint.h> - -#include <nuttx/arch.h> -#include <arch/board/board.h> -#include <stm32_i2c.h> -#include <stm32_dma.h> - -//#define DEBUG -#include "px4io.h" - -/* - * I2C register definitions. - */ -#define I2C_BASE STM32_I2C1_BASE - -#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) - -#define rCR1 REG(STM32_I2C_CR1_OFFSET) -#define rCR2 REG(STM32_I2C_CR2_OFFSET) -#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) -#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) -#define rDR REG(STM32_I2C_DR_OFFSET) -#define rSR1 REG(STM32_I2C_SR1_OFFSET) -#define rSR2 REG(STM32_I2C_SR2_OFFSET) -#define rCCR REG(STM32_I2C_CCR_OFFSET) -#define rTRISE REG(STM32_I2C_TRISE_OFFSET) - -static int i2c_interrupt(int irq, void *context); -static void i2c_rx_setup(void); -static void i2c_tx_setup(void); -static void i2c_rx_complete(void); -static void i2c_tx_complete(void); - -static DMA_HANDLE rx_dma; -static DMA_HANDLE tx_dma; - -static uint8_t rx_buf[68]; -static unsigned rx_len; - -static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff }; - -static const uint8_t *tx_buf = junk_buf; -static unsigned tx_len = sizeof(junk_buf); -unsigned tx_count; - -static uint8_t selected_page; -static uint8_t selected_offset; - -enum { - DIR_NONE = 0, - DIR_TX = 1, - DIR_RX = 2 -} direction; - -void -i2c_init(void) -{ - debug("i2c init"); - - /* allocate DMA handles and initialise DMA */ - rx_dma = stm32_dmachannel(DMACHAN_I2C1_RX); - i2c_rx_setup(); - tx_dma = stm32_dmachannel(DMACHAN_I2C1_TX); - i2c_tx_setup(); - - /* enable the i2c block clock and reset it */ - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); - - /* configure the i2c GPIOs */ - stm32_configgpio(GPIO_I2C1_SCL); - stm32_configgpio(GPIO_I2C1_SDA); - - /* soft-reset the block */ - rCR1 |= I2C_CR1_SWRST; - rCR1 = 0; - - /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; - - /* set the frequency value in CR2 */ - rCR2 &= ~I2C_CR2_FREQ_MASK; - rCR2 |= STM32_PCLK1_FREQUENCY / 1000000; - - /* set divisor and risetime for fast mode */ - uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) - result = 1; - result = 3; - rCCR &= ~I2C_CCR_CCR_MASK; - rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; - rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1); - - /* set our device address */ - rOAR1 = 0x1a << 1; - - /* enable event interrupts */ - irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt); - irq_attach(STM32_IRQ_I2C1ER, i2c_interrupt); - up_enable_irq(STM32_IRQ_I2C1EV); - up_enable_irq(STM32_IRQ_I2C1ER); - - /* and enable the I2C port */ - rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; - -#ifdef DEBUG - i2c_dump(); -#endif -} - - -/* - reset the I2C bus - used to recover from lockups - */ -void i2c_reset(void) -{ - rCR1 |= I2C_CR1_SWRST; - rCR1 = 0; - - /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; - - /* set the frequency value in CR2 */ - rCR2 &= ~I2C_CR2_FREQ_MASK; - rCR2 |= STM32_PCLK1_FREQUENCY / 1000000; - - /* set divisor and risetime for fast mode */ - uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) - result = 1; - result = 3; - rCCR &= ~I2C_CCR_CCR_MASK; - rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; - rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1); - - /* set our device address */ - rOAR1 = 0x1a << 1; - - /* and enable the I2C port */ - rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; -} - -static int -i2c_interrupt(int irq, FAR void *context) -{ - uint16_t sr1 = rSR1; - - if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) { - - if (sr1 & I2C_SR1_STOPF) { - /* write to CR1 to clear STOPF */ - (void)rSR1; /* as recommended, re-read SR1 */ - rCR1 |= I2C_CR1_PE; - } - - /* DMA never stops, so we should do that now */ - switch (direction) { - case DIR_TX: - i2c_tx_complete(); - break; - case DIR_RX: - i2c_rx_complete(); - break; - default: - /* not currently transferring - must be a new txn */ - break; - } - direction = DIR_NONE; - } - - if (sr1 & I2C_SR1_ADDR) { - - /* clear ADDR to ack our selection and get direction */ - (void)rSR1; /* as recommended, re-read SR1 */ - uint16_t sr2 = rSR2; - - if (sr2 & I2C_SR2_TRA) { - /* we are the transmitter */ - - direction = DIR_TX; - - } else { - /* we are the receiver */ - - direction = DIR_RX; - } - } - - /* clear any errors that might need it (this handles AF as well */ - if (sr1 & I2C_SR1_ERRORMASK) - rSR1 = 0; - - return 0; -} - -static void -i2c_rx_setup(void) -{ - /* - * Note that we configure DMA in circular mode; this means that a too-long - * transfer will overwrite the buffer, but that avoids us having to deal with - * bailing out of a transaction while the master is still babbling at us. - */ - rx_len = 0; - stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_32BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); - - stm32_dmastart(rx_dma, NULL, NULL, false); -} - -static void -i2c_rx_complete(void) -{ - rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); - stm32_dmastop(rx_dma); - - if (rx_len >= 2) { - selected_page = rx_buf[0]; - selected_offset = rx_buf[1]; - - /* work out how many registers are being written */ - unsigned count = (rx_len - 2) / 2; - if (count > 0) { - registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count); - } else { - /* no registers written, must be an address cycle */ - uint16_t *regs; - unsigned reg_count; - - /* work out which registers are being addressed */ - int ret = registers_get(selected_page, selected_offset, ®s, ®_count); - if (ret == 0) { - tx_buf = (uint8_t *)regs; - tx_len = reg_count * 2; - } else { - tx_buf = junk_buf; - tx_len = sizeof(junk_buf); - } - - /* disable interrupts while reconfiguring DMA for the selected registers */ - irqstate_t flags = irqsave(); - - stm32_dmastop(tx_dma); - i2c_tx_setup(); - - irqrestore(flags); - } - } - - /* prepare for the next transaction */ - i2c_rx_setup(); -} - -static void -i2c_tx_setup(void) -{ - /* - * Note that we configure DMA in circular mode; this means that a too-long - * transfer will copy the buffer more than once, but that avoids us having - * to deal with bailing out of a transaction while the master is still - * babbling at us. - */ - stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, - DMA_CCR_DIR | - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); - - stm32_dmastart(tx_dma, NULL, NULL, false); -} - -static void -i2c_tx_complete(void) -{ - tx_count = tx_len - stm32_dmaresidual(tx_dma); - stm32_dmastop(tx_dma); - - /* for debug purposes, save the length of the last transmit as seen by the DMA */ - - /* leave tx_buf/tx_len alone, so that a retry will see the same data */ - - /* prepare for the next transaction */ - i2c_tx_setup(); -} - -void -i2c_dump(void) -{ - debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); - debug("OAR1 0x%08x OAR2 0x%08x", rOAR1, rOAR2); - debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); - debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); -} diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp deleted file mode 100644 index f38593d2a..000000000 --- a/apps/px4io/mixer.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/**************************************************************************** - * - * 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 mixer.cpp - * - * Control channel input/output mixer and failsafe. - */ - -#include <nuttx/config.h> -#include <syslog.h> - -#include <sys/types.h> -#include <stdbool.h> -#include <string.h> - -#include <drivers/drv_pwm_output.h> -#include <drivers/drv_hrt.h> - -#include <systemlib/mixer/mixer.h> - -extern "C" { -//#define DEBUG -#include "px4io.h" -} - -/* - * Maximum interval in us before FMU signal is considered lost - */ -#define FMU_INPUT_DROP_LIMIT_US 200000 - -/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ -#define ROLL 0 -#define PITCH 1 -#define YAW 2 -#define THROTTLE 3 -#define OVERRIDE 4 - -/* current servo arm/disarm state */ -static bool mixer_servos_armed = false; - -/* selected control values and count for mixing */ -enum mixer_source { - MIX_NONE, - MIX_FMU, - MIX_OVERRIDE, - MIX_FAILSAFE -}; -static mixer_source source; - -static int mixer_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &control); - -static MixerGroup mixer_group(mixer_callback, 0); - -void -mixer_tick(void) -{ - /* check that we are receiving fresh data from the FMU */ - if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { - - /* too long without FMU input, time to go to failsafe */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - isr_debug(1, "AP RX timeout"); - } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); - r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; - - } else { - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; - } - - source = MIX_FAILSAFE; - - /* - * Decide which set of controls we're using. - */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - - /* don't actually mix anything - we already have raw PWM values or - not a valid mixer. */ - source = MIX_NONE; - - } else { - - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - - /* mix from FMU controls */ - source = MIX_FMU; - } - - if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - - /* if allowed, mix from RC inputs directly */ - source = MIX_OVERRIDE; - } - } - - /* - * Run the mixers. - */ - if (source == MIX_FAILSAFE) { - - /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) - r_page_servos[i] = r_page_servo_failsafe[i]; - - } else if (source != MIX_NONE) { - - float outputs[IO_SERVO_COUNT]; - unsigned mixed; - - /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); - - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < mixed; i++) { - - /* save actuator values for FMU readback */ - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 500.0f) + 1500; - - } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) - r_page_servos[i] = 0; - } - - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - bool should_arm = ( - /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* FMU is available or FMU is not available but override is an option */ - ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) - ); - - if (should_arm && !mixer_servos_armed) { - /* need to arm, but not armed */ - up_pwm_servo_arm(true); - mixer_servos_armed = true; - - } else if (!should_arm && mixer_servos_armed) { - /* armed but need to disarm */ - up_pwm_servo_arm(false); - mixer_servos_armed = false; - } - - if (mixer_servos_armed) { - /* update the servo outputs. */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) - up_pwm_servo_set(i, r_page_servos[i]); - } -} - -static int -mixer_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &control) -{ - if (control_group != 0) - return -1; - - switch (source) { - case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS) { - control = REG_TO_FLOAT(r_page_controls[control_index]); - break; - } - return -1; - - case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { - control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - break; - } - return -1; - - case MIX_FAILSAFE: - case MIX_NONE: - /* XXX we could allow for configuration of per-output failsafe values */ - return -1; - } - - return 0; -} - -/* - * XXX error handling here should be more aggressive; currently it is - * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has - * not loaded faithfully. - */ - -static char mixer_text[256]; /* large enough for one mixer */ -static unsigned mixer_text_length = 0; - -void -mixer_handle_text(const void *buffer, size_t length) -{ - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { - return; - } - - px4io_mixdata *msg = (px4io_mixdata *)buffer; - - isr_debug(2, "mix txt %u", length); - - if (length < sizeof(px4io_mixdata)) - return; - - unsigned text_length = length - sizeof(px4io_mixdata); - - switch (msg->action) { - case F2I_MIXER_ACTION_RESET: - isr_debug(2, "reset"); - - /* FIRST mark the mixer as invalid */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - /* THEN actually delete it */ - mixer_group.reset(); - mixer_text_length = 0; - - /* FALLTHROUGH */ - case F2I_MIXER_ACTION_APPEND: - isr_debug(2, "append %d", length); - - /* check for overflow - this is really fatal */ - /* XXX could add just what will fit & try to parse, then repeat... */ - if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - return; - } - - /* append mixer text and nul-terminate */ - memcpy(&mixer_text[mixer_text_length], msg->text, text_length); - mixer_text_length += text_length; - mixer_text[mixer_text_length] = '\0'; - isr_debug(2, "buflen %u", mixer_text_length); - - /* process the text buffer, adding new mixers as their descriptions can be parsed */ - unsigned resid = mixer_text_length; - mixer_group.load_from_buf(&mixer_text[0], resid); - - /* if anything was parsed */ - if (resid != mixer_text_length) { - - /* ideally, this should test resid == 0 ? */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; - - isr_debug(2, "used %u", mixer_text_length - resid); - - /* copy any leftover text to the base of the buffer for re-use */ - if (resid > 0) - memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); - - mixer_text_length = resid; - } - - break; - } -} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h deleted file mode 100644 index 8d8b7e941..000000000 --- a/apps/px4io/protocol.h +++ /dev/null @@ -1,204 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -/** - * @file protocol.h - * - * PX4IO I2C interface protocol. - * - * Communication is performed via writes to and reads from 16-bit virtual - * registers organised into pages of 255 registers each. - * - * The first two bytes of each write select a page and offset address - * respectively. Subsequent reads and writes increment the offset within - * the page. - * - * Most pages are readable or writable but not both. - * - * Note that some pages may permit offset values greater than 255, which - * can only be achieved by long writes. The offset does not wrap. - * - * Writes to unimplemented registers are ignored. Reads from unimplemented - * registers return undefined values. - * - * As convention, values that would be floating point in other parts of - * the PX4 system are expressed as signed integer values scaled by 10000, - * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and - * SIGNED_TO_REG macros to convert between register representation and - * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. - * - * Note that the implementation of readable pages prefers registers within - * readable pages to be densely packed. Page numbers do not need to be - * packed. - */ - -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 - -/* Per C, this is safe for all 2's complement systems */ -#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) -#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) - -#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) -#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) - -/* static configuration page */ -#define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ -#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ -#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ -#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ -#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ -#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ -#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ - -/* dynamic status page */ -#define PX4IO_PAGE_STATUS 1 -#define PX4IO_P_STATUS_FREEMEM 0 -#define PX4IO_P_STATUS_CPULOAD 1 - -#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ -#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ -#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ -#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ -#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ -#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ -#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ -#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ - -#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ -#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ -#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ -#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ -#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ -#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ - -#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */ - -/* array of post-mix actuator outputs, -10000..10000 */ -#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - -/* array of PWM servo output values, microseconds */ -#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - -/* array of raw RC input values, microseconds */ -#define PX4IO_PAGE_RAW_RC_INPUT 4 -#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ -#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ - -/* array of scaled RC input values, -10000..10000 */ -#define PX4IO_PAGE_RC_INPUT 5 -#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ -#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ - -/* array of raw ADC values */ -#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ - -/* PWM servo information */ -#define PX4IO_PAGE_PWM_INFO 7 -#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ - -/* setup page */ -#define PX4IO_PAGE_SETUP 100 -#define PX4IO_P_SETUP_FEATURES 0 - -#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ -#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */ -#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ -#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */ -#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ - -#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ -#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ -#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */ -#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ - -/* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ - -/* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 - -/* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ -#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ -#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ -#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ -#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */ -#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */ -#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */ -#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) -#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) -#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ - -/* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - -/* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - -/** - * As-needed mixer data upload. - * - * This message adds text to the mixer text buffer; the text - * buffer is drained as the definitions are consumed. - */ -#pragma pack(push, 1) -struct px4io_mixdata { - uint16_t f2i_mixer_magic; -#define F2I_MIXER_MAGIC 0x6d74 - - uint8_t action; -#define F2I_MIXER_ACTION_RESET 0 -#define F2I_MIXER_ACTION_APPEND 1 - - char text[0]; /* actual text size may vary */ -}; -#pragma pack(pop) - diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c deleted file mode 100644 index 9de37e118..000000000 --- a/apps/px4io/px4io.c +++ /dev/null @@ -1,231 +0,0 @@ -/**************************************************************************** - * - * 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 px4io.c - * Top-level logic for the PX4IO module. - */ - -#include <nuttx/config.h> - -#include <stdio.h> // required for task_create -#include <stdbool.h> -#include <stdlib.h> -#include <errno.h> -#include <string.h> -#include <poll.h> -#include <signal.h> - -#include <drivers/drv_pwm_output.h> -#include <drivers/drv_hrt.h> - -#include <systemlib/perf_counter.h> - -#include <stm32_uart.h> - -#define DEBUG -#include "px4io.h" - -__EXPORT int user_start(int argc, char *argv[]); - -extern void up_cxxinitialize(void); - -struct sys_state_s system_state; - -static struct hrt_call serial_dma_call; - -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; - -/* - * a set of debug buffers to allow us to send debug information from ISRs - */ - -static volatile uint32_t msg_counter; -static volatile uint32_t last_msg_counter; -static volatile uint8_t msg_next_out, msg_next_in; - -/* - * WARNING: too large buffers here consume the memory required - * for mixer handling. Do not allocate more than 80 bytes for - * output. - */ -#define NUM_MSG 2 -static char msg[NUM_MSG][40]; - -/* - * add a debug message to be printed on the console - */ -void -isr_debug(uint8_t level, const char *fmt, ...) -{ - if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { - return; - } - va_list ap; - va_start(ap, fmt); - vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); - va_end(ap); - msg_next_in = (msg_next_in+1) % NUM_MSG; - msg_counter++; -} - -/* - * show all pending debug messages - */ -static void -show_debug_messages(void) -{ - if (msg_counter != last_msg_counter) { - uint32_t n = msg_counter - last_msg_counter; - if (n > NUM_MSG) n = NUM_MSG; - last_msg_counter = msg_counter; - while (n--) { - debug("%s", msg[msg_next_out]); - msg_next_out = (msg_next_out+1) % NUM_MSG; - } - } -} - -int -user_start(int argc, char *argv[]) -{ - /* run C++ ctors before we go any further */ - up_cxxinitialize(); - - /* reset all to zero */ - memset(&system_state, 0, sizeof(system_state)); - - /* configure the high-resolution time/callout interface */ - hrt_init(); - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); - - /* print some startup info */ - lowsyslog("\nPX4IO: starting\n"); - - /* default all the LEDs to off while we start */ - LED_AMBER(false); - LED_BLUE(false); - LED_SAFETY(false); - - /* turn on servo power */ - POWER_SERVO(true); - - /* start the safety switch handler */ - safety_init(); - - /* configure the first 8 PWM outputs (i.e. all of them) */ - up_pwm_servo_init(0xff); - - /* initialise the control inputs */ - controls_init(); - - /* start the i2c handler */ - i2c_init(); - - /* add a performance counter for mixing */ - perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - - /* add a performance counter for controls */ - perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); - - /* and one for measuring the loop rate */ - perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); - - struct mallinfo minfo = mallinfo(); - lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); - -#if 0 - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { - lowsyslog("ERR: not enough MEM"); - bool phase = false; - - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } - - phase = !phase; - usleep(300000); - } -#endif - - /* - * Run everything in a tight loop. - */ - - uint64_t last_debug_time = 0; - for (;;) { - - /* track the rate at which the loop is running */ - perf_count(loop_perf); - - /* kick the mixer */ - perf_begin(mixer_perf); - mixer_tick(); - perf_end(mixer_perf); - - /* kick the control inputs */ - perf_begin(controls_perf); - controls_tick(); - perf_end(controls_perf); - - /* check for debug activity */ - show_debug_messages(); - - /* post debug state at ~1Hz */ - if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - - struct mallinfo minfo = mallinfo(); - - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", - (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], - (unsigned)r_status_flags, - (unsigned)r_setup_arming, - (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, - (unsigned)minfo.mxordblk); - last_debug_time = hrt_absolute_time(); - } - } -} - diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h deleted file mode 100644 index 202e9d9d9..000000000 --- a/apps/px4io/px4io.h +++ /dev/null @@ -1,188 +0,0 @@ -/**************************************************************************** - * - * 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 px4io.h - * - * General defines and structures for the PX4IO module firmware. - */ - -#include <nuttx/config.h> - -#include <stdbool.h> -#include <stdint.h> - -#include <drivers/boards/px4io/px4io_internal.h> - -#include "protocol.h" - -/* - * Constants and limits. - */ -#define MAX_CONTROL_CHANNELS 12 -#define IO_SERVO_COUNT 8 - -/* - * Debug logging - */ - -#ifdef DEBUG -# include <debug.h> -# define debug(fmt, args...) lowsyslog(fmt "\n", ##args) -#else -# define debug(fmt, args...) do {} while(0) -#endif - -/* - * Registers. - */ -extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */ -extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */ -extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */ -extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */ -extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */ -extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */ - -extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ -extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ -extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ -extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ - -/* - * Register aliases. - * - * Handy aliases for registers that are widely used. - */ -#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS] -#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS] - -#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] -#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) -#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) - -#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] -#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] -#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] -#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] -#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] -#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] - -#define r_control_values (&r_page_controls[0]) - -/* - * System state structure. - */ -struct sys_state_s { - - volatile uint64_t rc_channels_timestamp; - - /** - * Last FMU receive time, in microseconds since system boot - */ - volatile uint64_t fmu_data_received_time; - -}; - -extern struct sys_state_s system_state; - -/* - * GPIO handling. - */ -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) -#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) - -#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) -#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) - -#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) -#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) - -#define ADC_VBATT 4 -#define ADC_IN5 5 -#define ADC_CHANNEL_COUNT 2 - -/* - * Mixer - */ -extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); - -/** - * Safety switch/LED. - */ -extern void safety_init(void); - -/** - * FMU communications - */ -extern void i2c_init(void); - -/** - * Register space - */ -extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); -extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); - -/** - * Sensors/misc inputs - */ -extern int adc_init(void); -extern uint16_t adc_measure(unsigned channel); - -/** - * R/C receiver handling. - * - * Input functions return true when they receive an update from the RC controller. - */ -extern void controls_init(void); -extern void controls_tick(void); -extern int dsm_init(const char *device); -extern bool dsm_input(uint16_t *values, uint16_t *num_values); -extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values); - -/** global debug level for isr_debug() */ -extern volatile uint8_t debug_level; - -/* send a debug message to the console */ -extern void isr_debug(uint8_t level, const char *fmt, ...); - -void i2c_dump(void); -void i2c_reset(void); - diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c deleted file mode 100644 index 6c09def9e..000000000 --- a/apps/px4io/registers.c +++ /dev/null @@ -1,644 +0,0 @@ -/**************************************************************************** - * - * 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 registers.c - * - * Implementation of the PX4IO register space. - */ - -#include <nuttx/config.h> - -#include <stdbool.h> -#include <stdlib.h> - -#include <drivers/drv_hrt.h> - -#include "px4io.h" -#include "protocol.h" - -static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); -static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); - -/** - * PAGE 0 - * - * Static configuration parameters. - */ -static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, - [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, -}; - -/** - * PAGE 1 - * - * Status values. - */ -uint16_t r_page_status[] = { - [PX4IO_P_STATUS_FREEMEM] = 0, - [PX4IO_P_STATUS_CPULOAD] = 0, - [PX4IO_P_STATUS_FLAGS] = 0, - [PX4IO_P_STATUS_ALARMS] = 0, - [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_IBATT] = 0 -}; - -/** - * PAGE 2 - * - * Post-mixed actuator values. - */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; - -/** - * PAGE 3 - * - * Servo PWM values - */ -uint16_t r_page_servos[IO_SERVO_COUNT]; - -/** - * PAGE 4 - * - * Raw RC input - */ -uint16_t r_page_raw_rc_input[] = -{ - [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 -}; - -/** - * PAGE 5 - * - * Scaled/routed RC input - */ -uint16_t r_page_rc_input[] = { - [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 -}; - -/** - * Scratch page; used for registers that are constructed as-read. - * - * PAGE 6 Raw ADC input. - * PAGE 7 PWM rate maps. - */ -uint16_t r_page_scratch[32]; - -/** - * PAGE 100 - * - * Setup registers - */ -volatile uint16_t r_page_setup[] = -{ - [PX4IO_P_SETUP_FEATURES] = 0, - [PX4IO_P_SETUP_ARMING] = 0, - [PX4IO_P_SETUP_PWM_RATES] = 0, - [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, - [PX4IO_P_SETUP_PWM_ALTRATE] = 200, - [PX4IO_P_SETUP_RELAYS] = 0, - [PX4IO_P_SETUP_VBATT_SCALE] = 10000, - [PX4IO_P_SETUP_IBATT_SCALE] = 0, - [PX4IO_P_SETUP_IBATT_BIAS] = 0, - [PX4IO_P_SETUP_SET_DEBUG] = 0, -}; - -#define PX4IO_P_SETUP_FEATURES_VALID (0) -#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \ - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) -#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) -#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) - -/** - * PAGE 101 - * - * Control values from the FMU. - */ -volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; - -/* - * PAGE 102 does not have a buffer. - */ - -/** - * PAGE 103 - * - * R/C channel input configuration. - */ -uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; - -/* valid options */ -#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) - -/* - * PAGE 104 uses r_page_servos. - */ - -/** - * PAGE 105 - * - * Failsafe servo PWM values - */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; - -void -registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) -{ - - switch (page) { - - /* handle bulk controls input */ - case PX4IO_PAGE_CONTROLS: - - /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { - - /* XXX range-check value? */ - r_page_controls[offset] = *values; - - offset++; - num_values--; - values++; - } - - system_state.fmu_data_received_time = hrt_absolute_time(); - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - - break; - - /* handle raw PWM input */ - case PX4IO_PAGE_DIRECT_PWM: - - /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { - - /* XXX range-check value? */ - r_page_servos[offset] = *values; - - offset++; - num_values--; - values++; - } - - system_state.fmu_data_received_time = hrt_absolute_time(); - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; - - break; - - /* handle setup for servo failsafe values */ - case PX4IO_PAGE_FAILSAFE_PWM: - - /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { - - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; - - offset++; - num_values--; - values++; - } - break; - - /* handle text going to the mixer parser */ - case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); - break; - - default: - /* avoid offset wrap */ - if ((offset + num_values) > 255) - num_values = 255 - offset; - - /* iterate individual registers, set each in turn */ - while (num_values--) { - if (registers_set_one(page, offset, *values)) - break; - offset++; - values++; - } - } -} - -static int -registers_set_one(uint8_t page, uint8_t offset, uint16_t value) -{ - switch (page) { - - case PX4IO_PAGE_STATUS: - switch (offset) { - case PX4IO_P_STATUS_ALARMS: - /* clear bits being written */ - r_status_alarms &= ~value; - break; - - case PX4IO_P_STATUS_FLAGS: - /* - * Allow FMU override of arming state (to allow in-air restores), - * but only if the arming state is not in sync on the IO side. - */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { - r_status_flags = value; - } - break; - - default: - /* just ignore writes to other registers in this page */ - break; - } - break; - - case PX4IO_PAGE_SETUP: - switch (offset) { - case PX4IO_P_SETUP_FEATURES: - - value &= PX4IO_P_SETUP_FEATURES_VALID; - r_setup_features = value; - - /* no implemented feature selection at this point */ - - break; - - case PX4IO_P_SETUP_ARMING: - - value &= PX4IO_P_SETUP_ARMING_VALID; - - /* - * Update arming state - disarm if no longer OK. - * This builds on the requirement that the FMU driver - * asks about the FMU arming state on initialization, - * so that an in-air reset of FMU can not lead to a - * lockup of the IO arming state. - */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - } - - r_setup_arming = value; - - break; - - case PX4IO_P_SETUP_PWM_RATES: - value &= PX4IO_P_SETUP_RATES_VALID; - pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); - break; - - case PX4IO_P_SETUP_PWM_DEFAULTRATE: - if (value < 50) - value = 50; - if (value > 400) - value = 400; - pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); - break; - - case PX4IO_P_SETUP_PWM_ALTRATE: - if (value < 50) - value = 50; - if (value > 400) - value = 400; - pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); - break; - - case PX4IO_P_SETUP_RELAYS: - value &= PX4IO_P_SETUP_RELAYS_VALID; - r_setup_relays = value; - POWER_RELAY1(value & (1 << 0) ? 1 : 0); - POWER_RELAY2(value & (1 << 1) ? 1 : 0); - POWER_ACC1(value & (1 << 2) ? 1 : 0); - POWER_ACC2(value & (1 << 3) ? 1 : 0); - break; - - case PX4IO_P_SETUP_SET_DEBUG: - r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; - isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); - break; - - default: - return -1; - } - break; - - case PX4IO_PAGE_RC_CONFIG: { - - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { - break; - } - - unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; - unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; - uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - - if (channel >= MAX_CONTROL_CHANNELS) - return -1; - - /* disable the channel until we have a chance to sanity-check it */ - conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - switch (index) { - - case PX4IO_P_RC_CONFIG_MIN: - case PX4IO_P_RC_CONFIG_CENTER: - case PX4IO_P_RC_CONFIG_MAX: - case PX4IO_P_RC_CONFIG_DEADZONE: - case PX4IO_P_RC_CONFIG_ASSIGNMENT: - conf[index] = value; - break; - - case PX4IO_P_RC_CONFIG_OPTIONS: - value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - - /* set all options except the enabled option */ - conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - /* should the channel be enabled? */ - /* this option is normally set last */ - if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - uint8_t count = 0; - - /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { - count++; - } - - /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { - count++; - } - // The following check isn't needed as constraint checks in controls.c will catch this. - //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - // count++; - //} - //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - // count++; - //} - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { - count++; - } - - /* sanity checks pass, enable channel */ - if (count) { - isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else { - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - } - } - break; - /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ - - } - break; - /* case PX4IO_RC_PAGE_CONFIG */ - } - - default: - return -1; - } - return 0; -} - -uint8_t last_page; -uint8_t last_offset; - -int -registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) -{ -#define SELECT_PAGE(_page_name) \ - do { \ - *values = &_page_name[0]; \ - *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ - } while(0) - - switch (page) { - - /* - * Handle pages that are updated dynamically at read time. - */ - case PX4IO_PAGE_STATUS: - /* PX4IO_P_STATUS_FREEMEM */ - { - struct mallinfo minfo = mallinfo(); - r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks; - } - - /* XXX PX4IO_P_STATUS_CPULOAD */ - - /* PX4IO_P_STATUS_FLAGS maintained externally */ - - /* PX4IO_P_STATUS_ALARMS maintained externally */ - - /* PX4IO_P_STATUS_VBATT */ - { - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit: - * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 - * - * slope = 0.0046067 - * intercept = 0.3863 - * - * Intercept corrected for best results @ 12V. - */ - unsigned counts = adc_measure(ADC_VBATT); - unsigned mV = (4150 + (counts * 46)) / 10 - 200; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; - - r_page_status[PX4IO_P_STATUS_VBATT] = corrected; - } - - /* PX4IO_P_STATUS_IBATT */ - { - unsigned counts = adc_measure(ADC_VBATT); - unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000; - int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]); - if (corrected < 0) - corrected = 0; - r_page_status[PX4IO_P_STATUS_IBATT] = corrected; - } - - SELECT_PAGE(r_page_status); - break; - - case PX4IO_PAGE_RAW_ADC_INPUT: - memset(r_page_scratch, 0, sizeof(r_page_scratch)); - r_page_scratch[0] = adc_measure(ADC_VBATT); - r_page_scratch[1] = adc_measure(ADC_IN5); - - SELECT_PAGE(r_page_scratch); - break; - - case PX4IO_PAGE_PWM_INFO: - memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) - r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); - - SELECT_PAGE(r_page_scratch); - break; - - /* - * Pages that are just a straight read of the register state. - */ - - /* status pages */ - case PX4IO_PAGE_CONFIG: - SELECT_PAGE(r_page_config); - break; - case PX4IO_PAGE_ACTUATORS: - SELECT_PAGE(r_page_actuators); - break; - case PX4IO_PAGE_SERVOS: - SELECT_PAGE(r_page_servos); - break; - case PX4IO_PAGE_RAW_RC_INPUT: - SELECT_PAGE(r_page_raw_rc_input); - break; - case PX4IO_PAGE_RC_INPUT: - SELECT_PAGE(r_page_rc_input); - break; - - /* readback of input pages */ - case PX4IO_PAGE_SETUP: - SELECT_PAGE(r_page_setup); - break; - case PX4IO_PAGE_CONTROLS: - SELECT_PAGE(r_page_controls); - break; - case PX4IO_PAGE_RC_CONFIG: - SELECT_PAGE(r_page_rc_input_config); - break; - case PX4IO_PAGE_DIRECT_PWM: - SELECT_PAGE(r_page_servos); - break; - case PX4IO_PAGE_FAILSAFE_PWM: - SELECT_PAGE(r_page_servo_failsafe); - break; - - default: - return -1; - } - -#undef SELECT_PAGE -#undef COPY_PAGE - -last_page = page; -last_offset = offset; - - /* if the offset is at or beyond the end of the page, we have no data */ - if (offset >= *num_values) - return -1; - - /* correct the data pointer and count for the offset */ - *values += offset; - *num_values -= offset; - - return 0; -} - -/* - * Helper function to handle changes to the PWM rate control registers. - */ -static void -pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) -{ - for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { - - /* get the channel mask for this rate group */ - uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) - continue; - - /* all channels in the group must be either default or alt-rate */ - uint32_t alt = map & mask; - - if (pass == 0) { - /* preflight */ - if ((alt != 0) && (alt != mask)) { - /* not a legal map, bail with an alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; - return; - } - } else { - /* set it - errors here are unexpected */ - if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) - r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; - } else { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) - r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; - } - } - } - } - r_setup_pwm_rates = map; - r_setup_pwm_defaultrate = defaultrate; - r_setup_pwm_altrate = altrate; -} diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c deleted file mode 100644 index 5495d5ae1..000000000 --- a/apps/px4io/safety.c +++ /dev/null @@ -1,195 +0,0 @@ -/**************************************************************************** - * - * 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 Safety button logic. - */ - -#include <nuttx/config.h> - -#include <stdbool.h> - -#include <drivers/drv_hrt.h> - -#include "px4io.h" - -static struct hrt_call arming_call; -static struct hrt_call heartbeat_call; -static struct hrt_call failsafe_call; - -/* - * Count the number of times in a row that we see the arming button - * held down. - */ -static unsigned counter = 0; - -/* - * Define the various LED flash sequences for each system state. - */ -#define LED_PATTERN_SAFE 0xffff /**< always on */ -#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */ -#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ -#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ -#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ - -static unsigned blink_counter = 0; - -/* - * IMPORTANT: The arming state machine critically - * depends on using the same threshold - * for arming and disarming. Since disarming - * is quite deadly for the system, a similar - * length can be justified. - */ -#define ARM_COUNTER_THRESHOLD 10 - -static bool safety_button_pressed; - -static void safety_check_button(void *arg); -static void heartbeat_blink(void *arg); -static void failsafe_blink(void *arg); - -void -safety_init(void) -{ - /* arrange for the button handler to be called at 10Hz */ - hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); - - /* arrange for the heartbeat handler to be called at 4Hz */ - hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); - - /* arrange for the failsafe blinker to be called at 8Hz */ - hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); -} - -static void -safety_check_button(void *arg) -{ - /* - * Debounce the safety button, change state if it has been held for long enough. - * - */ - safety_button_pressed = BUTTON_SAFETY; - - /* - * Keep pressed for a while to arm. - * - * Note that the counting sequence has to be same length - * for arming / disarming in order to end up as proper - * state machine, keep ARM_COUNTER_THRESHOLD the same - * length in all cases of the if/else struct below. - */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { - - if (counter < ARM_COUNTER_THRESHOLD) { - counter++; - - } else if (counter == ARM_COUNTER_THRESHOLD) { - /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; - counter++; - } - - /* Disarm quickly */ - - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { - - if (counter < ARM_COUNTER_THRESHOLD) { - counter++; - - } else if (counter == ARM_COUNTER_THRESHOLD) { - /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - counter++; - } - - } else { - counter = 0; - } - - /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ - uint16_t pattern = LED_PATTERN_SAFE; - - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { - if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { - pattern = LED_PATTERN_IO_FMU_ARMED; - - } else { - pattern = LED_PATTERN_IO_ARMED; - } - - } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { - pattern = LED_PATTERN_FMU_ARMED; - - } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) { - pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; - } - - /* Turn the LED on if we have a 1 at the current bit position */ - LED_SAFETY(pattern & (1 << blink_counter++)); - - if (blink_counter > 15) { - blink_counter = 0; - } -} - -static void -heartbeat_blink(void *arg) -{ - static bool heartbeat = false; - - /* XXX add flags here that need to be frobbed by various loops */ - - LED_BLUE(heartbeat = !heartbeat); -} - -static void -failsafe_blink(void *arg) -{ - /* indicate that a serious initialisation error occured */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { - LED_AMBER(true); - return; - } - - static bool failsafe = false; - - /* blink the failsafe LED if we don't have FMU input */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - failsafe = !failsafe; - } else { - failsafe = false; - } - - LED_AMBER(failsafe); -}
\ No newline at end of file diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c deleted file mode 100644 index 073ddeaba..000000000 --- a/apps/px4io/sbus.c +++ /dev/null @@ -1,255 +0,0 @@ -/**************************************************************************** - * - * 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 sbus.c - * - * Serial protocol decoder for the Futaba S.bus protocol. - */ - -#include <nuttx/config.h> - -#include <fcntl.h> -#include <unistd.h> -#include <termios.h> - -#include <systemlib/ppm_decode.h> - -#include <drivers/drv_hrt.h> - -#define DEBUG -#include "px4io.h" -#include "protocol.h" -#include "debug.h" - -#define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 16 - -static int sbus_fd = -1; - -static hrt_abstime last_rx_time; -static hrt_abstime last_frame_time; - -static uint8_t frame[SBUS_FRAME_SIZE]; - -static unsigned partial_frame_count; - -unsigned sbus_frame_drops; - -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); - -int -sbus_init(const char *device) -{ - if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY | O_NONBLOCK); - - if (sbus_fd >= 0) { - struct termios t; - - /* 100000bps, even parity, two stop bits */ - tcgetattr(sbus_fd, &t); - cfsetspeed(&t, 100000); - t.c_cflag |= (CSTOPB | PARENB); - tcsetattr(sbus_fd, TCSANOW, &t); - - /* initialise the decoder */ - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); - - debug("S.Bus: ready"); - - } else { - debug("S.Bus: open failed"); - } - - return sbus_fd; -} - -bool -sbus_input(uint16_t *values, uint16_t *num_values) -{ - ssize_t ret; - hrt_abstime now; - - /* - * The S.bus protocol doesn't provide reliable framing, - * so we detect frame boundaries by the inter-frame delay. - * - * The minimum frame spacing is 7ms; with 25 bytes at 100000bps - * frame transmission time is ~2ms. - * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 3ms passes between calls, - * the first byte we read will be the first byte of a frame. - * - * In the case where byte(s) are dropped from a frame, this also - * provides a degree of protection. Of course, it would be better - * if we didn't drop bytes... - */ - now = hrt_absolute_time(); - - if ((now - last_rx_time) > 3000) { - if (partial_frame_count > 0) { - sbus_frame_drops++; - partial_frame_count = 0; - } - } - - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); - - /* if the read failed for any reason, just give up here */ - if (ret < 1) - return false; - - last_rx_time = now; - - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < SBUS_FRAME_SIZE) - return false; - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - return sbus_decode(now, values, num_values); -} - -/* - * S.bus decoder matrix. - * - * Each channel value can come from up to 3 input bytes. Each row in the - * matrix describes up to three bytes, and each entry gives: - * - * - byte offset in the data portion of the frame - * - right shift applied to the data byte - * - mask for the data byte - * - left shift applied to the result into the channel value - */ -struct sbus_bit_pick { - uint8_t byte; - uint8_t rshift; - uint8_t mask; - uint8_t lshift; -}; -static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { - /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, - /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, - /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, - /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, - /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, - /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } -}; - -static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) -{ - /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { - sbus_frame_drops++; - return false; - } - - /* if the failsafe or connection lost bit is set, we consider the frame invalid */ - if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ - - /* actively announce signal loss */ - *values = 0; - return false; - } - - /* we have received something we think is a frame */ - last_frame_time = frame_time; - - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; - - /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < chancount; channel++) { - unsigned value = 0; - - for (unsigned pick = 0; pick < 3; pick++) { - const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; - - if (decode->mask != 0) { - unsigned piece = frame[1 + decode->byte]; - piece >>= decode->rshift; - piece &= decode->mask; - piece <<= decode->lshift; - - value |= piece; - } - } - - /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - values[channel] = (value / 2) + 998; - } - - /* decode switch channels if data fields are wide enough */ - if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { - chancount = 18; - - /* channel 17 (index 16) */ - values[16] = (frame[23] & (1 << 0)) * 1000 + 998; - /* channel 18 (index 17) */ - values[17] = (frame[23] & (1 << 1)) * 1000 + 998; - } - - /* note the number of channels decoded */ - *num_values = chancount; - - return true; -} |