From 476db468697089895c15ab151e58b71c5b3a2c95 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 01:26:58 -0800 Subject: [PATCH 1/9] kill off the NuttX ADC driver config; we're not going to use it --- nuttx/configs/px4fmu/nsh/appconfig | 1 + nuttx/configs/px4fmu/nsh/defconfig | 26 +++----------------------- nuttx/configs/px4io/io/defconfig | 1 + 3 files changed, 5 insertions(+), 23 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 0959687ded..7f1a93df92 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -100,6 +100,7 @@ CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm +CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index bc724c0063..d9a48f2c86 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -189,9 +189,10 @@ CONFIG_STM32_TIM1=n CONFIG_STM32_TIM8=n CONFIG_STM32_USART1=y CONFIG_STM32_USART6=y -CONFIG_STM32_ADC1=n +# We use our own driver, but leave this on. +CONFIG_STM32_ADC1=y CONFIG_STM32_ADC2=n -CONFIG_STM32_ADC3=y +CONFIG_STM32_ADC3=n CONFIG_STM32_SDIO=n CONFIG_STM32_SPI1=y CONFIG_STM32_SYSCFG=y @@ -358,27 +359,6 @@ CONFIG_STM32_I2CTIMEOUS_START_STOP=700 # XXX this is bad and we want it gone CONFIG_I2C_WRITEREAD=y -# -# ADC configuration -# -# Enable ADC driver support. -# -# CONFIG_ADC=y : Enable the generic ADC infrastructure -# CONFIG_STM32_TIM1_ADC=y : Indicate that timer 1 will be used to trigger an ADC -# CONFIG_STM32_TIM1_ADC3=y : Assign timer 1 to drive ADC3 sampling -# CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=100 : Select a sampling frequency -# -CONFIG_ADC=y -CONFIG_STM32_TIM4_ADC3=y -# select sample frequency 1^=1.5Msamples/second -# 65535^=10samples/second 16bit-timer runs at 84/128 MHz -CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=6000 -# select timer channel 0=CC1,...,3=CC4 -CONFIG_STM32_ADC3_TIMTRIG=3 -CONFIG_ADC_DMA=y -# only 4 places usable! -CONFIG_ADC_FIFOSIZE=5 - # # General build options # diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 1ac70f8abf..043754e29b 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -131,6 +131,7 @@ CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n CONFIG_STM32_DAC=n # APB2: +# We use our own ADC driver, but leave this on for clocking purposes. CONFIG_STM32_ADC1=y CONFIG_STM32_ADC2=n # TIM1 is owned by the HRT From 22f5a1dc9423d04e70c109f74b1948536070598f Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 01:30:57 -0800 Subject: [PATCH 2/9] =?UTF-8?q?First=20cut=20at=20a=20simple=C2=AE=20ADC?= =?UTF-8?q?=20driver=20built=20on=20our=20driver=20framework.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- apps/drivers/boards/px4fmu/px4fmu_adc.c | 139 ---------- apps/drivers/boards/px4fmu/px4fmu_init.c | 23 +- apps/drivers/drv_adc.h | 54 ++++ apps/drivers/stm32/adc/Makefile | 43 ++++ apps/drivers/stm32/adc/adc.cpp | 310 +++++++++++++++++++++++ 5 files changed, 412 insertions(+), 157 deletions(-) delete mode 100644 apps/drivers/boards/px4fmu/px4fmu_adc.c create mode 100644 apps/drivers/drv_adc.h create mode 100644 apps/drivers/stm32/adc/Makefile create mode 100644 apps/drivers/stm32/adc/adc.cpp diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c deleted file mode 100644 index 9878941631..0000000000 --- a/apps/drivers/boards/px4fmu/px4fmu_adc.c +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4fmu_adc.c - * - * Board-specific ADC functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include - -#include -#include - -#include "chip.h" -#include "up_arch.h" - -#include "stm32_adc.h" -#include "px4fmu_internal.h" - -#define ADC3_NCHANNELS 4 - -/************************************************************************************ - * Private Data - ************************************************************************************/ -/* The PX4FMU board has four ADC channels: ADC323 IN10-13 - */ - -/* Identifying number of each ADC channel: Variable Resistor. */ - -#ifdef CONFIG_STM32_ADC3 -static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards - -/* Configurations of pins used byte each ADC channels */ -static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; -#endif - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: adc_devinit - * - * Description: - * All STM32 architectures must provide the following interface to work with - * examples/adc. - * - ************************************************************************************/ - -int adc_devinit(void) -{ - static bool initialized = false; - struct adc_dev_s *adc[ADC3_NCHANNELS]; - int ret; - int i; - - /* Check if we have already initialized */ - - if (!initialized) { - char name[11]; - - for (i = 0; i < ADC3_NCHANNELS; i++) { - stm32_configgpio(g_pinlist[i]); - } - - for (i = 0; i < 1; i++) { - /* Configure the pins as analog inputs for the selected channels */ - //stm32_configgpio(g_pinlist[i]); - - /* Call stm32_adcinitialize() to get an instance of the ADC interface */ - //multiple channels only supported with dma! - adc[i] = stm32_adcinitialize(3, (g_chanlist), 4); - - if (adc == NULL) { - adbg("ERROR: Failed to get ADC interface\n"); - return -ENODEV; - } - - - /* Register the ADC driver at "/dev/adc0" */ - sprintf(name, "/dev/adc%d", i); - ret = adc_register(name, adc[i]); - - if (ret < 0) { - adbg("adc_register failed for adc %s: %d\n", name, ret); - return ret; - } - } - - /* Now we are initialized */ - - initialized = true; - } - - return OK; -} diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 57ffb77d34..4ebc5a439e 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -209,8 +209,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); -#if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ + /* Get the SPI port for the microSD slot */ message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); @@ -233,23 +232,11 @@ __EXPORT int nsh_archinitialize(void) } message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); -#endif /* SPI3 */ -#ifdef CONFIG_ADC - int adc_state = adc_devinit(); - - if (adc_state != OK) { - /* Try again */ - adc_state = adc_devinit(); - - if (adc_state != OK) { - /* Give up */ - message("[boot] FAILED adc_devinit: %d\n", adc_state); - return -ENODEV; - } - } - -#endif + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + //stm32_configgpio(GPIO_ADC1_IN12); // XXX is this available? + //stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards return OK; } diff --git a/apps/drivers/drv_adc.h b/apps/drivers/drv_adc.h new file mode 100644 index 0000000000..f42350cbb5 --- /dev/null +++ b/apps/drivers/drv_adc.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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_adc.h + * + * ADC driver interface. + * + * This defines additional operations over and above the standard NuttX + * ADC API. + */ + +#pragma once + +#include +#include + +#include + +#define ADC_DEVICE_PATH "/dev/adc0" + +/* + * ioctl definitions + */ diff --git a/apps/drivers/stm32/adc/Makefile b/apps/drivers/stm32/adc/Makefile new file mode 100644 index 0000000000..443bc06239 --- /dev/null +++ b/apps/drivers/stm32/adc/Makefile @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# STM32 ADC driver +# + +APPNAME = adc +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp new file mode 100644 index 0000000000..f77499706e --- /dev/null +++ b/apps/drivers/stm32/adc/adc.cpp @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#define ADC_BASE STM32_ADC1_BASE + +/* + * Register accessors. + */ +#define REG(_reg) (*(volatile uint32_t *)(_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) + +class ADC : public device::CDev +{ +public: + ADC(); + ~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 */ + static const uint32_t _base = ADC_BASE; + + hrt_call _call; + + static void _tick_trampoline(void *arg); + void _tick(); + + uint16_t _sample(unsigned channel); + + uint16_t _result; +}; + +ADC::ADC() : + CDev("adc", ADC_DEVICE_PATH) +{ + _debug_enabled = true; +} + +ADC::~ADC() +{ +} + +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 + ADC_CR2_TSVREFE | +#endif + 0; + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 11; /* 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) +{ + if (len > sizeof(_result)) + len = sizeof(_result); + +// _result = _sample(11); + + memcpy(buffer, &_result, len); + return len; +} + +int +ADC::open_first(struct file *filp) +{ + 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() +{ + _result = _sample(11); +} + +uint16_t +ADC::_sample(unsigned channel) +{ + /* 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; + + return result; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int adc_main(int argc, char *argv[]); + +namespace +{ +ADC *g_adc; + +void +test(void) +{ + int fd; + + fd = open(ADC_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "can't open ADC device"); + + uint16_t value; + + for (unsigned i = 0; i < 50; i++) { + if (read(fd, &value, sizeof(value)) != sizeof(value)) + errx(1, "short read"); + printf(" %d\n", value); + usleep(500000); + } + + exit(0); +} +} + +int +adc_main(int argc, char *argv[]) +{ + if (g_adc == nullptr) { + g_adc = new ADC; + + 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); +} From bc432b1feb906e5c2895d35bf1430fa6bf004061 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 17:06:30 -0800 Subject: [PATCH 3/9] Cleanup and add support for multiple channels. --- apps/drivers/boards/px4fmu/px4fmu_init.c | 2 - apps/drivers/drv_adc.h | 2 - apps/drivers/stm32/adc/adc.cpp | 112 ++++++++++++++++++----- 3 files changed, 88 insertions(+), 28 deletions(-) diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 4ebc5a439e..c2aed9b546 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -95,8 +95,6 @@ * Protected Functions ****************************************************************************/ -extern int adc_devinit(void); - /**************************************************************************** * Public Functions ****************************************************************************/ diff --git a/apps/drivers/drv_adc.h b/apps/drivers/drv_adc.h index f42350cbb5..8ec6d1233f 100644 --- a/apps/drivers/drv_adc.h +++ b/apps/drivers/drv_adc.h @@ -45,8 +45,6 @@ #include #include -#include - #define ADC_DEVICE_PATH "/dev/adc0" /* diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp index f77499706e..ed40b478a1 100644 --- a/apps/drivers/stm32/adc/adc.cpp +++ b/apps/drivers/stm32/adc/adc.cpp @@ -62,13 +62,13 @@ #include #include - -#define ADC_BASE STM32_ADC1_BASE +#include /* * Register accessors. + * For now, no reason not to just use ADC1. */ -#define REG(_reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) #define rSR REG(STM32_ADC_SR_OFFSET) #define rCR1 REG(STM32_ADC_CR1_OFFSET) @@ -94,7 +94,7 @@ class ADC : public device::CDev { public: - ADC(); + ADC(uint32_t channels); ~ADC(); virtual int init(); @@ -107,27 +107,64 @@ protected: virtual int close_last(struct file *filp); private: - static const hrt_abstime _tickrate = 10000; /* 100Hz base rate */ - static const uint32_t _base = ADC_BASE; - + 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); - uint16_t _result; }; -ADC::ADC() : - CDev("adc", ADC_DEVICE_PATH) +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; + + /* 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 @@ -158,7 +195,7 @@ ADC::init() /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 11; /* will be updated with the channel each tick */ + rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; @@ -180,6 +217,8 @@ ADC::init() return 0xffff; } } + + debug("init done"); /* create the device node */ @@ -195,18 +234,26 @@ ADC::ioctl(file *filp, int cmd, unsigned long arg) ssize_t ADC::read(file *filp, char *buffer, size_t len) { - if (len > sizeof(_result)) - len = sizeof(_result); + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; -// _result = _sample(11); + 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); - memcpy(buffer, &_result, len); 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; @@ -228,12 +275,20 @@ ADC::_tick_trampoline(void *arg) void ADC::_tick() { - _result = _sample(11); + /* 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; @@ -252,6 +307,7 @@ ADC::_sample(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + perf_end(_sample_perf); return result; } @@ -267,18 +323,25 @@ ADC *g_adc; void test(void) { - int fd; - fd = open(ADC_DEVICE_PATH, O_RDONLY); + int fd = open(ADC_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "can't open ADC device"); - uint16_t value; - for (unsigned i = 0; i < 50; i++) { - if (read(fd, &value, sizeof(value)) != sizeof(value)) - errx(1, "short read"); - printf(" %d\n", value); + 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); } @@ -290,7 +353,8 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - g_adc = new ADC; + /* XXX this hardcodes the minimum channel set for PX4FMU - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11)); if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); From b167912b1b3137c494a5033fe0dea6bb373624d2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 18:31:37 -0800 Subject: [PATCH 4/9] Enable the temperature sensor channel for F2/F4 devices. --- apps/drivers/stm32/adc/adc.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp index ed40b478a1..87dac1ef91 100644 --- a/apps/drivers/stm32/adc/adc.cpp +++ b/apps/drivers/stm32/adc/adc.cpp @@ -91,6 +91,10 @@ #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: @@ -140,6 +144,9 @@ ADC::ADC(uint32_t channels) : { _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)) { @@ -188,10 +195,16 @@ ADC::init() /* 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; From bd2f6b58e67245b4fbe6da272b8c59be3511c53b Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 19:40:41 -0800 Subject: [PATCH 5/9] Configure ADC GPIOs on IO --- apps/drivers/boards/px4io/px4io_init.c | 3 +++ apps/drivers/boards/px4io/px4io_internal.h | 27 ++++------------------ 2 files changed, 8 insertions(+), 22 deletions(-) diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c index 33a6707cec..14e8dc13a9 100644 --- a/apps/drivers/boards/px4io/px4io_init.c +++ b/apps/drivers/boards/px4io/px4io_init.c @@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ACC_OC_DETECT); stm32_configgpio(GPIO_SERVO_OC_DETECT); stm32_configgpio(GPIO_BTN_SAFETY); + + stm32_configgpio(GPIO_ADC_VBATT); + stm32_configgpio(GPIO_ADC_IN5); } diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h index a0342ac8ad..f77d237a77 100644 --- a/apps/drivers/boards/px4io/px4io_internal.h +++ b/apps/drivers/boards/px4io/px4io_internal.h @@ -61,28 +61,6 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -/* R/C in/out channels **************************************************************/ - -/* XXX just GPIOs for now - eventually timer pins */ - -#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0) -#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1) -#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8) -#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9) -#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6) -#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7) -#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0) -#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1) - -#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) -#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) -#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) -#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) -#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) -#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) -#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) -#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) - /* Safety switch button *************************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) @@ -98,3 +76,8 @@ #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13) #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) + +/* 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) From 9be1f999357edd37658c301d9dd5aaa1d8db26a7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 19:41:18 -0800 Subject: [PATCH 6/9] Add trivial ADC support to PX4IO. --- apps/px4io/adc.c | 163 +++++++++++++++++++++++++++++++++++++++++++++ apps/px4io/px4io.h | 6 ++ 2 files changed, 169 insertions(+) create mode 100644 apps/px4io/adc.c diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c new file mode 100644 index 0000000000..62ff0b1f19 --- /dev/null +++ b/apps/px4io/adc.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include +#include + +#include +#include + +#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_absolute_time() - now) > 1000) { + debug("adc timeout"); + break; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + perf_end(adc_perf); + return result; +} \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 45b7cf847a..6221b08f24 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -160,6 +160,12 @@ extern void safety_init(void); */ extern void comms_main(void) __attribute__((noreturn)); +/* + * Sensors/misc inputs + */ +extern int adc_init(void); +extern uint16_t adc_measure(unsigned channel); + /* * R/C receiver handling. */ From d93fda20fd55746923d607e717f254bc92741eab Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 31 Dec 2012 21:06:26 -0800 Subject: [PATCH 7/9] Add ADC measurements and reporting to PX4IO, including calibration for the battery input. --- apps/px4io/comms.c | 66 +++++++++++++++++++++++++++++++++++-------- apps/px4io/protocol.h | 4 +++ apps/px4io/px4io.h | 28 ++++++++++++------ 3 files changed, 78 insertions(+), 20 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 83a006d43b..eaaea7817d 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -61,8 +61,7 @@ #define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */ #define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */ -int frame_rx; -int frame_bad; +#define FMU_STATUS_INTERVAL 1000000 /* 100ms */ static int fmu_fd; static hx_stream_t stream; @@ -87,6 +86,9 @@ comms_init(void) cfsetspeed(&t, 115200); t.c_cflag &= ~(CSTOPB | PARENB); tcsetattr(fmu_fd, TCSANOW, &t); + + /* init the ADC */ + adc_init(); } void @@ -135,9 +137,55 @@ comms_main(void) report.channel_count = system_state.rc_channels; report.armed = system_state.armed; + report.battery_mv = system_state.battery_mv; + report.adc_in = system_state.adc_in5; + report.overcurrent = system_state.overcurrent; + /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } + + /* + * Fetch ADC values, check overcurrent flags, etc. + */ + static hrt_abstime last_status_time; + + if ((now - last_status_time) > FMU_STATUS_INTERVAL) { + + /* + * 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); + system_state.battery_mv = (4150 + (counts * 46)) / 10; + + system_state.adc_in5 = adc_measure(ADC_IN5); + + system_state.overcurrent = + (OVERCURRENT_SERVO ? (1 << 0) : 0) | + (OVERCURRENT_ACC ? (1 << 1) : 0); + + last_status_time = now; + } } } @@ -146,12 +194,10 @@ comms_handle_config(const void *buffer, size_t length) { const struct px4io_config *cfg = (struct px4io_config *)buffer; - if (length != sizeof(*cfg)) { - frame_bad++; + if (length != sizeof(*cfg)) return; - } - frame_rx++; + /* XXX */ } static void @@ -159,12 +205,9 @@ comms_handle_command(const void *buffer, size_t length) { const struct px4io_command *cmd = (struct px4io_command *)buffer; - if (length != sizeof(*cmd)) { - frame_bad++; + if (length != sizeof(*cmd)) return; - } - - frame_rx++; + irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -209,7 +252,6 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) comms_handle_config(buffer, length); break; default: - frame_bad++; break; } } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b12016..a64e2f27de 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -73,6 +73,10 @@ struct px4io_report { uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; bool armed; uint8_t channel_count; + + uint16_t battery_mv; + uint16_t adc_in; + uint8_t overcurrent; }; #pragma pack(pop) \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 6221b08f24..af06c6ec1e 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -105,17 +105,25 @@ struct sys_state_s bool fmu_data_received; /* - * Current serial interface mode, per the serial_rx_mode parameter - * in the config packet. + * Measured battery voltage in mV */ - uint8_t serial_rx_mode; + uint16_t battery_mv; + + /* + * ADC IN5 measurement + */ + uint16_t adc_in5; + + /* + * Power supply overcurrent status bits. + */ + uint8_t overcurrent; + }; extern struct sys_state_s system_state; -extern int frame_rx; -extern int frame_bad; - +#if 0 /* * Software countdown timers. * @@ -127,6 +135,7 @@ extern int frame_bad; #define TIMER_SANITY 7 #define TIMER_NUM_TIMERS 8 extern volatile int timers[TIMER_NUM_TIMERS]; +#endif /* * GPIO handling. @@ -141,10 +150,13 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #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 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 + /* * Mixer */ From 4eb7df6ff5b0015a825ca07c1206dd545b4567b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jan 2013 13:30:24 +0100 Subject: [PATCH 8/9] Introduced battery_status uORB topic, changed sensors app to publish to it, extended px4io driver to publish to it. Both do only so if the battery voltage is reasonably high, at 3.3V --- apps/commander/commander.c | 26 ++++++++---- apps/drivers/px4io/px4io.cpp | 25 +++++++++++ apps/sdlog/sdlog.c | 2 +- apps/sensors/sensors.cpp | 33 ++++++++++----- apps/uORB/objects_common.cpp | 3 ++ apps/uORB/topics/battery_status.h | 68 ++++++++++++++++++++++++++++++ apps/uORB/topics/sensor_combined.h | 4 +- 7 files changed, 140 insertions(+), 21 deletions(-) create mode 100644 apps/uORB/topics/battery_status.h diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7277e9fa4c..17087ab8af 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -70,6 +70,7 @@ #include #include #include +#include #include #include #include @@ -1262,6 +1263,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1300,15 +1306,19 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } - battery_voltage = sensors.battery_voltage_v; - battery_voltage_valid = sensors.battery_voltage_valid; + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + battery_voltage = battery.voltage_v; + battery_voltage_valid = true; - /* - * Only update battery voltage estimate if voltage is - * valid and system has been running for two and a half seconds - */ - if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (hrt_absolute_time() - start_time > 2500000) { + bat_remain = battery_remaining_estimate_voltage(battery_voltage); + } } /* Slow but important 8 Hz checks */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9f3dba047f..90c0773630 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include "uploader.h" @@ -109,6 +110,9 @@ private: orb_advert_t _to_input_rc; ///< rc inputs from io rc_input_values _input_rc; ///< rc input values + orb_advert_t _to_battery; ///< battery status / voltage + battery_status_s _battery_status;///< battery status data + orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs @@ -321,6 +325,10 @@ PX4IO::task_main() memset(&_input_rc, 0, sizeof(_input_rc)); _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); + /* do not advertise the battery status until its clear that a battery is connected */ + memset(&_input_rc, 0, sizeof(_input_rc)); + _to_battery = -1; + /* poll descriptor */ pollfd fds[3]; fds[0].fd = _serial_fd; @@ -479,6 +487,23 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) /* remember the latched arming switch state */ _switch_armed = rep->armed; + /* publish battery information */ + + /* only publish if battery has a valid minimum voltage */ + if (rep->battery_mv > 3300) { + _battery_status.timestamp = hrt_absolute_time(); + _battery_status.voltage_v = rep->battery_mv / 1000.0f; + /* current and discharge are unknown */ + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + /* announce the battery voltage if needed, just publish else */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } + _send_needed = true; /* if monitoring, dump the received info */ diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index d38bf9122d..9b4cd16222 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -554,7 +554,7 @@ int sdlog_thread_main(int argc, char *argv[]) { .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, .actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, - .vbat = buf.raw.battery_voltage_v, + .vbat = 0.0f, /* XXX use battery_status uORB topic */ .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 07a9015fed..55786333ff 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ @@ -156,10 +157,12 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ + orb_advert_t _battery_pub; /**< battery status */ perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ + struct battery_status_s _battery_status; /**< battery status */ struct { float min[_rc_max_chan_count]; @@ -348,6 +351,7 @@ Sensors::Sensors() : _sensor_pub(-1), _manual_control_pub(-1), _rc_pub(-1), + _battery_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) @@ -844,14 +848,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { /* Voltage in volts */ - raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling))); + float voltage = (buf_adc.am_data1 * _parameters.battery_voltage_scaling); - if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - raw.battery_voltage_valid = false; - raw.battery_voltage_v = 0.f; + if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - } else { - raw.battery_voltage_valid = true; + _battery_status.timestamp = hrt_absolute_time(); + _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; + /* current and discharge are unknown */ + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + + /* announce the battery voltage if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } } raw.battery_voltage_counter++; @@ -879,7 +891,7 @@ Sensors::ppm_poll() */ if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - for (int i = 0; i < ppm_decoded_channels; i++) { + for (unsigned i = 0; i < ppm_decoded_channels; i++) { raw.values[i] = ppm_buffer[i]; } @@ -1039,12 +1051,13 @@ Sensors::task_main() struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); - raw.battery_voltage_v = BAT_VOL_INITIAL; raw.adc_voltage_v[0] = 0.9f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; - raw.battery_voltage_counter = 0; - raw.battery_voltage_valid = false; + raw.adc_voltage_v[3] = 0.0f; + + memset(&_battery_status, 0, sizeof(_battery_status)); + _battery_status.voltage_v = BAT_VOL_INITIAL; /* get a set of initial values */ accel_poll(raw); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index dbee15050c..2d249a47fc 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -71,6 +71,9 @@ ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/battery_status.h" +ORB_DEFINE(battery_status, struct battery_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/apps/uORB/topics/battery_status.h b/apps/uORB/topics/battery_status.h new file mode 100644 index 0000000000..c40d0d4e56 --- /dev/null +++ b/apps/uORB/topics/battery_status.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file battery_status.h + * + * Definition of the battery status uORB topic. + */ + +#ifndef BATTERY_STATUS_H_ +#define BATTERY_STATUS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Battery voltages and status + */ +struct battery_status_s { + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + float voltage_v; /**< Battery voltage in volts, filtered */ + float current_a; /**< Battery current in amperes, filtered, -1 if unknown */ + float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(battery_status); + +#endif \ No newline at end of file diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index 0324500acf..1d25af35a5 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -99,8 +99,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float battery_voltage_v; /**< Battery voltage in volts, filtered */ - float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 */ + float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ uint32_t battery_voltage_counter; /**< Number of voltage measurements taken */ bool battery_voltage_valid; /**< True if battery voltage can be measured */ From 803352e7225ace232ed4a419118956656f13b947 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jan 2013 13:39:00 +0100 Subject: [PATCH 9/9] Fixed stupid copy/paste typo --- apps/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 90c0773630..f6b350cc9e 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -326,7 +326,7 @@ PX4IO::task_main() _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); /* do not advertise the battery status until its clear that a battery is connected */ - memset(&_input_rc, 0, sizeof(_input_rc)); + memset(&_battery_status, 0, sizeof(_battery_status)); _to_battery = -1; /* poll descriptor */