diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-28 01:17:12 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-28 01:30:14 +0200 |
commit | f57439b90e23de260259dec051d3e2ead2d61c8c (patch) | |
tree | 5cd778d2cf0c9afb2851ab9042bbecc5b742a497 /src | |
parent | 8040b9b96e8f7c07aa981150c33f850096062f70 (diff) | |
download | px4-firmware-f57439b90e23de260259dec051d3e2ead2d61c8c.tar.gz px4-firmware-f57439b90e23de260259dec051d3e2ead2d61c8c.tar.bz2 px4-firmware-f57439b90e23de260259dec051d3e2ead2d61c8c.zip |
Moved all drivers to new world, PX4IO completely in new world
Diffstat (limited to 'src')
30 files changed, 7345 insertions, 2 deletions
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io/module.mk new file mode 100644 index 000000000..2601a1c15 --- /dev/null +++ b/src/drivers/boards/px4io/module.mk @@ -0,0 +1,6 @@ +# +# Board-specific startup code for the PX4IO +# + +SRCS = px4io_init.c \ + px4io_pwm_servo.c diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c new file mode 100644 index 000000000..14e8dc13a --- /dev/null +++ b/src/drivers/boards/px4io/px4io_init.c @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h new file mode 100644 index 000000000..44bb98513 --- /dev/null +++ b/src/drivers/boards/px4io/px4io_internal.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c new file mode 100644 index 000000000..a2f73c429 --- /dev/null +++ b/src/drivers/boards/px4io/px4io_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b21f83e44..4c2f1736f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,11 +80,11 @@ #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/parameter_update.h> +#include <debug.h> -#include <px4io/protocol.h> #include <mavlink/mavlink_log.h> #include "uploader.h" -#include <debug.h> +#include <modules/px4iofirmware/protocol.h> #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp new file mode 100644 index 000000000..911def943 --- /dev/null +++ b/src/drivers/stm32/adc/adc.cpp @@ -0,0 +1,387 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file adc.cpp + * + * Driver for the STM32 ADC. + * + * This is a low-rate driver, designed for sampling things like voltages + * and so forth. It avoids the gross complexity of the NuttX ADC driver. + */ + +#include <nuttx/config.h> +#include <drivers/device/device.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <stdio.h> +#include <unistd.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_adc.h> + +#include <arch/stm32/chip.h> +#include <stm32_internal.h> +#include <stm32_gpio.h> + +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> + +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) + +#define rSR REG(STM32_ADC_SR_OFFSET) +#define rCR1 REG(STM32_ADC_CR1_OFFSET) +#define rCR2 REG(STM32_ADC_CR2_OFFSET) +#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) +#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) +#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) +#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) +#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) +#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) +#define rHTR REG(STM32_ADC_HTR_OFFSET) +#define rLTR REG(STM32_ADC_LTR_OFFSET) +#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) +#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) +#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) +#define rJSQR REG(STM32_ADC_JSQR_OFFSET) +#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) +#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) +#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) +#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) +#define rDR REG(STM32_ADC_DR_OFFSET) + +#ifdef STM32_ADC_CCR +# define rCCR REG(STM32_ADC_CCR_OFFSET) +#endif + +class ADC : public device::CDev +{ +public: + ADC(uint32_t channels); + ~ADC(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t read(file *filp, char *buffer, size_t len); + +protected: + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + +private: + static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ + + hrt_call _call; + perf_counter_t _sample_perf; + + unsigned _channel_count; + adc_msg_s *_samples; /**< sample buffer */ + + /** work trampoline */ + static void _tick_trampoline(void *arg); + + /** worker function */ + void _tick(); + + /** + * Sample a single channel and return the measured value. + * + * @param channel The channel to sample. + * @return The sampled value, or 0xffff if + * sampling failed. + */ + uint16_t _sample(unsigned channel); + +}; + +ADC::ADC(uint32_t channels) : + CDev("adc", ADC_DEVICE_PATH), + _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), + _channel_count(0), + _samples(nullptr) +{ + _debug_enabled = true; + + /* always enable the temperature sensor */ + channels |= 1 << 16; + + /* allocate the sample array */ + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _channel_count++; + } + } + _samples = new adc_msg_s[_channel_count]; + + /* prefill the channel numbers in the sample array */ + if (_samples != nullptr) { + unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _samples[index].am_channel = i; + _samples[index].am_data = 0; + index++; + } + } + } +} + +ADC::~ADC() +{ + if (_samples != nullptr) + delete _samples; +} + +int +ADC::init() +{ + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_CAL; + usleep(100); + if (rCR2 & ADC_CR2_CAL) + return -1; +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1 = 0b00000011011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1 = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2 = +#ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ + ADC_CR2_TSVREFE | +#endif + 0; + +#ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR = ADC_CCR_TSVREFE; +#endif + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 0; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2 &= ~ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + rCR2 |= ADC_CR2_ADON; + usleep(10); + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + rCR2 |= ADC_CR2_SWSTART; + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 500) { + log("sample timeout"); + return -1; + return 0xffff; + } + } + + + debug("init done"); + + /* create the device node */ + return CDev::init(); +} + +int +ADC::ioctl(file *filp, int cmd, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t +ADC::read(file *filp, char *buffer, size_t len) +{ + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) + len = maxsize; + + /* block interrupts while copying samples to avoid racing with an update */ + irqstate_t flags = irqsave(); + memcpy(buffer, _samples, len); + irqrestore(flags); + + return len; +} + +int +ADC::open_first(struct file *filp) +{ + /* get fresh data */ + _tick(); + + /* and schedule regular updates */ + hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + + return 0; +} + +int +ADC::close_last(struct file *filp) +{ + hrt_cancel(&_call); + return 0; +} + +void +ADC::_tick_trampoline(void *arg) +{ + ((ADC *)arg)->_tick(); +} + +void +ADC::_tick() +{ + /* scan the channel set and sample each */ + for (unsigned i = 0; i < _channel_count; i++) + _samples[i].am_data = _sample(_samples[i].am_channel); +} + +uint16_t +ADC::_sample(unsigned channel) +{ + perf_begin(_sample_perf); + + /* clear any previous EOC */ + if (rSR & ADC_SR_EOC) + rSR &= ~ADC_SR_EOC; + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_SWSTART; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { + + /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 50) { + log("sample timeout"); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + perf_end(_sample_perf); + return result; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int adc_main(int argc, char *argv[]); + +namespace +{ +ADC *g_adc; + +void +test(void) +{ + + int fd = open(ADC_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "can't open ADC device"); + + for (unsigned i = 0; i < 50; i++) { + adc_msg_s data[8]; + ssize_t count = read(fd, data, sizeof(data)); + + if (count < 0) + errx(1, "read error"); + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + printf ("%d: %u ", data[j].am_channel, data[j].am_data); + } + + printf("\n"); + usleep(500000); + } + + exit(0); +} +} + +int +adc_main(int argc, char *argv[]) +{ + if (g_adc == nullptr) { + /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); + + if (g_adc == nullptr) + errx(1, "couldn't allocate the ADC driver"); + + if (g_adc->init() != OK) { + delete g_adc; + errx(1, "ADC init failed"); + } + } + + if (argc > 1) { + if (!strcmp(argv[1], "test")) + test(); + } + + exit(0); +} diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk new file mode 100644 index 000000000..48620feea --- /dev/null +++ b/src/drivers/stm32/adc/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# STM32 ADC driver +# + +MODULE_COMMAND = adc + +SRCS = adc.cpp + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c new file mode 100644 index 000000000..cec0c49ff --- /dev/null +++ b/src/drivers/stm32/drv_hrt.c @@ -0,0 +1,908 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_hrt.c + * + * High-resolution timer callouts and timekeeping. + * + * This can use any general or advanced STM32 timer. + * + * Note that really, this could use systick too, but that's + * monopolised by NuttX and stealing it would just be awkward. + * + * We don't use the NuttX STM32 driver per se; rather, we + * claim the timer and then drive it directly. + */ + +#include <nuttx/config.h> +#include <nuttx/arch.h> +#include <nuttx/irq.h> + +#include <sys/types.h> +#include <stdbool.h> + +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32_internal.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" + +#ifdef CONFIG_HRT_TIMER + +/* HRT configuration */ +#if HRT_TIMER == 1 +# define HRT_TIMER_BASE STM32_TIM1_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1CC +# define HRT_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +# if CONFIG_STM32_TIM1 +# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1 +# endif +#elif HRT_TIMER == 2 +# define HRT_TIMER_BASE STM32_TIM2_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM2 +# define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN +# if CONFIG_STM32_TIM2 +# error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2 +# endif +#elif HRT_TIMER == 3 +# define HRT_TIMER_BASE STM32_TIM3_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM3 +# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN +# if CONFIG_STM32_TIM3 +# error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3 +# endif +#elif HRT_TIMER == 4 +# define HRT_TIMER_BASE STM32_TIM4_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM4EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM4 +# define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN +# if CONFIG_STM32_TIM4 +# error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4 +# endif +#elif HRT_TIMER == 5 +# define HRT_TIMER_BASE STM32_TIM5_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM5 +# define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN +# if CONFIG_STM32_TIM5 +# error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5 +# endif +#elif HRT_TIMER == 8 +# define HRT_TIMER_BASE STM32_TIM8_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC +# define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +# if CONFIG_STM32_TIM8 +# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8 +# endif +#elif HRT_TIMER == 9 +# define HRT_TIMER_BASE STM32_TIM9_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK +# define HRT_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +# if CONFIG_STM32_TIM9 +# error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9 +# endif +#elif HRT_TIMER == 10 +# define HRT_TIMER_BASE STM32_TIM10_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP +# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN +# if CONFIG_STM32_TIM10 +# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 +# endif +#elif HRT_TIMER == 11 +# define HRT_TIMER_BASE STM32_TIM11_BASE +# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN +# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN +# if CONFIG_STM32_TIM11 +# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 +# endif +#else +# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y +#endif + +/* + * HRT clock must be a multiple of 1MHz greater than 1MHz + */ +#if (HRT_TIMER_CLOCK % 1000000) != 0 +# error HRT_TIMER_CLOCK must be a multiple of 1MHz +#endif +#if HRT_TIMER_CLOCK <= 1000000 +# error HRT_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * Minimum/maximum deadlines. + * + * These are suitable for use with a 16-bit timer/counter clocked + * at 1MHz. The high-resolution timer need only guarantee that it + * not wrap more than once in the 50ms period for absolute time to + * be consistently maintained. + * + * The minimum deadline must be such that the time taken between + * reading a time and writing a deadline to the timer cannot + * result in missing the deadline. + */ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* + * Period of the free-running counter, in microseconds. + */ +#define HRT_COUNTER_PERIOD 65536 + +/* + * Scaling factor(s) for the free-running counter; convert an input + * in counts to a time in microseconds. + */ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if HRT_TIMER_CHANNEL == 1 +# define rCCR_HRT rCCR1 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC1IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 2 +# define rCCR_HRT rCCR2 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC2IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC2IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 3 +# define rCCR_HRT rCCR3 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC3IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC3IF /* interrupt status for HRT */ +#elif HRT_TIMER_CHANNEL == 4 +# define rCCR_HRT rCCR4 /* compare register for HRT */ +# define DIER_HRT GTIM_DIER_CC4IE /* interrupt enable for HRT */ +# define SR_INT_HRT GTIM_SR_CC4IF /* interrupt status for HRT */ +#else +# error HRT_TIMER_CHANNEL must be a value between 1 and 4 +#endif + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint16_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint16_t latency_actual; + +/* latency histogram */ +#define LATENCY_BUCKET_COUNT 8 +static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context); +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + +/* + * Specific registers and bits used by PPM sub-functions + */ +#ifdef CONFIG_HRT_PPM +/* + * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. + */ +# ifndef GTIM_CCER_CC1NP +# define GTIM_CCER_CC1NP 0 +# define GTIM_CCER_CC2NP 0 +# define GTIM_CCER_CC3NP 0 +# define GTIM_CCER_CC4NP 0 +# define PPM_EDGE_FLIP +# endif + +# if HRT_PPM_CHANNEL == 1 +# define rCCR_PPM rCCR1 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC1IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC1OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 1 /* not on TI1/TI2 */ +# define CCMR2_PPM 0 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC1P +# elif HRT_PPM_CHANNEL == 2 +# define rCCR_PPM rCCR2 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC2IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC2OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 2 /* not on TI1/TI2 */ +# define CCMR2_PPM 0 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC2P +# elif HRT_PPM_CHANNEL == 3 +# define rCCR_PPM rCCR3 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC3IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC3OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 0 /* not on TI1/TI2 */ +# define CCMR2_PPM 1 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC3P +# elif HRT_PPM_CHANNEL == 4 +# define rCCR_PPM rCCR4 /* capture register for PPM */ +# define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */ +# define SR_INT_PPM GTIM_SR_CC4IF /* capture interrupt (non-DMA mode) */ +# define SR_OVF_PPM GTIM_SR_CC4OF /* capture overflow (non-DMA mode) */ +# define CCMR1_PPM 0 /* not on TI1/TI2 */ +# define CCMR2_PPM 2 /* on TI3, not on TI4 */ +# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC4P +# else +# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set +# endif + +/* + * PPM decoder tuning parameters + */ +# define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +# define PPM_MIN_START 2500 /* shortest valid start gap */ + +/* decoded PPM buffer */ +#define PPM_MAX_CHANNELS 12 +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT unsigned ppm_decoded_channels = 0; +__EXPORT uint64_t ppm_last_valid_decode = 0; + +/* PPM edge history */ +__EXPORT uint16_t ppm_edge_history[32]; +unsigned ppm_edge_next; + +/* PPM pulse history */ +__EXPORT uint16_t ppm_pulse_history[32]; +unsigned ppm_pulse_next; + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/* PPM decoder state machine */ +struct { + uint16_t last_edge; /* last capture time */ + uint16_t last_mark; /* last significant edge */ + unsigned next_channel; + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + +static void hrt_ppm_decode(uint32_t status); + +#else +/* disable the PPM configuration */ +# define rCCR_PPM 0 +# define DIER_PPM 0 +# define SR_INT_PPM 0 +# define SR_OVF_PPM 0 +# define CCMR1_PPM 0 +# define CCMR2_PPM 0 +# define CCER_PPM 0 +#endif /* CONFIG_HRT_PPM */ + +/* + * Initialise the timer we are going to use. + * + * We expect that we'll own one of the reduced-function STM32 general + * timers, and that we can use channel 1 in compare mode. + */ +static void +hrt_tim_init(void) +{ + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + + /* clock/power on our timer */ + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_HRT | DIER_PPM; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PPM; + rCCMR2 = CCMR2_PPM; + rCCER = CCER_PPM; + rDCR = 0; + + /* configure the timer to free-run at 1MHz */ + rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ + + /* run the full span of the counter */ + rARR = 0xffff; + + /* set an initial capture a little ways off */ + rCCR_HRT = 1000; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); +} + +#ifdef CONFIG_HRT_PPM +/* + * Handle the PPM decoder state machine. + */ +static void +hrt_ppm_decode(uint32_t status) +{ + uint16_t count = rCCR_PPM; + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PPM) + goto error; + + /* how long since the last edge? */ + width = count - ppm.last_edge; + ppm.last_edge = count; + + ppm_edge_history[ppm_edge_next++] = width; + + if (ppm_edge_next >= 32) + ppm_edge_next = 0; + + /* + * if this looks like a start pulse, then push the last set of values + * and reset the state machine + */ + if (width >= PPM_MIN_START) { + + /* export the last set of samples if we got something sensible */ + if (ppm.next_channel > 4) { + for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; + + ppm_decoded_channels = i; + ppm_last_valid_decode = hrt_absolute_time(); + + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + return; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = count; + ppm.phase = INACTIVE; + return; + + case INACTIVE: + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + return; + + case ACTIVE: + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + + ppm_pulse_history[ppm_pulse_next++] = interval; + + if (ppm_pulse_next >= 32) + ppm_pulse_next = 0; + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + goto error; + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) + ppm_temp_buffer[ppm.next_channel++] = interval; + + ppm.phase = INACTIVE; + return; + + } + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; + +} +#endif /* CONFIG_HRT_PPM */ + +/* + * Handle the compare interupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_tim_isr(int irq, void *context) +{ + uint32_t status; + + /* grab the timer for latency tracking purposes */ + latency_actual = rCNT; + + /* copy interrupt status */ + status = rSR; + + /* ack the interrupts we just read */ + rSR = ~status; + +#ifdef CONFIG_HRT_PPM + + /* was this a PPM edge? */ + if (status & (SR_INT_PPM | SR_OVF_PPM)) { + /* if required, flip edge sensitivity */ +# ifdef PPM_EDGE_FLIP + rCCER ^= CCER_PPM_FLIP; +# endif + + hrt_ppm_decode(status); + } +#endif + + /* was this a timer tick? */ + if (status & SR_INT_HRT) { + + /* do latency calculations */ + hrt_latency_update(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + } + + return OK; +} + +/* + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ + hrt_abstime abstime; + uint32_t count; + irqstate_t flags; + + /* + * Counter state. Marked volatile as they may change + * inside this routine but outside the irqsave/restore + * pair. Discourage the compiler from moving loads/stores + * to these outside of the protected range. + */ + static volatile hrt_abstime base_time; + static volatile uint32_t last_count; + + /* prevent re-entry */ + flags = irqsave(); + + /* get the current counter value */ + count = rCNT; + + /* + * Determine whether the counter has wrapped since the + * last time we're called. + * + * This simple test is sufficient due to the guarantee that + * we are always called at least once per counter period. + */ + if (count < last_count) + base_time += HRT_COUNTER_PERIOD; + + /* save the count for next time */ + last_count = count; + + /* compute the current time */ + abstime = HRT_COUNTER_SCALE(base_time + count); + + irqrestore(flags); + + return abstime; +} + +/* + * Convert a timespec to absolute time + */ +hrt_abstime +ts_to_abstime(struct timespec *ts) +{ + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; +} + +/* + * Convert absolute time to a timespec. + */ +void +abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +{ + ts->tv_sec = abstime / 1000000; + abstime -= ts->tv_sec * 1000000; + ts->tv_nsec = abstime * 1000; +} + +/* + * Compare a time value with the current time. + */ +hrt_abstime +hrt_elapsed_time(const volatile hrt_abstime *then) +{ + irqstate_t flags = irqsave(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + irqrestore(flags); + + return delta; +} + +/* + * Store the absolute time in an interrupt-safe fashion + */ +hrt_abstime +hrt_store_absolute_time(volatile hrt_abstime *now) +{ + irqstate_t flags = irqsave(); + + hrt_abstime ts = hrt_absolute_time(); + + irqrestore(flags); + + return ts; +} + +/* + * Initalise the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); + +#ifdef CONFIG_HRT_PPM + /* configure the PPM input pin */ + stm32_configgpio(GPIO_PPM_IN); +#endif +} + +/* + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/* + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/* + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = irqsave(); + + /* if the entry is currently queued, remove it */ + if (entry->deadline != 0) + sq_rem(&entry->link, &callout_queue); + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + irqrestore(flags); +} + +/* + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/* + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = irqsave(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + irqrestore(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + //lldbg("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + //lldbg("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) + break; + + if (call->deadline > now) + break; + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + call->deadline = deadline + call->period; + hrt_call_enter(call); + } + } +} + +/* + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + //lldbg("entry in queue\n"); + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + //lldbg("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + deadline = next->deadline; + } + } + + //lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + rCCR_HRT = latency_baseline = deadline & 0xffff; +} + +static void +hrt_latency_update(void) +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + + +#endif /* CONFIG_HRT_TIMER */ diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c new file mode 100644 index 000000000..c1efb8515 --- /dev/null +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -0,0 +1,332 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have output pins, does not require an interrupt. + */ + +#include <nuttx/config.h> +#include <nuttx/arch.h> +#include <nuttx/irq.h> + +#include <sys/types.h> +#include <stdbool.h> + +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include "drv_pwm_servo.h" + +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> + +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg)) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) + +static void pwm_timer_init(unsigned timer); +static void pwm_timer_set_rate(unsigned timer, unsigned rate); +static void pwm_channel_init(unsigned channel); + +static void +pwm_timer_init(unsigned timer) +{ + /* enable the timer clock before we try to talk to it */ + modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + /* configure the timer to free-run at 1MHz */ + rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; + + /* default to updating at 50Hz */ + pwm_timer_set_rate(timer, 50); + + /* note that the timer is left disabled - arming is performed separately */ +} + +static void +pwm_timer_set_rate(unsigned timer, unsigned rate) +{ + /* configure the timer to update at the desired rate */ + rARR(timer) = 1000000 / rate; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; +} + +static void +pwm_channel_init(unsigned channel) +{ + unsigned timer = pwm_channels[channel].timer_index; + + /* configure the GPIO first */ + stm32_configgpio(pwm_channels[channel].gpio); + + /* configure the channel */ + switch (pwm_channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; + rCCR1(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC1E; + break; + + case 2: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; + rCCR2(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC2E; + break; + + case 3: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; + rCCR3(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC3E; + break; + + case 4: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; + rCCR4(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= GTIM_CCER_CC4E; + break; + } +} + +int +up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) + return -1; + + unsigned timer = pwm_channels[channel].timer_index; + + /* test timer for validity */ + if ((pwm_timers[timer].base == 0) || + (pwm_channels[channel].gpio == 0)) + return -1; + + /* configure the channel */ + if (value > 0) + value--; + + switch (pwm_channels[channel].timer_channel) { + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; + } + + return 0; +} + +servo_position_t +up_pwm_servo_get(unsigned channel) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) + return 0; + + unsigned timer = pwm_channels[channel].timer_index; + servo_position_t value = 0; + + /* test timer for validity */ + if ((pwm_timers[timer].base == 0) || + (pwm_channels[channel].timer_channel == 0)) + return 0; + + /* configure the channel */ + switch (pwm_channels[channel].timer_channel) { + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; + } + + return value + 1; +} + +int +up_pwm_servo_init(uint32_t channel_mask) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) + pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + /* don't do init for disabled channels; this leaves the pin configs alone */ + if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) + pwm_channel_init(i); + } + + return OK; +} + +void +up_pwm_servo_deinit(void) +{ + /* disable the timers */ + up_pwm_servo_arm(false); +} + +int +up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) +{ + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + if (rate < 1) + return -ERANGE; + if (rate > 10000) + return -ERANGE; + + if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) + return ERROR; + + pwm_timer_set_rate(group, rate); + + return OK; +} + +int +up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) + up_pwm_servo_set_rate_group_update(i, rate); + + return 0; +} + +uint32_t +up_pwm_servo_get_rate_group(unsigned group) +{ + unsigned channels = 0; + + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) + channels |= (1 << i); + } + return channels; +} + +void +up_pwm_servo_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + // XXX This leads to FMU PWM being still active + // but uncontrollable. Just disable the timer + // and risk a runt. + ///* on disarm, just stop auto-reload so we don't generate runts */ + //rCR1(i) &= ~GTIM_CR1_ARPE; + rCR1(i) = 0; + } + } + } +} diff --git a/src/drivers/stm32/drv_pwm_servo.h b/src/drivers/stm32/drv_pwm_servo.h new file mode 100644 index 000000000..5dd4cf70c --- /dev/null +++ b/src/drivers/stm32/drv_pwm_servo.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_pwm_servo.h + * + * stm32-specific PWM output data. + */ + +#pragma once + +#include <drivers/drv_pwm_output.h> + +/* configuration limits */ +#define PWM_SERVO_MAX_TIMERS 4 +#define PWM_SERVO_MAX_CHANNELS 8 + +/* array of timers dedicated to PWM servo use */ +struct pwm_servo_timer { + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; +}; + +/* array of channels in logical order */ +struct pwm_servo_channel { + uint32_t gpio; + uint8_t timer_index; + uint8_t timer_channel; + servo_position_t default_value; +}; + +/* supplied by board-specific code */ +__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS]; +__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS]; diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk new file mode 100644 index 000000000..bb751c7f6 --- /dev/null +++ b/src/drivers/stm32/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# STM32 driver support code +# +# Modules in this directory are compiled for all STM32 targets. +# + +SRCS = drv_hrt.c \ + drv_pwm_servo.c + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk new file mode 100644 index 000000000..827cf30b2 --- /dev/null +++ b/src/drivers/stm32/tone_alarm/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Tone alarm driver +# + +MODULE_COMMAND = tone_alarm + +SRCS = tone_alarm.cpp + +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp new file mode 100644 index 000000000..ac5511e60 --- /dev/null +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -0,0 +1,1018 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Driver for the PX4 audio alarm port, /dev/tone_alarm. + * + * The tone_alarm driver supports a set of predefined "alarm" + * tunes and one user-supplied tune. + * + * The TONE_SET_ALARM ioctl can be used to select a predefined + * alarm tune, from 1 - <TBD>. Selecting tune zero silences + * the alarm. + * + * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY + * statement, with some exceptions and extensions. + * + * From Wikibooks: + * + * PLAY "[string expression]" + * + * Used to play notes and a score ... The tones are indicated by letters A through G. + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * immediately after the note letter. See this example: + * + * PLAY "C C# C C#" + * + * Whitespaces are ignored inside the string expression. There are also codes that + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators + * that change the properties are effective for the notes following that indicator. + * + * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration + * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. + * (L8, L16, L32, L64, ...). By default, n = 4. + * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. + * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" + * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. + * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. + * Remember that C- is equivalent to B. + * < > Changes the current octave respectively down or up one level. + * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) + * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. + * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. + * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. + * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. + * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. + * It can be used for a pause as well. + * + * Extensions/variations: + * + * MB MF The MF command causes the tune to play once and then stop. The MB command causes the + * tune to repeat when it ends. + * + */ + +#include <nuttx/config.h> +#include <debug.h> + +#include <drivers/device/device.h> +#include <drivers/drv_tone_alarm.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <stdio.h> +#include <unistd.h> +#include <math.h> +#include <ctype.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> + +#include <arch/stm32/chip.h> +#include <up_internal.h> +#include <up_arch.h> + +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +#include <systemlib/err.h> + +#ifndef CONFIG_HRT_TIMER +# error This driver requires CONFIG_HRT_TIMER +#endif + +/* Tone alarm configuration */ +#if TONE_ALARM_TIMER == 2 +# define TONE_ALARM_BASE STM32_TIM2_BASE +# define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM2EN +# ifdef CONFIG_STM32_TIM2 +# error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2 +# endif +#elif TONE_ALARM_TIMER == 3 +# define TONE_ALARM_BASE STM32_TIM3_BASE +# define TONE_ALARM_CLOCK STM32_APB1_TIM3_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM3EN +# ifdef CONFIG_STM32_TIM3 +# error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3 +# endif +#elif TONE_ALARM_TIMER == 4 +# define TONE_ALARM_BASE STM32_TIM4_BASE +# define TONE_ALARM_CLOCK STM32_APB1_TIM4_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM4EN +# ifdef CONFIG_STM32_TIM4 +# error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4 +# endif +#elif TONE_ALARM_TIMER == 5 +# define TONE_ALARM_BASE STM32_TIM5_BASE +# define TONE_ALARM_CLOCK STM32_APB1_TIM5_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM5EN +# ifdef CONFIG_STM32_TIM5 +# error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5 +# endif +#elif TONE_ALARM_TIMER == 9 +# define TONE_ALARM_BASE STM32_TIM9_BASE +# define TONE_ALARM_CLOCK STM32_APB1_TIM9_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM9EN +# ifdef CONFIG_STM32_TIM9 +# error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9 +# endif +#elif TONE_ALARM_TIMER == 10 +# define TONE_ALARM_BASE STM32_TIM10_BASE +# define TONE_ALARM_CLOCK STM32_APB1_TIM10_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM10EN +# ifdef CONFIG_STM32_TIM10 +# error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10 +# endif +#elif TONE_ALARM_TIMER == 11 +# define TONE_ALARM_BASE STM32_TIM11_BASE +# define TONE_ALARM_CLOCK STM32_APB1_TIM11_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM11EN +# ifdef CONFIG_STM32_TIM11 +# error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11 +# endif +#else +# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver. +#endif + +#if TONE_ALARM_CHANNEL == 1 +# define TONE_CCMR1 (3 << 4) +# define TONE_CCMR2 0 +# define TONE_CCER (1 << 0) +# define TONE_rCCR rCCR1 +#elif TONE_ALARM_CHANNEL == 2 +# define TONE_CCMR1 (3 << 12) +# define TONE_CCMR2 0 +# define TONE_CCER (1 << 4) +# define TONE_rCCR rCCR2 +#elif TONE_ALARM_CHANNEL == 3 +# define TONE_CCMR1 0 +# define TONE_CCMR2 (3 << 4) +# define TONE_CCER (1 << 8) +# define TONE_rCCR rCCR3 +#elif TONE_ALARM_CHANNEL == 4 +# define TONE_CCMR1 0 +# define TONE_CCMR2 (3 << 12) +# define TONE_CCER (1 << 12) +# define TONE_rCCR rCCR4 +#else +# error Must set TONE_ALARM_CHANNEL to a value between 1 and 4 to use this driver. +#endif + + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + +class ToneAlarm : public device::CDev +{ +public: + ToneAlarm(); + ~ToneAlarm(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + +private: + static const unsigned _tune_max = 1024; // be reasonable about user tunes + static const char * const _default_tunes[]; + static const unsigned _default_ntunes; + static const uint8_t _note_tab[]; + + const char *_user_tune; + + const char *_tune; // current tune string + const char *_next; // next note in the string + + unsigned _tempo; + unsigned _note_length; + enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; + unsigned _octave; + unsigned _silence_length; // if nonzero, silence before next note + bool _repeat; // if true, tune restarts at end + + hrt_call _note_call; // HRT callout for note completion + + // Convert a note value in the range C1 to B7 into a divisor for + // the configured timer's clock. + // + unsigned note_to_divisor(unsigned note); + + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of + // dots following in the play string. + // + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + + // Calculate the duration in microseconds of a rest corresponding to + // a given note length. + // + unsigned rest_duration(unsigned rest_length, unsigned dots); + + // Start playing the note + // + void start_note(unsigned note); + + // Stop playing the current note and make the player 'safe' + // + void stop_note(); + + // Start playing the tune + // + void start_tune(const char *tune); + + // Parse the next note out of the string and play it + // + void next_note(); + + // Find the next character in the string, discard any whitespace and + // return the canonical (uppercase) version. + // + int next_char(); + + // Extract a number from the string, consuming all the digit characters. + // + unsigned next_number(); + + // Consume dot characters from the string, returning the number consumed. + // + unsigned next_dots(); + + // hrt_call trampoline for next_note + // + static void next_trampoline(void *arg); + +}; + +// predefined tune array +const char * const ToneAlarm::_default_tunes[] = { + "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune + "MBT200a8a8a8PaaaP", // ERROR tone + "MFT200e8a8a", // NotifyPositive tone + "MFT200e8e", // NotifyNeutral tone + "MFT200e8c8e8c8e8c8", // NotifyNegative tone + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! + "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie + "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha + "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee + "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy + "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell + "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" + "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" + "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" + "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" + "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" + "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" + "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" + "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" + "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" + "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" + "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" + "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" + "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" + "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" + "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" + "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" + "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" + "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" + "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" + "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" + "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" + "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" + "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" + "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" + "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" + "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" + "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" + "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" + "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" + "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" + "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" + "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" + "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" + "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" + "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" + "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" + "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" + "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" + "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" + "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" + "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" + "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" + "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" + "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" + "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" + "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" + "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" + "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" + "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" + "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" + "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" + "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." + "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" + "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" + "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" + "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" + "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" + "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" + "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" + "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" + "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" + "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" + "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" + "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" + "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" + "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" + "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" + "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" + "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" + "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" + "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" + "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" + "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" + "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" + "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" + "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" + "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" + "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" + "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" + "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" + "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" + "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" + "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" + "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" + "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" + "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" + "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" + "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" + "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" + "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" + "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" + "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" + "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" + "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" + "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" + "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" + "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" + "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" + "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" + "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" + "O2E2P64", +}; + +const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); + +// semitone offsets from C for the characters 'A'-'G' +const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); + + +ToneAlarm::ToneAlarm() : + CDev("tone_alarm", "/dev/tone_alarm"), + _user_tune(nullptr), + _tune(nullptr), + _next(nullptr) +{ + // enable debug() calls + //_debug_enabled = true; +} + +ToneAlarm::~ToneAlarm() +{ +} + +int +ToneAlarm::init() +{ + int ret; + + ret = CDev::init(); + + if (ret != OK) + return ret; + + /* configure the GPIO to the idle state */ + stm32_configgpio(GPIO_TONE_ALARM_IDLE); + + /* clock/power on our timer */ + modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); + + /* initialise the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = 0; + rCCER &= TONE_CCER; /* unlock CCMR* registers */ + rCCMR1 = TONE_CCMR1; + rCCMR2 = TONE_CCMR2; + rCCER = TONE_CCER; + rDCR = 0; + + /* toggle the CC output each time the count passes 1 */ + TONE_rCCR = 1; + + /* default the timer to a prescale value of 1; playing notes will change this */ + rPSC = 0; + + /* make sure the timer is running */ + rCR1 = GTIM_CR1_CEN; + + debug("ready"); + return OK; +} + +unsigned +ToneAlarm::note_to_divisor(unsigned note) +{ + // compute the frequency first (Hz) + float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); + + float period = 0.5f / freq; + + // and the divisor, rounded to the nearest integer + unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f; + + return divisor; +} + +unsigned +ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) +{ + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; + + if (note_length == 0) + note_length = 1; + unsigned note_period = whole_note_period / note_length; + + switch (_note_mode) { + case MODE_NORMAL: + silence = note_period / 8; + break; + case MODE_STACCATO: + silence = note_period / 4; + break; + default: + case MODE_LEGATO: + silence = 0; + break; + } + note_period -= silence; + + unsigned dot_extension = note_period / 2; + while (dots--) { + note_period += dot_extension; + dot_extension /= 2; + } + + return note_period; +} + +unsigned +ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) +{ + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; + + if (rest_length == 0) + rest_length = 1; + + unsigned rest_period = whole_note_period / rest_length; + + unsigned dot_extension = rest_period / 2; + while (dots--) { + rest_period += dot_extension; + dot_extension /= 2; + } + + return rest_period; +} + +void +ToneAlarm::start_note(unsigned note) +{ + // compute the divisor + unsigned divisor = note_to_divisor(note); + + // pick the lowest prescaler value that we can use + // (note that the effective prescale value is 1 greater) + unsigned prescale = divisor / 65536; + + // calculate the timer period for the selected prescaler value + unsigned period = (divisor / (prescale + 1)) - 1; + + rPSC = prescale; // load new prescaler + rARR = period; // load new toggle period + rEGR = GTIM_EGR_UG; // force a reload of the period + rCCER |= TONE_CCER; // enable the output + + // configure the GPIO to enable timer output + stm32_configgpio(GPIO_TONE_ALARM); +} + +void +ToneAlarm::stop_note() +{ + /* stop the current note */ + rCCER &= ~TONE_CCER; + + /* + * Make sure the GPIO is not driving the speaker. + */ + stm32_configgpio(GPIO_TONE_ALARM_IDLE); +} + +void +ToneAlarm::start_tune(const char *tune) +{ + // kill any current playback + hrt_cancel(&_note_call); + + // record the tune + _tune = tune; + _next = tune; + + // initialise player state + _tempo = 120; + _note_length = 4; + _note_mode = MODE_NORMAL; + _octave = 4; + _silence_length = 0; + _repeat = false; // otherwise command-line tunes repeat forever... + + // schedule a callback to start playing + hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this); +} + +void +ToneAlarm::next_note() +{ + // do we have an inter-note gap to wait for? + if (_silence_length > 0) { + stop_note(); + hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + _silence_length = 0; + return; + } + + // make sure we still have a tune - may be removed by the write / ioctl handler + if ((_next == nullptr) || (_tune == nullptr)) { + stop_note(); + return; + } + + // parse characters out of the string until we have resolved a note + unsigned note = 0; + unsigned note_length = _note_length; + unsigned duration; + + while (note == 0) { + // we always need at least one character from the string + int c = next_char(); + if (c == 0) + goto tune_end; + _next++; + + switch (c) { + case 'L': // select note length + _note_length = next_number(); + if (_note_length < 1) + goto tune_error; + break; + + case 'O': // select octave + _octave = next_number(); + if (_octave > 6) + _octave = 6; + break; + + case '<': // decrease octave + if (_octave > 0) + _octave--; + break; + + case '>': // increase octave + if (_octave < 6) + _octave++; + break; + + case 'M': // select inter-note gap + c = next_char(); + if (c == 0) + goto tune_error; + _next++; + switch (c) { + case 'N': + _note_mode = MODE_NORMAL; + break; + case 'L': + _note_mode = MODE_LEGATO; + break; + case 'S': + _note_mode = MODE_STACCATO; + break; + case 'F': + _repeat = false; + break; + case 'B': + _repeat = true; + break; + default: + goto tune_error; + } + break; + + case 'P': // pause for a note length + stop_note(); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); + return; + + case 'T': { // change tempo + unsigned nt = next_number(); + + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + } else { + goto tune_error; + } + break; + } + + case 'N': // play an arbitrary note + note = next_number(); + if (note > 84) + goto tune_error; + if (note == 0) { + // this is a rest - pause for the current note length + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; + } + break; + + case 'A'...'G': // play a note in the current octave + note = _note_tab[c - 'A'] + (_octave * 12) + 1; + c = next_char(); + switch (c) { + case '#': // up a semitone + case '+': + if (note < 84) + note++; + _next++; + break; + case '-': // down a semitone + if (note > 1) + note--; + _next++; + break; + default: + // 0 / no next char here is OK + break; + } + // shorthand length notation + note_length = next_number(); + if (note_length == 0) + note_length = _note_length; + break; + + default: + goto tune_error; + } + } + + // compute the duration of the note and the following silence (if any) + duration = note_duration(_silence_length, note_length, next_dots()); + + // start playing the note + start_note(note); + + // and arrange a callback when the note should stop + hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); + return; + + // tune looks bad (unexpected EOF, bad character, etc.) +tune_error: + lowsyslog("tune error\n"); + _repeat = false; // don't loop on error + + // stop (and potentially restart) the tune +tune_end: + stop_note(); + if (_repeat) + start_tune(_tune); + return; +} + +int +ToneAlarm::next_char() +{ + while (isspace(*_next)) { + _next++; + } + return toupper(*_next); +} + +unsigned +ToneAlarm::next_number() +{ + unsigned number = 0; + int c; + + for (;;) { + c = next_char(); + if (!isdigit(c)) + return number; + _next++; + number = (number * 10) + (c - '0'); + } +} + +unsigned +ToneAlarm::next_dots() +{ + unsigned dots = 0; + + while (next_char() == '.') { + _next++; + dots++; + } + return dots; +} + +void +ToneAlarm::next_trampoline(void *arg) +{ + ToneAlarm *ta = (ToneAlarm *)arg; + + ta->next_note(); +} + + +int +ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + debug("ioctl %i %u", cmd, arg); + +// irqstate_t flags = irqsave(); + + /* decide whether to increase the alarm level to cmd or leave it alone */ + switch (cmd) { + case TONE_SET_ALARM: + debug("TONE_SET_ALARM %u", arg); + + if (arg <= _default_ntunes) { + if (arg == 0) { + // stop the tune + _tune = nullptr; + _next = nullptr; + } else { + // play the selected tune + start_tune(_default_tunes[arg - 1]); + } + } else { + result = -EINVAL; + } + + break; + + default: + result = -ENOTTY; + break; + } + +// irqrestore(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) + result = CDev::ioctl(filp, cmd, arg); + + return result; +} + +int +ToneAlarm::write(file *filp, const char *buffer, size_t len) +{ + // sanity-check the buffer for length and nul-termination + if (len > _tune_max) + return -EFBIG; + + // if we have an existing user tune, free it + if (_user_tune != nullptr) { + + // if we are playing the user tune, stop + if (_tune == _user_tune) { + _tune = nullptr; + _next = nullptr; + } + + // free the old user tune + free((void *)_user_tune); + _user_tune = nullptr; + } + + // if the new tune is empty, we're done + if (buffer[0] == '\0') + return OK; + + // allocate a copy of the new tune + _user_tune = strndup(buffer, len); + if (_user_tune == nullptr) + return -ENOMEM; + + // and play it + start_tune(_user_tune); + + return len; +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +ToneAlarm *g_dev; + +int +play_tune(unsigned tune) +{ + int fd, ret; + + fd = open("/dev/tone_alarm", 0); + + if (fd < 0) + err(1, "/dev/tone_alarm"); + + ret = ioctl(fd, TONE_SET_ALARM, tune); + close(fd); + + if (ret != 0) + err(1, "TONE_SET_ALARM"); + + exit(0); +} + +int +play_string(const char *str) +{ + int fd, ret; + + fd = open("/dev/tone_alarm", O_WRONLY); + + if (fd < 0) + err(1, "/dev/tone_alarm"); + + ret = write(fd, str, strlen(str) + 1); + close(fd); + + if (ret < 0) + err(1, "play tune"); + exit(0); +} + +} // namespace + +int +tone_alarm_main(int argc, char *argv[]) +{ + unsigned tune; + + /* start the driver lazily */ + if (g_dev == nullptr) { + g_dev = new ToneAlarm; + + if (g_dev == nullptr) + errx(1, "couldn't allocate the ToneAlarm driver"); + + if (g_dev->init() != OK) { + delete g_dev; + errx(1, "ToneAlarm init failed"); + } + } + + if ((argc > 1) && !strcmp(argv[1], "start")) + play_tune(1); + + if ((argc > 1) && !strcmp(argv[1], "stop")) + play_tune(0); + + if ((tune = strtol(argv[1], nullptr, 10)) != 0) + play_tune(tune); + + /* if it looks like a PLAY string... */ + if (strlen(argv[1]) > 2) { + const char *str = argv[1]; + if (str[0] == 'M') { + play_string(str); + } + } + + errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); +} diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c new file mode 100644 index 000000000..670b8d635 --- /dev/null +++ b/src/modules/px4iofirmware/adc.c @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c new file mode 100644 index 000000000..dc36f6c93 --- /dev/null +++ b/src/modules/px4iofirmware/controls.c @@ -0,0 +1,332 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file 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/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c new file mode 100644 index 000000000..ea35e5513 --- /dev/null +++ b/src/modules/px4iofirmware/dsm.c @@ -0,0 +1,356 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/hx_stream.c b/src/modules/px4iofirmware/hx_stream.c new file mode 100644 index 000000000..8d77f14a8 --- /dev/null +++ b/src/modules/px4iofirmware/hx_stream.c @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * 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 hx_stream.c + * + * A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <crc32.h> +#include <unistd.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include "perf_counter.h" + +#include "hx_stream.h" + + +struct hx_stream { + uint8_t buf[HX_STREAM_MAX_FRAME + 4]; + unsigned frame_bytes; + bool escaped; + bool txerror; + + int fd; + hx_stream_rx_callback callback; + void *callback_arg; + + perf_counter_t pc_tx_frames; + perf_counter_t pc_rx_frames; + perf_counter_t pc_rx_errors; +}; + +/* + * Protocol magic numbers, straight out of HDLC. + */ +#define FBO 0x7e /**< Frame Boundary Octet */ +#define CEO 0x7c /**< Control Escape Octet */ + +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static int hx_rx_frame(hx_stream_t stream); + +static void +hx_tx_raw(hx_stream_t stream, uint8_t c) +{ + if (write(stream->fd, &c, 1) != 1) + stream->txerror = true; +} + +static void +hx_tx_byte(hx_stream_t stream, uint8_t c) +{ + switch (c) { + case FBO: + case CEO: + hx_tx_raw(stream, CEO); + c ^= 0x20; + break; + } + + hx_tx_raw(stream, c); +} + +static int +hx_rx_frame(hx_stream_t stream) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + unsigned length = stream->frame_bytes; + + /* reset the stream */ + stream->frame_bytes = 0; + stream->escaped = false; + + /* not a real frame - too short */ + if (length < 4) { + if (length > 1) + perf_count(stream->pc_rx_errors); + + return 0; + } + + length -= 4; + + /* compute expected CRC */ + u.w = crc32(&stream->buf[0], length); + + /* compare computed and actual CRC */ + for (unsigned i = 0; i < 4; i++) { + if (u.b[i] != stream->buf[length + i]) { + perf_count(stream->pc_rx_errors); + return 0; + } + } + + /* frame is good */ + perf_count(stream->pc_rx_frames); + stream->callback(stream->callback_arg, &stream->buf[0], length); + return 1; +} + +hx_stream_t +hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg) +{ + hx_stream_t stream; + + stream = (hx_stream_t)malloc(sizeof(struct hx_stream)); + + if (stream != NULL) { + memset(stream, 0, sizeof(struct hx_stream)); + stream->fd = fd; + stream->callback = callback; + stream->callback_arg = arg; + } + + return stream; +} + +void +hx_stream_free(hx_stream_t stream) +{ + /* free perf counters (OK if they are NULL) */ + perf_free(stream->pc_tx_frames); + perf_free(stream->pc_rx_frames); + perf_free(stream->pc_rx_errors); + + free(stream); +} + +void +hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors) +{ + stream->pc_tx_frames = tx_frames; + stream->pc_rx_frames = rx_frames; + stream->pc_rx_errors = rx_errors; +} + +int +hx_stream_send(hx_stream_t stream, + const void *data, + size_t count) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + const uint8_t *p = (const uint8_t *)data; + unsigned resid = count; + + if (resid > HX_STREAM_MAX_FRAME) + return -EINVAL; + + /* start the frame */ + hx_tx_raw(stream, FBO); + + /* transmit the data */ + while (resid--) + hx_tx_byte(stream, *p++); + + /* compute the CRC */ + u.w = crc32(data, count); + + /* send the CRC */ + p = &u.b[0]; + resid = 4; + + while (resid--) + hx_tx_byte(stream, *p++); + + /* and the trailing frame separator */ + hx_tx_raw(stream, FBO); + + /* check for transmit error */ + if (stream->txerror) { + stream->txerror = false; + return -EIO; + } + + perf_count(stream->pc_tx_frames); + return 0; +} + +void +hx_stream_rx(hx_stream_t stream, uint8_t c) +{ + /* frame end? */ + if (c == FBO) { + hx_rx_frame(stream); + return; + } + + /* escaped? */ + if (stream->escaped) { + stream->escaped = false; + c ^= 0x20; + + } else if (c == CEO) { + /* now escaped, ignore the byte */ + stream->escaped = true; + return; + } + + /* save for later */ + if (stream->frame_bytes < sizeof(stream->buf)) + stream->buf[stream->frame_bytes++] = c; +} diff --git a/src/modules/px4iofirmware/hx_stream.h b/src/modules/px4iofirmware/hx_stream.h new file mode 100644 index 000000000..128689953 --- /dev/null +++ b/src/modules/px4iofirmware/hx_stream.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 hx_stream.h + * + * A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#ifndef _SYSTEMLIB_HX_STREAM_H +#define _SYSTEMLIB_HX_STREAM_H + +#include <sys/types.h> + +#include "perf_counter.h" + +struct hx_stream; +typedef struct hx_stream *hx_stream_t; + +#define HX_STREAM_MAX_FRAME 64 + +typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length); + +__BEGIN_DECLS + +/** + * Allocate a new hx_stream object. + * + * @param fd The file handle over which the protocol will + * communicate. + * @param callback Called when a frame is received. + * @param callback_arg Passed to the callback. + * @return A handle to the stream, or NULL if memory could + * not be allocated. + */ +__EXPORT extern hx_stream_t hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg); + +/** + * Free a hx_stream object. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_free(hx_stream_t stream); + +/** + * Set performance counters for the stream. + * + * Any counter may be set to NULL to disable counting that datum. + * + * @param tx_frames Counter for transmitted frames. + * @param rx_frames Counter for received frames. + * @param rx_errors Counter for short and corrupt received frames. + */ +__EXPORT extern void hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors); + +/** + * Send a frame. + * + * This function will block until all frame bytes are sent if + * the descriptor passed to hx_stream_init is marked blocking, + * otherwise it will return -1 (but may transmit a + * runt frame at the same time). + * + * @todo Handling of non-blocking streams needs to be better. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, -errno on error. + */ +__EXPORT extern int hx_stream_send(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Handle a byte from the stream. + * + * @param stream A handle returned from hx_stream_init. + * @param c The character to process. + */ +__EXPORT extern void hx_stream_rx(hx_stream_t stream, + uint8_t c); + +__END_DECLS + +#endif diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c new file mode 100644 index 000000000..4485daa5b --- /dev/null +++ b/src/modules/px4iofirmware/i2c.c @@ -0,0 +1,340 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp new file mode 100644 index 000000000..f38593d2a --- /dev/null +++ b/src/modules/px4iofirmware/mixer.cpp @@ -0,0 +1,311 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk new file mode 100644 index 000000000..085697fbb --- /dev/null +++ b/src/modules/px4iofirmware/module.mk @@ -0,0 +1,4 @@ + + +SRCS = adc.c controls.c dsm.c i2c.c mixer.cpp px4io.c registers.c safety.c sbus.c \ + up_cxxinitialize.c hx_stream.c perf_counter.c diff --git a/src/modules/px4iofirmware/perf_counter.c b/src/modules/px4iofirmware/perf_counter.c new file mode 100644 index 000000000..879f4715a --- /dev/null +++ b/src/modules/px4iofirmware/perf_counter.c @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * 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 perf_counter.c + * + * @brief Performance measuring tools. + */ + +#include <stdlib.h> +#include <stdio.h> +#include <sys/queue.h> +#include <drivers/drv_hrt.h> + +#include "perf_counter.h" + +/** + * Header common to all counters. + */ +struct perf_ctr_header { + sq_entry_t link; /**< list linkage */ + enum perf_counter_type type; /**< counter type */ + const char *name; /**< counter name */ +}; + +/** + * PC_EVENT counter. + */ +struct perf_ctr_count { + struct perf_ctr_header hdr; + uint64_t event_count; +}; + +/** + * PC_ELAPSED counter. + */ +struct perf_ctr_elapsed { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_start; + uint64_t time_total; + uint64_t time_least; + uint64_t time_most; +}; + +/** + * PC_INTERVAL counter. + */ +struct perf_ctr_interval { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_event; + uint64_t time_first; + uint64_t time_last; + uint64_t time_least; + uint64_t time_most; + +}; + +/** + * List of all known counters. + */ +static sq_queue_t perf_counters; + + +perf_counter_t +perf_alloc(enum perf_counter_type type, const char *name) +{ + perf_counter_t ctr = NULL; + + switch (type) { + case PC_COUNT: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1); + break; + + case PC_ELAPSED: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1); + break; + + case PC_INTERVAL: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; + + default: + break; + } + + if (ctr != NULL) { + ctr->type = type; + ctr->name = name; + sq_addfirst(&ctr->link, &perf_counters); + } + + return ctr; +} + +void +perf_free(perf_counter_t handle) +{ + if (handle == NULL) + return; + + sq_rem(&handle->link, &perf_counters); + free(handle); +} + +void +perf_count(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count++; + break; + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + hrt_abstime now = hrt_absolute_time(); + + switch (pci->event_count) { + case 0: + pci->time_first = now; + break; + case 1: + pci->time_least = now - pci->time_last; + pci->time_most = now - pci->time_last; + break; + default: { + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + break; + } + } + pci->time_last = now; + pci->event_count++; + break; + } + + default: + break; + } +} + +void +perf_begin(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: + ((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time(); + break; + + default: + break; + } +} + +void +perf_end(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; + + pce->event_count++; + pce->time_total += elapsed; + + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + } + + default: + break; + } +} + +void +perf_reset(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count = 0; + break; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + pce->event_count = 0; + pce->time_start = 0; + pce->time_total = 0; + pce->time_least = 0; + pce->time_most = 0; + break; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + pci->event_count = 0; + pci->time_event = 0; + pci->time_first = 0; + pci->time_last = 0; + pci->time_least = 0; + pci->time_most = 0; + break; + } + } +} + +void +perf_print_counter(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + printf("%s: %llu events\n", + handle->name, + ((struct perf_ctr_count *)handle)->event_count); + break; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + handle->name, + pce->event_count, + pce->time_total, + pce->time_least, + pce->time_most); + break; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + + printf("%s: %llu events, %llu avg, min %lluus max %lluus\n", + handle->name, + pci->event_count, + (pci->time_last - pci->time_first) / pci->event_count, + pci->time_least, + pci->time_most); + break; + } + + default: + break; + } +} + +void +perf_print_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_print_counter(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} + +void +perf_reset_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_reset(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} diff --git a/src/modules/px4iofirmware/perf_counter.h b/src/modules/px4iofirmware/perf_counter.h new file mode 100644 index 000000000..5c2cb15b2 --- /dev/null +++ b/src/modules/px4iofirmware/perf_counter.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 perf_counter.h + * Performance measuring tools. + */ + +#ifndef _SYSTEMLIB_PERF_COUNTER_H +#define _SYSTEMLIB_PERF_COUNTER_H value + +/** + * Counter types. + */ +enum perf_counter_type { + PC_COUNT, /**< count the number of times an event occurs */ + PC_ELAPSED, /**< measure the time elapsed performing an event */ + PC_INTERVAL /**< measure the interval between instances of an event */ +}; + +struct perf_ctr_header; +typedef struct perf_ctr_header *perf_counter_t; + +__BEGIN_DECLS + +/** + * Create a new counter. + * + * @param type The type of the new counter. + * @param name The counter name. + * @return Handle for the new counter, or NULL if a counter + * could not be allocated. + */ +__EXPORT extern perf_counter_t perf_alloc(enum perf_counter_type type, const char *name); + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +__EXPORT extern void perf_free(perf_counter_t handle); + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_count(perf_counter_t handle); + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_begin(perf_counter_t handle); + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_end(perf_counter_t handle); + +/** + * Reset a performance event. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_reset(perf_counter_t handle); + +/** + * Print one performance counter. + * + * @param handle The counter to print. + */ +__EXPORT extern void perf_print_counter(perf_counter_t handle); + +/** + * Print all of the performance counters. + */ +__EXPORT extern void perf_print_all(void); + +/** + * Reset all of the performance counters. + */ +__EXPORT extern void perf_reset_all(void); + +__END_DECLS + +#endif diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h new file mode 100644 index 000000000..8d8b7e941 --- /dev/null +++ b/src/modules/px4iofirmware/protocol.h @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c new file mode 100644 index 000000000..9de37e118 --- /dev/null +++ b/src/modules/px4iofirmware/px4io.c @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h new file mode 100644 index 000000000..202e9d9d9 --- /dev/null +++ b/src/modules/px4iofirmware/px4io.h @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c new file mode 100644 index 000000000..6c09def9e --- /dev/null +++ b/src/modules/px4iofirmware/registers.c @@ -0,0 +1,644 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c new file mode 100644 index 000000000..5495d5ae1 --- /dev/null +++ b/src/modules/px4iofirmware/safety.c @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * 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/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c new file mode 100644 index 000000000..073ddeaba --- /dev/null +++ b/src/modules/px4iofirmware/sbus.c @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * 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; +} diff --git a/src/modules/px4iofirmware/up_cxxinitialize.c b/src/modules/px4iofirmware/up_cxxinitialize.c new file mode 100644 index 000000000..c78f29570 --- /dev/null +++ b/src/modules/px4iofirmware/up_cxxinitialize.c @@ -0,0 +1,150 @@ +/************************************************************************************ + * configs/stm32f4discovery/src/up_cxxinitialize.c + * arch/arm/src/board/up_cxxinitialize.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <debug.h> + +#include <nuttx/arch.h> + +//#include <arch/stm32/chip.h> +//#include "chip.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ +/* Debug ****************************************************************************/ +/* Non-standard debug that may be enabled just for testing the static constructors */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_CXX +#endif + +#ifdef CONFIG_DEBUG_CXX +# define cxxdbg dbg +# define cxxlldbg lldbg +# ifdef CONFIG_DEBUG_VERBOSE +# define cxxvdbg vdbg +# define cxxllvdbg llvdbg +# else +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +# endif +#else +# define cxxdbg(x...) +# define cxxlldbg(x...) +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ +/* This type defines one entry in initialization array */ + +typedef void (*initializer_t)(void); + +/************************************************************************************ + * External references + ************************************************************************************/ +/* _sinit and _einit are symbols exported by the linker script that mark the + * beginning and the end of the C++ initialization section. + */ + +extern initializer_t _sinit; +extern initializer_t _einit; + +/* _stext and _etext are symbols exported by the linker script that mark the + * beginning and the end of text. + */ + +extern uint32_t _stext; +extern uint32_t _etext; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/**************************************************************************** + * Name: up_cxxinitialize + * + * Description: + * If C++ and C++ static constructors are supported, then this function + * must be provided by board-specific logic in order to perform + * initialization of the static C++ class instances. + * + * This function should then be called in the application-specific + * user_start logic in order to perform the C++ initialization. NOTE + * that no component of the core NuttX RTOS logic is involved; This + * function defintion only provides the 'contract' between application + * specific C++ code and platform-specific toolchain support + * + ***************************************************************************/ + +__EXPORT void up_cxxinitialize(void) +{ + initializer_t *initp; + + cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); + + /* Visit each entry in the initialzation table */ + + for (initp = &_sinit; initp != &_einit; initp++) + { + initializer_t initializer = *initp; + cxxdbg("initp: %p initializer: %p\n", initp, initializer); + + /* Make sure that the address is non-NULL and lies in the text region + * defined by the linker script. Some toolchains may put NULL values + * or counts in the initialization table + */ + + if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext) + { + cxxdbg("Calling %p\n", initializer); + initializer(); + } + } +} |