From 811790a14f567b1a58a4434c9fae0cf764e8fd2d Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 7 Jan 2013 21:33:04 -0800 Subject: [PATCH 001/167] Checkpoint I2C slave work on IO --- apps/px4io/i2c.c | 159 +++++++++++++++++++++++++++ apps/px4io/px4io.c | 3 + apps/px4io/px4io.h | 1 + apps/systemcmds/i2c/Makefile | 42 +++++++ apps/systemcmds/i2c/i2c.c | 139 +++++++++++++++++++++++ nuttx/configs/px4fmu/include/board.h | 2 +- nuttx/configs/px4fmu/nsh/appconfig | 2 +- nuttx/fs/stGrJvFk | Bin 0 -> 3394 bytes 8 files changed, 346 insertions(+), 2 deletions(-) create mode 100644 apps/px4io/i2c.c create mode 100644 apps/systemcmds/i2c/Makefile create mode 100644 apps/systemcmds/i2c/i2c.c create mode 100644 nuttx/fs/stGrJvFk diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c new file mode 100644 index 0000000000..3b92fe3ef7 --- /dev/null +++ b/apps/px4io/i2c.c @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include + +#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_dump(void); + +void +i2c_init(void) +{ + // 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 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); + up_enable_irq(STM32_IRQ_I2C1EV); + rCR2 |= I2C_CR2_ITEVFEN; + + // and enable the I2C port + rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; + + i2c_dump(); +} + +static int +i2c_interrupt(int irq, FAR void *context) +{ + uint16_t sr1 = rSR1; + + // XXX not sure what else we need to do here... + if (sr1 & I2C_SR1_ERRORMASK) { + debug("errors 0x%04x", sr1 & I2C_SR1_ERRORMASK); + i2c_dump(); + rSR1 = 0; + } + + if (sr1 & I2C_SR1_ADDR) { + + /* XXX we have been addressed, set up to receive */ + + /* clear ADDR */ + (void)rSR2; + + debug("addr"); + } + + if (sr1 & I2C_SR1_RXNE) { + + debug("data 0x%02x", rDR); + } + + if (sr1 & I2C_SR1_STOPF) { + /* write to CR1 to clear STOPF */ + rCR1 |= I2C_CR1_PE; + + debug("stop"); + + /* XXX handle txn */ + } + + return 0; +} + +static void +i2c_dump(void) +{ + debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); + debug("OAR1 0x%08x OAR2 0x%08x", rOAR1, rOAR2); + debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); + debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); +} + diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index bea9d59bce..4e3555b13e 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -100,6 +100,9 @@ int user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + /* start the i2c test code */ + i2c_init(); + /* we're done here, go run the communications loop */ comms_main(); } \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index e388f65e30..afbaa78dcc 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -222,6 +222,7 @@ extern void safety_init(void); * FMU communications */ extern void comms_main(void) __attribute__((noreturn)); +extern void i2c_init(void); /* * Sensors/misc inputs diff --git a/apps/systemcmds/i2c/Makefile b/apps/systemcmds/i2c/Makefile new file mode 100644 index 0000000000..046e747571 --- /dev/null +++ b/apps/systemcmds/i2c/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the i2c test tool. +# + +APPNAME = i2c +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c new file mode 100644 index 0000000000..1124e560d2 --- /dev/null +++ b/apps/systemcmds/i2c/i2c.c @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 hacking tool + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, no device interface +#endif +#ifndef PX4_I2C_OBDEV_PX4IO +# error PX4_I2C_OBDEV_PX4IO not defined +#endif + +__EXPORT int i2c_main(int argc, char *argv[]); + +static int transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + +static struct i2c_dev_s *i2c; + +int i2c_main(int argc, char *argv[]) +{ + /* find the right I2C */ + i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + usleep(100000); + + uint32_t val = 0x12345678; + int ret = transfer(PX4_I2C_OBDEV_PX4IO, (uint8_t *)&val, sizeof(val), NULL, 0); + + if (ret) + errx(1, "transfer failed"); + exit(0); +} + +static int +transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + struct i2c_msg_s msgv[2]; + unsigned msgs; + int ret; + unsigned tries = 0; + + do { + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + + msgs = 0; + + if (send_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = send; + msgv[msgs].length = send_len; + msgs++; + } + + if (recv_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -1; + + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(i2c, 320000); + ret = I2C_TRANSFER(i2c, &msgv[0], msgs); + + if (ret == OK) + break; + + // reset the I2C bus to unwedge on error + up_i2creset(i2c); + } while (tries++ < 5); + + return ret; +} diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 3f0f26ba14..8ad56a4c6a 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -299,7 +299,7 @@ #define PX4_I2C_OBDEV_EEPROM NOTDEFINED #define PX4_I2C_OBDEV_PX4IO_BL 0x18 -#define PX4_I2C_OBDEV_PX4IO 0x19 +#define PX4_I2C_OBDEV_PX4IO 0x1a /* * SPI diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index fcdf689aab..5a85a8877d 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -95,7 +95,7 @@ CONFIGURED_APPS += attitude_estimator_ekf # Hacking tools #CONFIGURED_APPS += system/i2c -#CONFIGURED_APPS += tools/i2c_dev +CONFIGURED_APPS += systemcmds/i2c # Communication and Drivers CONFIGURED_APPS += drivers/boards/px4fmu diff --git a/nuttx/fs/stGrJvFk b/nuttx/fs/stGrJvFk new file mode 100644 index 0000000000000000000000000000000000000000..640665f6c2e9dc23e5da09011bda42214442d9bb GIT binary patch literal 3394 zcmb_eO>7iZ9Di?Sx4X2Z^aBMHbV_Pk6!xRt7DYgy0ureLI znI@;eG+8KOTFg`|6sAgGnHZlOkGU4RUp;*Mh?FFYMT#*+ZR}6dN=ug%jEODcPLUbw z|1AC8>aD+4#aQ+S;q+f$H$p45_~gyiLjUzg58e9tYvfyMPD)+{%7QXFAC~O0v9u+# z+Yzv=^c}RU%uQH!DqU|z!b)ZO7|W_UtZb%5E0x;7%xvmcS~7b{In&=O$W#C+`4f#d zaqI<?(DbnQjqJ z8IWo<&;Tv*ib56-9pr6U2UUvMK>!PcYGO1U<(lWYaLQG1({pB%Ma2g*MC z%-&&3qP~XTTyoTd{ncuQ^`)j)b!ea&{8qwOLFu@Z8Yb;XHS6ZYJ-ol)Y-ztL`c-OzkQMZnLuN1l~*a zu8`8s+b7oJ;*+%hlPD`}mBUcvaNapObMfIRyXuu&OAGa8%}1)z-EtYn9%sj%hF?jVF5b1CJIQw*&Cc7Je4t+Ay8zwHfhr2%W1bNcI}b;a?^1$qJz}Q5 zx1gIjLslp>q!Vf$}@}?lmN^sd9Xz(c}$);6?b1 zm^{{k-aBpl7m=4Y2GCG{na~;j6nSG*w6H$P7Z}NXe3&?wjfgRi`x?M7Jr7mf&PK>fzY(=g;^%oJ5+hapBU!6AJpIEhmb_>Ie*v2Q!f9N}@x9SK z6#u>IMu|KX3rB%g5f-+{-XLxx;s}O`2(sVkXMYl>H15;*xW+k+&uCoKxTLYEQT^ow z55HydUDWs{5&w&?YJ8iB2KzwcHI1KZn)t@unmSG3LUur|cs64(11(HfP! literal 0 HcmV?d00001 From 3725292e6244e7b14826b4b5e70160d4e17f7c1c Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 00:08:06 -0800 Subject: [PATCH 002/167] Actually only one DMA on F100 --- nuttx/arch/arm/include/stm32/chip.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/arch/arm/include/stm32/chip.h b/nuttx/arch/arm/include/stm32/chip.h index d01929e1ca..2a05f92d75 100644 --- a/nuttx/arch/arm/include/stm32/chip.h +++ b/nuttx/arch/arm/include/stm32/chip.h @@ -73,7 +73,7 @@ # define STM32_NATIM 1 /* One advanced timer TIM1 */ # define STM32_NGTIM 3 /* 16-bit general timers TIM2,3,4 with DMA */ # define STM32_NBTIM 0 /* No basic timers */ -# define STM32_NDMA 2 /* DMA1-2 */ +# define STM32_NDMA 1 /* DMA1 only */ # define STM32_NSPI 2 /* SPI1-2 */ # define STM32_NI2S 0 /* No I2S (?) */ # define STM32_NUSART 3 /* USART1-3 */ From c51b130f17c9a3e06c6b41f1c67b4d58c5f39915 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 00:08:20 -0800 Subject: [PATCH 003/167] Enable DMA --- nuttx/configs/px4io/io/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 30ec5be089..791a7cfeed 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -86,7 +86,7 @@ CONFIG_ARCH_BOOTLOADER=n CONFIG_ARCH_LEDS=n CONFIG_ARCH_BUTTONS=n CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=n +CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y @@ -112,7 +112,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=n # Individual subsystems can be enabled: # # AHB: -CONFIG_STM32_DMA1=n +CONFIG_STM32_DMA1=y CONFIG_STM32_DMA2=n CONFIG_STM32_CRC=n # APB1: From 7c2445f74d9616d2f55166b02eea39003d48280c Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 00:42:18 -0800 Subject: [PATCH 004/167] Don't waste time printing when we have errors - that causes the master to time out --- apps/px4io/i2c.c | 4 ++-- nuttx/fs/stGrJvFk | Bin 3394 -> 0 bytes 2 files changed, 2 insertions(+), 2 deletions(-) delete mode 100644 nuttx/fs/stGrJvFk diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 3b92fe3ef7..c3c8978110 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -116,8 +116,8 @@ i2c_interrupt(int irq, FAR void *context) // XXX not sure what else we need to do here... if (sr1 & I2C_SR1_ERRORMASK) { - debug("errors 0x%04x", sr1 & I2C_SR1_ERRORMASK); - i2c_dump(); + //debug("errors 0x%04x", sr1 & I2C_SR1_ERRORMASK); + //i2c_dump(); rSR1 = 0; } diff --git a/nuttx/fs/stGrJvFk b/nuttx/fs/stGrJvFk deleted file mode 100644 index 640665f6c2e9dc23e5da09011bda42214442d9bb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3394 zcmb_eO>7iZ9Di?Sx4X2Z^aBMHbV_Pk6!xRt7DYgy0ureLI znI@;eG+8KOTFg`|6sAgGnHZlOkGU4RUp;*Mh?FFYMT#*+ZR}6dN=ug%jEODcPLUbw z|1AC8>aD+4#aQ+S;q+f$H$p45_~gyiLjUzg58e9tYvfyMPD)+{%7QXFAC~O0v9u+# z+Yzv=^c}RU%uQH!DqU|z!b)ZO7|W_UtZb%5E0x;7%xvmcS~7b{In&=O$W#C+`4f#d zaqI<?(DbnQjqJ z8IWo<&;Tv*ib56-9pr6U2UUvMK>!PcYGO1U<(lWYaLQG1({pB%Ma2g*MC z%-&&3qP~XTTyoTd{ncuQ^`)j)b!ea&{8qwOLFu@Z8Yb;XHS6ZYJ-ol)Y-ztL`c-OzkQMZnLuN1l~*a zu8`8s+b7oJ;*+%hlPD`}mBUcvaNapObMfIRyXuu&OAGa8%}1)z-EtYn9%sj%hF?jVF5b1CJIQw*&Cc7Je4t+Ay8zwHfhr2%W1bNcI}b;a?^1$qJz}Q5 zx1gIjLslp>q!Vf$}@}?lmN^sd9Xz(c}$);6?b1 zm^{{k-aBpl7m=4Y2GCG{na~;j6nSG*w6H$P7Z}NXe3&?wjfgRi`x?M7Jr7mf&PK>fzY(=g;^%oJ5+hapBU!6AJpIEhmb_>Ie*v2Q!f9N}@x9SK z6#u>IMu|KX3rB%g5f-+{-XLxx;s}O`2(sVkXMYl>H15;*xW+k+&uCoKxTLYEQT^ow z55HydUDWs{5&w&?YJ8iB2KzwcHI1KZn)t@unmSG3LUur|cs64(11(HfP! From 7c7112a157d863665fe1b8fae2a94fbbb17dc199 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 01:11:35 -0800 Subject: [PATCH 005/167] Implement stm32_dmaresidual for the F1 DMA driver. --- nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c index 89b279beaa..66a806d0c4 100644 --- a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c +++ b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c @@ -558,6 +558,24 @@ void stm32_dmastop(DMA_HANDLE handle) stm32_dmachandisable(dmach); } +/**************************************************************************** + * Name: stm32_dmaresidual + * + * Description: + * Returns the number of bytes remaining to be transferred + * + * Assumptions: + * - DMA handle allocated by stm32_dmachannel() + * + ****************************************************************************/ + +size_t stm32_dmaresidual(DMA_HANDLE handle) +{ + struct stm32_dma_s *dmach = (struct stm32_dma_s *)handle; + + return dmachan_getreg(dmach, STM32_DMACHAN_CNDTR_OFFSET); +} + /**************************************************************************** * Name: stm32_dmasample * From f12fa7ee060b33194723df983b643b51410ea4f6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 01:11:52 -0800 Subject: [PATCH 006/167] Don't do retries, since it just complicates things. --- apps/systemcmds/i2c/i2c.c | 59 ++++++++++++++++++--------------------- 1 file changed, 27 insertions(+), 32 deletions(-) diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 1124e560d2..57c61e8242 100644 --- a/apps/systemcmds/i2c/i2c.c +++ b/apps/systemcmds/i2c/i2c.c @@ -94,46 +94,41 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig struct i2c_msg_s msgv[2]; unsigned msgs; int ret; - unsigned tries = 0; - do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - msgs = 0; + msgs = 0; - if (send_len > 0) { - msgv[msgs].addr = address; - msgv[msgs].flags = 0; - msgv[msgs].buffer = send; - msgv[msgs].length = send_len; - msgs++; - } + if (send_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = send; + msgv[msgs].length = send_len; + msgs++; + } - if (recv_len > 0) { - msgv[msgs].addr = address; - msgv[msgs].flags = I2C_M_READ; - msgv[msgs].buffer = recv; - msgv[msgs].length = recv_len; - msgs++; - } + if (recv_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } - if (msgs == 0) - return -1; + if (msgs == 0) + return -1; - /* - * I2C architecture means there is an unavoidable race here - * if there are any devices on the bus with a different frequency - * preference. Really, this is pointless. - */ - I2C_SETFREQUENCY(i2c, 320000); - ret = I2C_TRANSFER(i2c, &msgv[0], msgs); + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(i2c, 320000); + ret = I2C_TRANSFER(i2c, &msgv[0], msgs); - if (ret == OK) - break; - - // reset the I2C bus to unwedge on error + // reset the I2C bus to unwedge on error + if (ret != OK) up_i2creset(i2c); - } while (tries++ < 5); return ret; } From 0dab53ae2674d7a907e861e912806f0ae8e35a35 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 01:12:25 -0800 Subject: [PATCH 007/167] Implement I2C slave DMA. Not working yet. --- apps/px4io/i2c.c | 132 ++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 118 insertions(+), 14 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index c3c8978110..215326835d 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -42,6 +42,7 @@ #include #include #include +#include #define DEBUG #include "px4io.h" @@ -63,30 +64,54 @@ #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_dump(void); +static int i2c_interrupt(int irq, void *context); +static void i2c_dump(void); + +static void i2c_rx_setup(void); +static void i2c_tx_setup(void); +static void i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg); +static void i2c_tx_complete(DMA_HANDLE handle, uint8_t status, void *arg); + +static DMA_HANDLE rx_dma; +static DMA_HANDLE tx_dma; + +uint8_t rx_buf[64]; +unsigned rx_len; +uint8_t tx_buf[64]; +unsigned tx_len; void i2c_init(void) { - // enable the i2c block clock and reset it + 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 + /* configure the i2c GPIOs */ stm32_configgpio(GPIO_I2C1_SCL); stm32_configgpio(GPIO_I2C1_SDA); - // soft-reset the block + /* soft-reset the block */ rCR1 |= I2C_CR1_SWRST; rCR1 = 0; - // set the frequency value in CR2 + /* set for DMA operation */ + rCR2 |= I2C_CR2_ITEVFEN | 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 + /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); if (result < 1) result = 1; @@ -95,15 +120,14 @@ i2c_init(void) rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1); - // set our device address + /* set our device address */ rOAR1 = 0x1a << 1; - // enable event interrupts + /* enable event interrupts */ irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt); up_enable_irq(STM32_IRQ_I2C1EV); - rCR2 |= I2C_CR2_ITEVFEN; - // and enable the I2C port + /* and enable the I2C port */ rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; i2c_dump(); @@ -114,22 +138,46 @@ i2c_interrupt(int irq, FAR void *context) { uint16_t sr1 = rSR1; - // XXX not sure what else we need to do here... + /* XXX not sure what else we need to do here... */ if (sr1 & I2C_SR1_ERRORMASK) { //debug("errors 0x%04x", sr1 & I2C_SR1_ERRORMASK); //i2c_dump(); rSR1 = 0; } +#if 1 if (sr1 & I2C_SR1_ADDR) { - /* XXX we have been addressed, set up to receive */ + /* disable event interrupts since the DMA will be handling the transfer */ + rCR2 &= ~I2C_CR2_ITEVFEN; - /* clear ADDR */ + /* clear ADDR to ack our selection and get direction */ + uint16_t sr2 = rSR2; + + debug("addr"); + + if (sr2 & I2C_SR2_TRA) { + /* we are the transmitter */ + debug("tx"); + + stm32_dmastart(tx_dma, i2c_tx_complete, NULL, false); + } else { + /* we are the receiver */ + debug("rx"); + + stm32_dmastart(rx_dma, i2c_rx_complete, NULL, false); + } + + } +#else + if (sr1 & I2C_SR1_ADDR) { + + /* clear ADDR to ack our selection and get direction */ (void)rSR2; debug("addr"); } +#endif if (sr1 & I2C_SR1_RXNE) { @@ -148,6 +196,62 @@ i2c_interrupt(int irq, FAR void *context) return 0; } +static void +i2c_rx_setup(void) +{ + rx_len = 0; + stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); +} + +static void +i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg) +{ + debug("dma rx %u", status); + + rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); + + debug("len %u", rx_len); + + /* XXX handle reception */ + i2c_rx_setup(); + + /* re-enable event interrupts */ + rCR2 |= I2C_CR2_ITEVFEN; +} + +static void +i2c_tx_setup(void) +{ + tx_len = 0; + stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], sizeof(tx_buf), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); +} + +static void +i2c_tx_complete(DMA_HANDLE handle, uint8_t status, void *arg) +{ + debug("dma tx %u", status); + + tx_len = sizeof(tx_buf) - stm32_dmaresidual(tx_dma); + + debug("len %u", tx_len); + + /* XXX handle reception */ + i2c_rx_setup(); + + /* re-enable event interrupts */ + rCR2 |= I2C_CR2_ITEVFEN; +} + + static void i2c_dump(void) { From 2fb820fabd7c7b675c4e0da026c95546b62424e6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 23:24:17 -0800 Subject: [PATCH 008/167] I2C slave RX DMA works. --- apps/px4io/i2c.c | 84 ++++++++++++++++++++---------------------------- 1 file changed, 34 insertions(+), 50 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 215326835d..e55e992fd6 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -80,6 +80,13 @@ unsigned rx_len; uint8_t tx_buf[64]; unsigned tx_len; +enum { + DIR_NONE = 0, + DIR_TX = 1, + DIR_RX = 2 +}; +unsigned direction; + void i2c_init(void) { @@ -138,61 +145,52 @@ i2c_interrupt(int irq, FAR void *context) { uint16_t sr1 = rSR1; - /* XXX not sure what else we need to do here... */ - if (sr1 & I2C_SR1_ERRORMASK) { - //debug("errors 0x%04x", sr1 & I2C_SR1_ERRORMASK); - //i2c_dump(); - rSR1 = 0; - } - -#if 1 if (sr1 & I2C_SR1_ADDR) { - /* disable event interrupts since the DMA will be handling the transfer */ - rCR2 &= ~I2C_CR2_ITEVFEN; - /* clear ADDR to ack our selection and get direction */ + (void)rSR1; /* as recommended, re-read SR1 */ uint16_t sr2 = rSR2; - debug("addr"); - if (sr2 & I2C_SR2_TRA) { /* we are the transmitter */ - debug("tx"); + direction = DIR_TX; stm32_dmastart(tx_dma, i2c_tx_complete, NULL, false); + } else { /* we are the receiver */ - debug("rx"); - stm32_dmastart(rx_dma, i2c_rx_complete, NULL, false); + direction = DIR_RX; + stm32_dmastart(rx_dma, i2c_rx_complete, NULL, false); } - - } -#else - if (sr1 & I2C_SR1_ADDR) { - - /* clear ADDR to ack our selection and get direction */ - (void)rSR2; - - debug("addr"); - } -#endif - - if (sr1 & I2C_SR1_RXNE) { - - debug("data 0x%02x", rDR); } if (sr1 & I2C_SR1_STOPF) { /* write to CR1 to clear STOPF */ + (void)rSR1; /* as recommended, re-read SR1 */ rCR1 |= I2C_CR1_PE; - debug("stop"); - - /* XXX handle txn */ + /* it's likely that the DMA hasn't stopped, so we have to do it here */ + switch (direction) { + case DIR_TX: + stm32_dmastop(tx_dma); + i2c_tx_complete(tx_dma, 0, NULL); + break; + case DIR_RX: + stm32_dmastop(rx_dma); + i2c_rx_complete(rx_dma, 0, NULL); + break; + default: + /* spurious stop, ignore */ + break; + } + direction = DIR_NONE; } + /* clear any errors that might need it */ + if (sr1 & I2C_SR1_ERRORMASK) + rSR1 = 0; + return 0; } @@ -202,7 +200,7 @@ i2c_rx_setup(void) rx_len = 0; stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | + DMA_CCR_PSIZE_32BITS | DMA_CCR_MSIZE_8BITS | DMA_CCR_PRIMED); } @@ -210,17 +208,10 @@ i2c_rx_setup(void) static void i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg) { - debug("dma rx %u", status); - rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); - debug("len %u", rx_len); - /* XXX handle reception */ i2c_rx_setup(); - - /* re-enable event interrupts */ - rCR2 |= I2C_CR2_ITEVFEN; } static void @@ -238,17 +229,10 @@ i2c_tx_setup(void) static void i2c_tx_complete(DMA_HANDLE handle, uint8_t status, void *arg) { - debug("dma tx %u", status); - tx_len = sizeof(tx_buf) - stm32_dmaresidual(tx_dma); - debug("len %u", tx_len); - /* XXX handle reception */ - i2c_rx_setup(); - - /* re-enable event interrupts */ - rCR2 |= I2C_CR2_ITEVFEN; + i2c_tx_setup(); } From 3cea0959b72fe160e6a05e8efef1d325d12d4544 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 9 Jan 2013 21:39:54 -0800 Subject: [PATCH 009/167] Implement a simple byte loopback server on I2C for more testing. --- apps/px4io/i2c.c | 9 ++++----- apps/systemcmds/i2c/i2c.c | 9 +++++++-- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index e55e992fd6..3e4ac34881 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -78,7 +78,6 @@ static DMA_HANDLE tx_dma; uint8_t rx_buf[64]; unsigned rx_len; uint8_t tx_buf[64]; -unsigned tx_len; enum { DIR_NONE = 0, @@ -210,6 +209,9 @@ i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg) { rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); + for (unsigned i = 0; i < rx_len; i++) + tx_buf[i] = rx_buf[i] + 1; + /* XXX handle reception */ i2c_rx_setup(); } @@ -217,7 +219,6 @@ i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg) static void i2c_tx_setup(void) { - tx_len = 0; stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], sizeof(tx_buf), DMA_CCR_DIR | DMA_CCR_MINC | @@ -229,9 +230,7 @@ i2c_tx_setup(void) static void i2c_tx_complete(DMA_HANDLE handle, uint8_t status, void *arg) { - tx_len = sizeof(tx_buf) - stm32_dmaresidual(tx_dma); - - /* XXX handle reception */ + /* XXX handle transmit-done */ i2c_tx_setup(); } diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 57c61e8242..422d9f915f 100644 --- a/apps/systemcmds/i2c/i2c.c +++ b/apps/systemcmds/i2c/i2c.c @@ -84,8 +84,13 @@ int i2c_main(int argc, char *argv[]) int ret = transfer(PX4_I2C_OBDEV_PX4IO, (uint8_t *)&val, sizeof(val), NULL, 0); if (ret) - errx(1, "transfer failed"); - exit(0); + errx(1, "send failed - %d", ret); + + ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val)); + if (ret) + errx(1, "recive failed - %d", ret); + + errx(0, "got 0x%08x", val); } static int From 81115166a703f993c76b3ed0c627d7a27fd4077e Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 9 Jan 2013 21:40:42 -0800 Subject: [PATCH 010/167] Fix clock_time2ticks to round up, not down. This makes sem_timedwait for at least the necessary period. --- nuttx/sched/clock_time2ticks.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/sched/clock_time2ticks.c b/nuttx/sched/clock_time2ticks.c index 383264d513..9265872bb6 100644 --- a/nuttx/sched/clock_time2ticks.c +++ b/nuttx/sched/clock_time2ticks.c @@ -97,6 +97,6 @@ int clock_time2ticks(FAR const struct timespec *reltime, FAR int *ticks) /* Convert microseconds to clock ticks */ - *ticks = relusec / USEC_PER_TICK; + *ticks = (relusec + USEC_PER_TICK - 1) / USEC_PER_TICK; return OK; } From dadd5d01f996d355cfdc600b7487d71815436610 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 9 Jan 2013 21:41:02 -0800 Subject: [PATCH 011/167] Now that sem_timedwait works, we can turn on interrupt-driven I2C. --- nuttx/configs/px4fmu/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 52013457e3..659cf7e938 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -348,7 +348,7 @@ CONFIG_CAN2_BAUD=700000 # I2C configuration # CONFIG_I2C=y -CONFIG_I2C_POLLED=y +CONFIG_I2C_POLLED=n CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n # Allow 180 us per byte, a wide margin for the 400 KHz clock we're using From bd543fd7fc7ec4a1722e1fcb628735488fe704d9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 9 Jan 2013 21:41:51 -0800 Subject: [PATCH 012/167] =?UTF-8?q?A=20couple=20of=20minor=20tracing=20fix?= =?UTF-8?q?es;=20print=20the=20state=20names=20in=20the=20trace=20dump,=20?= =?UTF-8?q?and=20set=20the=20timestamp=20for=20each=20entry=20(not=20reall?= =?UTF-8?q?y=20a=20useful=20number=20yet=E2=80=A6)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- nuttx/arch/arm/src/stm32/stm32_i2c.c | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index a4b10b55c6..80f1575042 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -185,6 +185,22 @@ enum stm32_trace_e I2CEVENT_ERROR /* Error occurred, param = 0 */ }; +#ifdef CONFIG_I2C_TRACE +static const char *stm32_trace_names[] = { + "NONE ", + "SENDADDR ", + "SENDBYTE ", + "ITBUFEN ", + "RCVBYTE ", + "REITBUFEN ", + "DISITBUFEN", + "BTFNOSTART", + "BTFRESTART", + "BTFSTOP ", + "ERROR " +}; +#endif + /* Trace data */ struct stm32_trace_s @@ -847,6 +863,7 @@ static void stm32_i2c_traceevent(FAR struct stm32_i2c_priv_s *priv, trace->event = event; trace->parm = parm; + trace->time = clock_systimer(); /* Bump up the trace index (unless we are out of trace entries) */ @@ -870,8 +887,8 @@ static void stm32_i2c_tracedump(FAR struct stm32_i2c_priv_s *priv) for (i = 0; i <= priv->tndx; i++) { trace = &priv->trace[i]; - lib_rawprintf("%2d. STATUS: %08x COUNT: %3d EVENT: %2d PARM: %08x TIME: %d\n", - i+1, trace->status, trace->count, trace->event, trace->parm, + lib_rawprintf("%2d. STATUS: %08x COUNT: %3d EVENT: %s PARM: %08x TIME: %d\n", + i+1, trace->status, trace->count, stm32_trace_names[trace->event], trace->parm, trace->time - priv->start_time); } } @@ -1613,8 +1630,8 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms status = stm32_i2c_getstatus(priv); errval = ETIMEDOUT; - i2cdbg("Timed out: CR1: %04x status: %08x\n", - stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET), status); + i2cdbg("Timed out: CR1: %04x status: %08x after %d\n", + stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET), status, timeout_us); /* "Note: When the STOP, START or PEC bit is set, the software must * not perform any write access to I2C_CR1 before this bit is From 770f2545fb60dac5a87b0050fffad94e681063ab Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 00:07:04 -0800 Subject: [PATCH 013/167] Implement non-stop DMA mode (circular) for the F1x DMA driver. --- nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c | 38 +++++++++++++++------- 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c index 66a806d0c4..4d5371e7a5 100644 --- a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c +++ b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c @@ -90,6 +90,7 @@ struct stm32_dma_s { uint8_t chan; /* DMA channel number (0-6) */ uint8_t irq; /* DMA channel IRQ number */ + bool nonstop; /* Stream is configured in non-stopping mode */ sem_t sem; /* Used to wait for DMA channel to become available */ uint32_t base; /* DMA register channel base address */ dma_callback_t callback; /* Callback invoked when the DMA completes */ @@ -303,14 +304,10 @@ static int stm32_dmainterrupt(int irq, void *context) } dmach = &g_dma[chndx]; - /* Get the interrupt status (for this channel only) -- not currently used */ + /* Get the interrupt status (for this channel only) */ isr = dmabase_getreg(dmach, STM32_DMA_ISR_OFFSET) & DMA_ISR_CHAN_MASK(dmach->chan); - /* Disable the DMA channel */ - - stm32_dmachandisable(dmach); - /* Invoke the callback */ if (dmach->callback) @@ -493,6 +490,7 @@ void stm32_dmasetup(DMA_HANDLE handle, uint32_t paddr, uint32_t maddr, size_t nt ccr &= (DMA_CCR_MEM2MEM|DMA_CCR_PL_MASK|DMA_CCR_MSIZE_MASK|DMA_CCR_PSIZE_MASK| DMA_CCR_MINC|DMA_CCR_PINC|DMA_CCR_CIRC|DMA_CCR_DIR); regval |= ccr; + dmach->nonstop = (ccr & DMA_CCR_CIRC) != 0; dmachan_putreg(dmach, STM32_DMACHAN_CCR_OFFSET, regval); } @@ -528,14 +526,30 @@ void stm32_dmastart(DMA_HANDLE handle, dma_callback_t callback, void *arg, bool ccr = dmachan_getreg(dmach, STM32_DMACHAN_CCR_OFFSET); ccr |= DMA_CCR_EN; - /* Once half of the bytes are transferred, the half-transfer flag (HTIF) is - * set and an interrupt is generated if the Half-Transfer Interrupt Enable - * bit (HTIE) is set. At the end of the transfer, the Transfer Complete Flag - * (TCIF) is set and an interrupt is generated if the Transfer Complete - * Interrupt Enable bit (TCIE) is set. - */ + if (!dmach->nonstop) + { + /* Once half of the bytes are transferred, the half-transfer flag (HTIF) is + * set and an interrupt is generated if the Half-Transfer Interrupt Enable + * bit (HTIE) is set. At the end of the transfer, the Transfer Complete Flag + * (TCIF) is set and an interrupt is generated if the Transfer Complete + * Interrupt Enable bit (TCIE) is set. + */ + + ccr |= (half ? (DMA_CCR_HTIE|DMA_CCR_TEIE) : (DMA_CCR_TCIE|DMA_CCR_TEIE)); + + } + else + { + /* In nonstop mode, when the transfer completes it immediately resets + * and starts again. The transfer-complete interrupt is thus always + * enabled, and the half-complete interrupt can be used in circular + * mode to determine when the buffer is half-full, or in double-buffered + * mode to determine when one of the two buffers is full. + */ + + ccr |= (half ? DMA_CCR_HTIE : 0) | DMA_CCR_TCIE | DMA_CCR_TEIE; + } - ccr |= (half ? (DMA_CCR_HTIE|DMA_CCR_TEIE) : (DMA_CCR_TCIE|DMA_CCR_TEIE)); dmachan_putreg(dmach, STM32_DMACHAN_CCR_OFFSET, ccr); } From 4f285f7c80904bf7685ab3d5d8bf515b5c0ad7ab Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 00:08:13 -0800 Subject: [PATCH 014/167] Configure the DMA channels in circular mode so that we don't have to deal with the case where DMA stops but the master is still talking. Use AF as well as STOPF to decide when transfer has ended. We don't seem to get STOPF when we are transmitting. --- apps/px4io/i2c.c | 63 +++++++++++++++++++++++++++++++++--------------- 1 file changed, 44 insertions(+), 19 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 3e4ac34881..61daa9ada4 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -69,8 +69,8 @@ static void i2c_dump(void); static void i2c_rx_setup(void); static void i2c_tx_setup(void); -static void i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg); -static void i2c_tx_complete(DMA_HANDLE handle, uint8_t status, void *arg); +static void i2c_rx_complete(void); +static void i2c_tx_complete(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; @@ -111,7 +111,7 @@ i2c_init(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -131,7 +131,9 @@ i2c_init(void) /* 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; @@ -154,30 +156,31 @@ i2c_interrupt(int irq, FAR void *context) /* we are the transmitter */ direction = DIR_TX; - stm32_dmastart(tx_dma, i2c_tx_complete, NULL, false); + stm32_dmastart(tx_dma, NULL, NULL, false); } else { /* we are the receiver */ direction = DIR_RX; - stm32_dmastart(rx_dma, i2c_rx_complete, NULL, false); + stm32_dmastart(rx_dma, NULL, NULL, false); } } - if (sr1 & I2C_SR1_STOPF) { - /* write to CR1 to clear STOPF */ - (void)rSR1; /* as recommended, re-read SR1 */ - rCR1 |= I2C_CR1_PE; + if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF)) { + + if (sr1 & I2C_SR1_STOPF) { + /* write to CR1 to clear STOPF */ + (void)rSR1; /* as recommended, re-read SR1 */ + rCR1 |= I2C_CR1_PE; + } /* it's likely that the DMA hasn't stopped, so we have to do it here */ switch (direction) { case DIR_TX: - stm32_dmastop(tx_dma); - i2c_tx_complete(tx_dma, 0, NULL); + i2c_tx_complete(); break; case DIR_RX: - stm32_dmastop(rx_dma); - i2c_rx_complete(rx_dma, 0, NULL); + i2c_rx_complete(); break; default: /* spurious stop, ignore */ @@ -186,7 +189,7 @@ i2c_interrupt(int irq, FAR void *context) direction = DIR_NONE; } - /* clear any errors that might need it */ + /* clear any errors that might need it (this handles AF as well */ if (sr1 & I2C_SR1_ERRORMASK) rSR1 = 0; @@ -196,8 +199,14 @@ i2c_interrupt(int irq, FAR void *context) 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 | @@ -205,22 +214,33 @@ i2c_rx_setup(void) } static void -i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg) +i2c_rx_complete(void) { rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); - - for (unsigned i = 0; i < rx_len; i++) - tx_buf[i] = rx_buf[i] + 1; + stm32_dmastop(rx_dma); /* XXX handle reception */ + for (unsigned i = 0; i < rx_len; i++) { + tx_buf[i] = rx_buf[i] + 1; + rx_buf[i] = 0; + } + + /* 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], sizeof(tx_buf), DMA_CCR_DIR | + DMA_CCR_CIRC | DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | DMA_CCR_MSIZE_8BITS | @@ -228,9 +248,14 @@ i2c_tx_setup(void) } static void -i2c_tx_complete(DMA_HANDLE handle, uint8_t status, void *arg) +i2c_tx_complete(void) { + stm32_dmastop(tx_dma); + /* XXX handle transmit-done */ + + /* prepare for the next transaction */ + memset(tx_buf, 0, sizeof(tx_buf)); i2c_tx_setup(); } From 97136375e393d71000a8f5e7c4c5a1b1bcb0f464 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 01:56:46 -0800 Subject: [PATCH 015/167] Turn off the I2C register dump at startup. --- apps/px4io/i2c.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 61daa9ada4..037c64522e 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -65,7 +65,9 @@ #define rTRISE REG(STM32_I2C_TRISE_OFFSET) static int i2c_interrupt(int irq, void *context); +#ifdef DEBUG static void i2c_dump(void); +#endif static void i2c_rx_setup(void); static void i2c_tx_setup(void); @@ -138,7 +140,9 @@ i2c_init(void) /* and enable the I2C port */ rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; +#ifdef DEBUG i2c_dump(); +#endif } static int @@ -259,7 +263,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } - +#ifdef DEBUG static void i2c_dump(void) { @@ -268,4 +272,4 @@ i2c_dump(void) debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); } - +#endif \ No newline at end of file From e2f7a468121cb41131b453821dcc79d4de8d8e28 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 01:57:16 -0800 Subject: [PATCH 016/167] Clear the interrupting condition when we take the DMA interrupt. --- nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c index 4d5371e7a5..a02ceb230f 100644 --- a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c +++ b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c @@ -308,6 +308,10 @@ static int stm32_dmainterrupt(int irq, void *context) isr = dmabase_getreg(dmach, STM32_DMA_ISR_OFFSET) & DMA_ISR_CHAN_MASK(dmach->chan); + /* Clear the interrupts we are handling */ + + dmabase_putreg(dmach, STM32_DMA_IFCR_OFFSET, isr); + /* Invoke the callback */ if (dmach->callback) From 469d13fdfe3f7aa1258209ef2bc6645a5f0c231c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 01:57:50 -0800 Subject: [PATCH 017/167] Implement serial receive DMA for the F1xx. This is not quite working right yet. Some clients work, others not so much. --- apps/drivers/boards/px4io/px4io_init.c | 21 +++++ nuttx/arch/arm/src/stm32/stm32_serial.c | 108 ++++++++++++++++-------- nuttx/arch/arm/src/stm32/stm32_uart.h | 2 +- nuttx/configs/px4io/io/defconfig | 5 ++ 4 files changed, 100 insertions(+), 36 deletions(-) diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c index 14e8dc13a9..cb288aca84 100644 --- a/apps/drivers/boards/px4io/px4io_init.c +++ b/apps/drivers/boards/px4io/px4io_init.c @@ -95,4 +95,25 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VBATT); stm32_configgpio(GPIO_ADC_IN5); + + /* set up the serial DMA polling */ +#ifdef SERIAL_HAVE_DMA + { + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + } +#endif } diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c index 0868c3cd3d..f938903454 100644 --- a/nuttx/arch/arm/src/stm32/stm32_serial.c +++ b/nuttx/arch/arm/src/stm32/stm32_serial.c @@ -79,50 +79,74 @@ #if SERIAL_HAVE_DMA -/* Verify that DMA has been enabled an the DMA channel has been defined. - * NOTE: These assignments may only be true for the F4. +# if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +/* Verify that DMA has been enabled and the DMA channel has been defined. */ -# if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART6_RXDMA) -# ifndef CONFIG_STM32_DMA2 -# error STM32 USART1/6 receive DMA requires CONFIG_STM32_DMA2 +# if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART6_RXDMA) +# ifndef CONFIG_STM32_DMA2 +# error STM32 USART1/6 receive DMA requires CONFIG_STM32_DMA2 +# endif # endif -# endif -# if defined(CONFIG_USART2_RXDMA) || defined(CONFIG_USART3_RXDMA) || \ +# if defined(CONFIG_USART2_RXDMA) || defined(CONFIG_USART3_RXDMA) || \ defined(CONFIG_UART4_RXDMA) || defined(CONFIG_UART5_RXDMA) -# ifndef CONFIG_STM32_DMA1 -# error STM32 USART2/3/4/5 receive DMA requires CONFIG_STM32_DMA1 +# ifndef CONFIG_STM32_DMA1 +# error STM32 USART2/3/4/5 receive DMA requires CONFIG_STM32_DMA1 +# endif # endif -# endif /* For the F4, there are alternate DMA channels for USART1 and 6. * Logic in the board.h file make the DMA channel selection by defining * the following in the board.h file. */ -# if defined(CONFIG_USART1_RXDMA) && !defined(DMAMAP_USART1_RX) -# error "USART1 DMA channel not defined (DMAMAP_USART1_RX)" -# endif +# if defined(CONFIG_USART1_RXDMA) && !defined(DMAMAP_USART1_RX) +# error "USART1 DMA channel not defined (DMAMAP_USART1_RX)" +# endif -# if defined(CONFIG_USART2_RXDMA) && !defined(DMAMAP_USART2_RX) -# error "USART2 DMA channel not defined (DMAMAP_USART2_RX)" -# endif +# if defined(CONFIG_USART2_RXDMA) && !defined(DMAMAP_USART2_RX) +# error "USART2 DMA channel not defined (DMAMAP_USART2_RX)" +# endif -# if defined(CONFIG_USART3_RXDMA) && !defined(DMAMAP_USART3_RX) -# error "USART3 DMA channel not defined (DMAMAP_USART3_RX)" -# endif +# if defined(CONFIG_USART3_RXDMA) && !defined(DMAMAP_USART3_RX) +# error "USART3 DMA channel not defined (DMAMAP_USART3_RX)" +# endif -# if defined(CONFIG_UART4_RXDMA) && !defined(DMAMAP_UART4_RX) -# error "UART4 DMA channel not defined (DMAMAP_UART4_RX)" -# endif +# if defined(CONFIG_UART4_RXDMA) && !defined(DMAMAP_UART4_RX) +# error "UART4 DMA channel not defined (DMAMAP_UART4_RX)" +# endif -# if defined(CONFIG_UART5_RXDMA) && !defined(DMAMAP_UART5_RX) -# error "UART5 DMA channel not defined (DMAMAP_UART5_RX)" -# endif +# if defined(CONFIG_UART5_RXDMA) && !defined(DMAMAP_UART5_RX) +# error "UART5 DMA channel not defined (DMAMAP_UART5_RX)" +# endif + +# if defined(CONFIG_USART6_RXDMA) && !defined(DMAMAP_USART6_RX) +# error "USART6 DMA channel not defined (DMAMAP_USART6_RX)" +# endif + +# elif defined(CONFIG_STM32_STM32F10XX) + +# if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART2_RXDMA) || \ + defined(CONFIG_USART3_RXDMA) +# ifndef CONFIG_STM32_DMA1 +# error STM32 USART1/2/3 receive DMA requires CONFIG_STM32_DMA1 +# endif +# endif + +# if defined(CONFIG_UART4_RXDMA) +# ifndef CONFIG_STM32_DMA2 +# error STM32 USART4 receive DMA requires CONFIG_STM32_DMA2 +# endif +# endif + +/* There are no optional DMA channel assignments for the F1 */ + +# define DMAMAP_USART1_RX DMACHAN_USART1_RX +# define DMAMAP_USART2_RX DMACHAN_USART2_RX +# define DMAMAP_USART3_RX DMACHAN_USART3_RX +# define DMAMAP_UART4_RX DMACHAN_USART4_RX -# if defined(CONFIG_USART6_RXDMA) && !defined(DMAMAP_USART6_RX) -# error "USART6 DMA channel not defined (DMAMAP_USART6_RX)" # endif /* The DMA buffer size when using RX DMA to emulate a FIFO. @@ -156,6 +180,27 @@ # error "Unknown STM32 DMA" # endif +/* DMA control word */ + +# if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) +# define SERIAL_DMA_CONTROL_WORD \ + (DMA_SCR_DIR_P2M | \ + DMA_SCR_CIRC | \ + DMA_SCR_MINC | \ + DMA_SCR_PSIZE_8BITS | \ + DMA_SCR_MSIZE_8BITS | \ + CONFIG_USART_DMAPRIO | \ + DMA_SCR_PBURST_SINGLE | \ + DMA_SCR_MBURST_SINGLE) +# else +# define SERIAL_DMA_CONTROL_WORD \ + (DMA_CCR_CIRC | \ + DMA_CCR_MINC | \ + DMA_CCR_PSIZE_8BITS | \ + DMA_CCR_MSIZE_8BITS | \ + CONFIG_USART_DMAPRIO) +# endif + #endif /* Power management definitions */ @@ -1037,14 +1082,7 @@ static int up_dma_setup(struct uart_dev_s *dev) priv->usartbase + STM32_USART_DR_OFFSET, (uint32_t)priv->rxfifo, RXDMA_BUFFER_SIZE, - DMA_SCR_DIR_P2M | - DMA_SCR_CIRC | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - CONFIG_USART_DMAPRIO | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); + SERIAL_DMA_CONTROL_WORD); /* Reset our DMA shadow pointer to match the address just * programmed above. diff --git a/nuttx/arch/arm/src/stm32/stm32_uart.h b/nuttx/arch/arm/src/stm32/stm32_uart.h index a70923cbf9..194701a3f3 100644 --- a/nuttx/arch/arm/src/stm32/stm32_uart.h +++ b/nuttx/arch/arm/src/stm32/stm32_uart.h @@ -145,7 +145,7 @@ * extended to the F1 and F2 with a little effort in the DMA code. */ -#if !defined(HAVE_UART) || !defined(CONFIG_ARCH_DMA) || !defined(CONFIG_STM32_STM32F40XX) +#if !defined(HAVE_UART) || !defined(CONFIG_ARCH_DMA) # undef CONFIG_USART1_RXDMA # undef CONFIG_USART2_RXDMA # undef CONFIG_USART3_RXDMA diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 791a7cfeed..e0986b72c9 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -188,6 +188,11 @@ CONFIG_USART1_2STOP=0 CONFIG_USART2_2STOP=0 CONFIG_USART3_2STOP=0 +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +CONFIG_USART2_RXDMA=y +CONFIG_USART3_RXDMA=y + # # PX4IO specific driver settings # From 329f595bca22ca7f253833d0189c1bf61c23c631 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 02:11:53 -0800 Subject: [PATCH 018/167] Don't try to set up serial polling before the HRT has been started. --- apps/drivers/boards/px4io/px4io_init.c | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c index cb288aca84..14e8dc13a9 100644 --- a/apps/drivers/boards/px4io/px4io_init.c +++ b/apps/drivers/boards/px4io/px4io_init.c @@ -95,25 +95,4 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VBATT); stm32_configgpio(GPIO_ADC_IN5); - - /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif } From 5e35491a386006c8f27b7761b3845a73bf85f3fa Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 02:12:49 -0800 Subject: [PATCH 019/167] We can't have DMA on both I2C1 and USART2. Since we need it more for I2C, and since USART2 is going back to being ignored once I2C works, let's make the call. Turn off the debug output on I2C for now. --- apps/px4io/px4io.c | 10 ++++++++++ nuttx/configs/px4io/io/defconfig | 3 ++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 4e3555b13e..e51c1c73c1 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -51,6 +51,8 @@ #include #include +#include + #include "px4io.h" __EXPORT int user_start(int argc, char *argv[]); @@ -59,6 +61,8 @@ extern void up_cxxinitialize(void); struct sys_state_s system_state; +static struct hrt_call serial_dma_call; + int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ @@ -72,6 +76,12 @@ int user_start(int argc, char *argv[]) /* 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 */ lib_lowprintf("\nPX4IO: starting\n"); diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index e0986b72c9..d2a4f38487 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -190,7 +190,8 @@ CONFIG_USART3_2STOP=0 CONFIG_USART1_RXDMA=y SERIAL_HAVE_CONSOLE_DMA=y -CONFIG_USART2_RXDMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n CONFIG_USART3_RXDMA=y # From 1cecba2a86520b042600b7194e8f7166560d1fdd Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 09:40:55 -0800 Subject: [PATCH 020/167] Turn off i2c slave debug output for real. --- apps/px4io/i2c.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 037c64522e..4e95dd5831 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -44,7 +44,7 @@ #include #include -#define DEBUG +//#define DEBUG #include "px4io.h" /* From 5e91a36623ed9e267be18ad8e348fdc5fc7a887f Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 10:00:47 -0800 Subject: [PATCH 021/167] Drop the 'nonstop' flag since we can infer it when we need it. Submitted this version of the serial DMA changes for integration into mainline NuttX. --- nuttx/arch/arm/src/stm32/stm32_uart.h | 5 +---- nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c | 8 +++++--- nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c | 11 +++++++---- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_uart.h b/nuttx/arch/arm/src/stm32/stm32_uart.h index 194701a3f3..13d49249c4 100644 --- a/nuttx/arch/arm/src/stm32/stm32_uart.h +++ b/nuttx/arch/arm/src/stm32/stm32_uart.h @@ -140,10 +140,7 @@ # undef HAVE_CONSOLE #endif -/* DMA support is only provided if CONFIG_ARCH_DMA is in the NuttX configuration. - * Furthermore, DMA support is currently only implemented for the F4 (but could be - * extended to the F1 and F2 with a little effort in the DMA code. - */ +/* DMA support is only provided if CONFIG_ARCH_DMA is in the NuttX configuration */ #if !defined(HAVE_UART) || !defined(CONFIG_ARCH_DMA) # undef CONFIG_USART1_RXDMA diff --git a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c index a02ceb230f..13324b8ac0 100644 --- a/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c +++ b/nuttx/arch/arm/src/stm32/stm32f10xxx_dma.c @@ -90,7 +90,6 @@ struct stm32_dma_s { uint8_t chan; /* DMA channel number (0-6) */ uint8_t irq; /* DMA channel IRQ number */ - bool nonstop; /* Stream is configured in non-stopping mode */ sem_t sem; /* Used to wait for DMA channel to become available */ uint32_t base; /* DMA register channel base address */ dma_callback_t callback; /* Callback invoked when the DMA completes */ @@ -494,7 +493,6 @@ void stm32_dmasetup(DMA_HANDLE handle, uint32_t paddr, uint32_t maddr, size_t nt ccr &= (DMA_CCR_MEM2MEM|DMA_CCR_PL_MASK|DMA_CCR_MSIZE_MASK|DMA_CCR_PSIZE_MASK| DMA_CCR_MINC|DMA_CCR_PINC|DMA_CCR_CIRC|DMA_CCR_DIR); regval |= ccr; - dmach->nonstop = (ccr & DMA_CCR_CIRC) != 0; dmachan_putreg(dmach, STM32_DMACHAN_CCR_OFFSET, regval); } @@ -530,7 +528,11 @@ void stm32_dmastart(DMA_HANDLE handle, dma_callback_t callback, void *arg, bool ccr = dmachan_getreg(dmach, STM32_DMACHAN_CCR_OFFSET); ccr |= DMA_CCR_EN; - if (!dmach->nonstop) + /* In normal mode, interrupt at either half or full completion. In circular mode, + * always interrupt on buffer wrap, and optionally interrupt at the halfway point. + */ + + if ((ccr & DMA_CCR_CIRC) == 0) { /* Once half of the bytes are transferred, the half-transfer flag (HTIF) is * set and an interrupt is generated if the Half-Transfer Interrupt Enable diff --git a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c index dcbbf1856f..40fce8cb54 100644 --- a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c +++ b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c @@ -94,7 +94,6 @@ struct stm32_dma_s uint8_t irq; /* DMA stream IRQ number */ uint8_t shift; /* ISR/IFCR bit shift value */ uint8_t channel; /* DMA channel number (0-7) */ - bool nonstop; /* Stream is configured in a non-stopping mode. */ sem_t sem; /* Used to wait for DMA channel to become available */ uint32_t base; /* DMA register channel base address */ dma_callback_t callback; /* Callback invoked when the DMA completes */ @@ -728,7 +727,6 @@ void stm32_dmasetup(DMA_HANDLE handle, uint32_t paddr, uint32_t maddr, DMA_SCR_DBM|DMA_SCR_CIRC| DMA_SCR_PBURST_MASK|DMA_SCR_MBURST_MASK); regval |= scr; - dmast->nonstop = (scr & (DMA_SCR_DBM|DMA_SCR_CIRC)) != 0; dmast_putreg(dmast, STM32_DMA_SCR_OFFSET, regval); } @@ -764,7 +762,12 @@ void stm32_dmastart(DMA_HANDLE handle, dma_callback_t callback, void *arg, bool scr = dmast_getreg(dmast, STM32_DMA_SCR_OFFSET); scr |= DMA_SCR_EN; - if (!dmast->nonstop) + /* In normal mode, interrupt at either half or full completion. In circular + * and double-buffered modes, always interrupt on buffer wrap, and optionally + * interrupt at the halfway point. + */ + + if ((scr & (DMA_SCR_DBM|DMA_SCR_CIRC)) == 0) { /* Once half of the bytes are transferred, the half-transfer flag (HTIF) is * set and an interrupt is generated if the Half-Transfer Interrupt Enable @@ -777,7 +780,7 @@ void stm32_dmastart(DMA_HANDLE handle, dma_callback_t callback, void *arg, bool } else { - /* In nonstop mode, when the transfer completes it immediately resets + /* In non-stop modes, when the transfer completes it immediately resets * and starts again. The transfer-complete interrupt is thus always * enabled, and the half-complete interrupt can be used in circular * mode to determine when the buffer is half-full, or in double-buffered From b0fb86a693db595c7adcd61c4179f0ddb8807c4e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 23:43:03 -0800 Subject: [PATCH 022/167] Sketch out the protocol as it will be on top of I2C --- apps/px4io/protocol.h | 106 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 104 insertions(+), 2 deletions(-) diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 90236b40c9..218ab90308 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -31,15 +31,117 @@ * ****************************************************************************/ +#pragma once + /** - * @file PX4FMU <-> PX4IO messaging protocol. + * @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. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + */ + +/* 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 */ +#define PX4IO_P_CONFIG_POWERSW_COUNT 9 /* harcoded # of switched power 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_PPM (1 << 2) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */ + +#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_AP_LOST (1 << 4) + +#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ +#define PX4IO_P_STATUS_TEMPERATURE 5 /* temperature in (units tbd) */ + +/* 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 /* 0..CONFIG_RC_INPUT_COUNT-1 */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RAW_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 100 +#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 (1 << 1) /* request local override */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ + +#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */ + +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_POWERSW 6 /* bitmask of switched power outputs, 0 = off, 1 = on */ + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 102 + + + +/* + * Old serial PX4FMU <-> PX4IO messaging protocol. * * This initial version of the protocol is very simple; each side transmits a * complete update with each frame. This avoids the sending of many small * messages and the corresponding complexity involved. */ -#pragma once #define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 From 8ebe21b27b279b5d941d4829e5ebee28b84b146c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 13 Jan 2013 02:20:01 -0800 Subject: [PATCH 023/167] Checkpoint - I2C protocol register decode --- Firmware.sublime-project | 1 + apps/px4io/protocol.h | 20 +-- apps/px4io/px4io.h | 36 +++-- apps/px4io/registers.c | 334 +++++++++++++++++++++++++++++++++++++++ 4 files changed, 369 insertions(+), 22 deletions(-) create mode 100644 apps/px4io/registers.c diff --git a/Firmware.sublime-project b/Firmware.sublime-project index a316ae2fa7..72bacee9fd 100644 --- a/Firmware.sublime-project +++ b/Firmware.sublime-project @@ -7,6 +7,7 @@ [ "*.o", "*.a", + "*.d", ".built", ".context", ".depend", diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 218ab90308..aabc476dd7 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -62,6 +62,11 @@ * packed. */ +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_RELAY_CHANNELS 4 + + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ @@ -73,7 +78,6 @@ #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 */ -#define PX4IO_P_CONFIG_POWERSW_COUNT 9 /* harcoded # of switched power outputs */ /* dynamic status page */ #define PX4IO_PAGE_STATUS 1 @@ -83,9 +87,10 @@ #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_PPM (1 << 2) /* PPM input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */ +#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_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 */ @@ -107,7 +112,7 @@ #define PX4IO_PAGE_RAW_RC_INPUT 4 /* 0..CONFIG_RC_INPUT_COUNT-1 */ /* array of scaled RC input values, -10000..10000 */ -#define PX4IO_PAGE_RAW_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */ +#define PX4IO_PAGE_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */ /* array of raw ADC values */ #define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ @@ -123,8 +128,7 @@ #define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay outputs, 0 = off, 1 = on */ -#define PX4IO_P_SETUP_POWERSW 6 /* bitmask of switched power outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */ @@ -143,8 +147,6 @@ */ -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 #pragma pack(push, 1) diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index afbaa78dcc..fb06596023 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -63,8 +63,24 @@ # 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 */ + /* * System state structure. + * + * XXX note that many fields here are deprecated and replaced by + * registers. */ struct sys_state_s { @@ -128,12 +144,6 @@ struct sys_state_s { */ uint64_t fmu_data_received_time; - /** - * Current serial interface mode, per the serial_rx_mode parameter - * in the config packet. - */ - uint8_t serial_rx_mode; - /** * If true, the RC signal has been lost for more than a timeout interval */ @@ -206,6 +216,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define ADC_VBATT 4 #define ADC_IN5 5 +#define ADC_CHANNEL_COUNT 2 /* * Mixer @@ -224,6 +235,12 @@ extern void safety_init(void); extern void comms_main(void) __attribute__((noreturn)); 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 */ @@ -238,10 +255,3 @@ extern int dsm_init(const char *device); extern bool dsm_input(void); extern int sbus_init(const char *device); extern bool sbus_input(void); - -/* - * Assertion codes - */ -#define A_GPIO_OPEN_FAIL 100 -#define A_SERVO_OPEN_FAIL 101 -#define A_INPUTQ_OPEN_FAIL 102 diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c new file mode 100644 index 0000000000..1397dd2a44 --- /dev/null +++ b/apps/px4io/registers.c @@ -0,0 +1,334 @@ +/**************************************************************************** + * + * 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 "px4io.h" +#include "protocol.h" + +static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); + +/** + * Setup registers + */ +uint16_t r_page_setup[] = +{ + [PX4IO_P_SETUP_ARMING] = 0, + [PX4IO_P_SETUP_PWM_RATES] = 0, + [PX4IO_P_SETUP_PWM_LOWRATE] = 50, + [PX4IO_P_SETUP_PWM_HIGHRATE] = 200, + [PX4IO_P_SETUP_RELAYS] = 0, +}; + +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE) +#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) +#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PXIO_RELAY_CHANNELS) - 1) + +/** + * Control values from the FMU. + */ +uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; + +/** + * Static configuration parameters. + */ +static const uint16_t r_page_config[] = { + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 0, + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0, + [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 0, + [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, + [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, +}; + +/** + * 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_TEMPERATURE] = 0 +}; + +/** + * ADC input buffer. + */ +uint16_t r_page_adc[ADC_CHANNEL_COUNT]; + +/** + * Post-mixed actuator values. + */ +uint16_t r_page_actuators[IO_SERVO_COUNT]; + +/** + * Servo PWM values + */ +uint16_t r_page_servos[IO_SERVO_COUNT]; + +/** + * Scaled/routed RC input + */ +uint16_t r_page_rc_input[MAX_CONTROL_CHANNELS]; + +/** + * Raw RC input + */ +uint16_t r_page_raw_rc_input[MAX_CONTROL_CHANNELS]; + + +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 scaling - should be -10000..10000 */ + r_page_controls[offset] = *values; + + offset++; + num_values--; + values++; + } + + /* XXX we should cause a mixer tick ASAP */ + system_state.mixer_fmu_available = true; + 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_page_status[PX4IO_P_STATUS_ALARMS] &= ~value; + break; + + default: + /* just ignore writes to other registers in this page */ + break; + } + break; + + case PX4IO_PAGE_SETUP: + switch (offset) { + case PX4IO_P_SETUP_ARMING: + + value &= PX4IO_P_SETUP_ARMING_VALID; + r_page_setup[PX4IO_P_SETUP_ARMING] = value; + + /* update arming state - disarm if no longer OK */ + if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) + system_state.armed = false; + + break; + + case PX4IO_P_SETUP_PWM_RATES: + value &= PX4IO_P_SETUP_RATES_VALID; + r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value; + /* XXX re-configure timers */ + break; + + case PX4IO_P_SETUP_PWM_LOWRATE: + if (value < 50) + value = 50; + if (value > 400) + value = 400; + r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value; + /* XXX re-configure timers */ + break; + + case PX4IO_P_SETUP_PWM_HIGHRATE: + if (value < 50) + value = 50; + if (value > 400) + value = 400; + r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value; + /* XXX re-configure timers */ + break; + + case PX4IO_P_SETUP_RELAYS: + value &= PX4IO_P_SETUP_RELAYS_VALID; + r_page_setup[PX4IO_P_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; + + default: + return -1; + } + break; + + default: + return -1; + } + return 0; +} + +int +registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) +{ + + switch (page) { + case PX4IO_PAGE_CONFIG: + *values = r_page_config; + *num_values = sizeof(r_page_config) / sizeof(r_page_config[0]); + break; + + case PX4IO_PAGE_STATUS: + { + struct mallinfo minfo = mallinfo(); + r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks; + } + /* XXX PX4IO_P_STATUS_CPULOAD */ + r_page_status[PX4IO_P_STATUS_FLAGS] = + (system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) | + (system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) | + ((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0)) + /* XXX specific receiver status */ + + /* XXX PX4IO_P_STATUS_ALARMS] */ + + { + /* + * 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); + r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10; + } + /* XXX PX4IO_P_STATUS_TEMPERATURE */ + + *values = r_page_status; + *num_values = sizeof(r_page_status) / sizeof(r_page_status[0]); + break; + + case PX4IO_PAGE_ACTUATORS: + *values = r_page_actuators; + *num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]); + break; + + case PX4IO_PAGE_SERVOS: + *values = system_state.servos; + *num_values = IO_SERVO_COUNT; + break; + + case PX4IO_PAGE_RAW_RC_INPUT: + *values = r_page_raw_rc_input; + *num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]); + break; + + case PX4IO_PAGE_RC_INPUT: + *values = system_state.rc_channel_data; + *num_values = system_state.rc_channels; + return -1; + + case PX4IO_PAGE_RAW_ADC_INPUT: + r_page_adc[0] = adc_measure(ADC_VBATT); + r_page_adc[1] = adc_measure(ADC_IN5); + *values = r_page_adc; + *num_values = ADC_CHANNEL_COUNT; + break; + + default: + return -1; + } + + /* if the offset is beyond the end of the page, we have no data */ + if (*num_values <= offset) + return -1; + + /* adjust value count and pointer for the page offset */ + *num_values -= offset; + *values += offset; + + return 0; +} From 4e38615595abd9d27d0cb000caafb98cc3670abe Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 13 Jan 2013 18:57:27 -0800 Subject: [PATCH 024/167] Major workover of the PX4IO firmware for I2C operation. --- apps/px4io/Makefile | 12 +- apps/px4io/controls.c | 288 +++++++++++++++++++++++++++++------------ apps/px4io/dsm.c | 44 +++---- apps/px4io/i2c.c | 1 - apps/px4io/mixer.cpp | 130 ++++++++++--------- apps/px4io/protocol.h | 39 +++++- apps/px4io/px4io.c | 32 +++-- apps/px4io/px4io.h | 77 ++++++----- apps/px4io/registers.c | 187 +++++++++++++++++++------- apps/px4io/safety.c | 29 ++--- apps/px4io/sbus.c | 42 +++--- 11 files changed, 561 insertions(+), 320 deletions(-) diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index d97f963ab4..0eb3ffcf7a 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -39,11 +39,19 @@ # Note that we pull a couple of specific files from the systemlib, since # we can't support it all. # -CSRCS = $(wildcard *.c) \ +CSRCS = adc.c \ + controls.c \ + dsm.c \ + i2c.c \ + px4io.c \ + registers.c \ + safety.c \ + sbus.c \ ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ ../systemlib/up_cxxinitialize.c -CXXSRCS = $(wildcard *.cpp) + +CXXSRCS = mixer.cpp INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 169d5bb817..072d296dab 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -37,34 +37,22 @@ * R/C inputs and servo outputs. */ - #include -#include #include -#include -#include -#include -#include -#include -#include -#include #include -#include - #include -#include #include #include -#define DEBUG +//#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 1700 -#define RC_CHANNEL_LOW_THRESH 1300 +#define RC_CHANNEL_HIGH_THRESH 5000 +#define RC_CHANNEL_LOW_THRESH -5000 -static void ppm_input(void); +static bool ppm_input(uint16_t *values, uint16_t *num_values); void controls_main(void) @@ -79,9 +67,26 @@ controls_main(void) fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; + ASSERT(fds[0].fd >= 0); + ASSERT(fds[1].fd >= 0); + + /* default to a 1:1 input map */ + 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] = 50; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; + } + for (;;) { /* run this loop at ~100Hz */ - poll(fds, 2, 10); + int result = poll(fds, 2, 10); + + ASSERT(result >= 0); /* * Gather R/C control inputs from supported sources. @@ -90,95 +95,212 @@ controls_main(void) * one control input source, they're going to fight each * other. Don't do that. */ - bool locked = false; + + 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; + + 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; /* - * Store RC channel count to detect switch to RC loss sooner - * than just by timeout - */ - unsigned rc_channels = system_state.rc_channels; - - if (fds[0].revents & POLLIN) - locked |= dsm_input(); - - if (fds[1].revents & POLLIN) - locked |= sbus_input(); - - /* - * If we don't have lock from one of the serial receivers, - * look for PPM. It shares an input with S.bus, so there's - * a possibility it will mis-parse an S.bus frame. - * * 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 an alternate - * receiver lock. + * disable the PPM decoder completely if we have S.bus signal. */ - if (!locked) - ppm_input(); + 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; - /* check for manual override status */ - if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { - /* force manual input override */ - system_state.mixer_manual_override = true; + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - } else { - /* override not engaged, use FMU */ - system_state.mixer_manual_override = false; + /* + * 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]; + + /* implement the deadzone */ + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; + } + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; + } + + /* constrain to min/max values */ + 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]; + + int16_t scaled = raw; + + /* adjust to zero-relative (-500..500) */ + scaled -= 1500; + + /* scale to fixed-point representation (-10000..10000) */ + scaled *= 20; + + ASSERT(scaled >= -15000); + ASSERT(scaled <= 15000); + + 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); + + 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. + */ + if (assigned_channels == 0) + rc_input_lost = true; + + /* + * 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 and tell FMU. + * have lost input. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { - - if (system_state.rc_channels > 0) { - /* - * If the RC signal status (lost / present) has - * just changed, request an update immediately. - */ - system_state.fmu_report_due = true; - } - - /* set the number of channels to zero - no inputs */ - system_state.rc_channels = 0; + rc_input_lost = true; } /* - * PWM output updates are performed in addition on each comm update. - * the updates here are required to ensure operation if FMU is not started - * or stopped responding. + * Handle losing RC input */ - mixer_tick(); - } -} + if (rc_input_lost) { -static void -ppm_input(void) -{ - /* - * Look for new PPM input. - */ - if (ppm_last_valid_decode != 0) { + /* Clear the RC input status flags, clear manual override control */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK | + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); - /* avoid racing with PPM updates */ - irqstate_t state = irqsave(); + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - /* PPM data exists, copy it */ - system_state.rc_channels = ppm_decoded_channels; - - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - system_state.rc_channel_data[i] = ppm_buffer[i]; + /* Mark the arrays as empty */ + r_raw_rc_count = 0; + r_rc_valid = 0; } - /* copy the timestamp and clear it */ - system_state.rc_channels_timestamp = ppm_last_valid_decode; - ppm_last_valid_decode = 0; + /* + * Check for manual override. + * + * The OVERRIDE_OK feature 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_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { - irqrestore(state); + bool override = false; + + /* + * Check mapped channel 5; if the value is 'high' then the pilot has + * requested override. + */ + if ((r_rc_valid & (1 << 4)) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) + override = true; + + /* + * Check for an explicit manual override request from the AP. + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)) + 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; + } + } - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; } } + +static bool +ppm_input(uint16_t *values, uint16_t *num_values) +{ + bool result = false; + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + + /* + * Look for recent PPM input. + */ + if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { + + /* PPM data exists, copy it */ + result = true; + *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; + } + + irqrestore(state); + + return result; +} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 560ef47d94..f0e8e0f322 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -43,17 +43,13 @@ #include #include -#include #include -#include - #include #define DEBUG #include "px4io.h" -#include "protocol.h" #define DSM_FRAME_SIZE 16 #define DSM_FRAME_CHANNELS 7 @@ -72,13 +68,13 @@ 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 void dsm_decode(hrt_abstime now); +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); + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); if (dsm_fd >= 0) { struct termios t; @@ -106,7 +102,7 @@ dsm_init(const char *device) } bool -dsm_input(void) +dsm_input(uint16_t *values, uint16_t *num_values) { ssize_t ret; hrt_abstime now; @@ -143,7 +139,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - goto out; + return false; last_rx_time = now; @@ -156,20 +152,14 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - goto out; + return false; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - dsm_decode(now); partial_frame_count = 0; - -out: - /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' - */ - return (now - last_frame_time) < 200000; + return dsm_decode(now, values, num_values); } static bool @@ -273,8 +263,8 @@ dsm_guess_format(bool reset) dsm_guess_format(true); } -static void -dsm_decode(hrt_abstime frame_time) +static bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { /* @@ -295,7 +285,7 @@ dsm_decode(hrt_abstime frame_time) /* if we don't know the frame format, update the guessing state machine */ if (channel_shift == 0) { dsm_guess_format(false); - return; + return false; } /* @@ -323,8 +313,8 @@ dsm_decode(hrt_abstime frame_time) continue; /* update the decoded channel count */ - if (channel >= system_state.rc_channels) - system_state.rc_channels = channel + 1; + 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) @@ -355,13 +345,11 @@ dsm_decode(hrt_abstime frame_time) break; } - system_state.rc_channel_data[channel] = value; + values[channel] = value; } - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - system_state.rc_channels_timestamp = frame_time; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + /* + * XXX Note that we may be in failsafe here; we need to work out how to detect that. + */ + return true; } diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 4e95dd5831..d08da2ff14 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -259,7 +259,6 @@ i2c_tx_complete(void) /* XXX handle transmit-done */ /* prepare for the next transaction */ - memset(tx_buf, 0, sizeof(tx_buf)); i2c_tx_setup(); } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed7d684b6f..7ccf915b00 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -38,22 +38,13 @@ */ #include -#include #include #include #include -#include -#include -#include -#include -#include - -#include #include #include -#include #include @@ -78,10 +69,12 @@ extern "C" { bool mixer_servos_armed = false; /* selected control values and count for mixing */ -static uint16_t *control_values; -static int control_count; - -static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; +enum mixer_source { + MIX_FMU, + MIX_OVERRIDE, + MIX_FAILSAFE +}; +static mixer_source source; static int mixer_callback(uintptr_t handle, uint8_t control_group, @@ -93,23 +86,32 @@ static MixerGroup mixer_group(mixer_callback, 0); void mixer_tick(void) { - bool should_arm; - /* check that we are receiving fresh data from the FMU */ if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ - system_state.mixer_manual_override = true; - system_state.mixer_fmu_available = false; - lib_lowprintf("RX timeout\n"); + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + debug("AP RX timeout"); } /* - * Decide which set of inputs we're using. + * Decide which set of controls we're using. */ - - /* this is for planes, where manual override makes sense */ - if (system_state.manual_override_ok) { + if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) { + /* this is for planes, where manual override makes sense */ + source = MIX_OVERRIDE; + } else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + source = MIX_FMU; + } else { + source = MIX_FAILSAFE; + } + +#if 0 /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { /* we have recent control data from the FMU */ control_count = PX4IO_CONTROL_CHANNELS; @@ -170,43 +172,40 @@ mixer_tick(void) control_count = 0; } } +#endif + /* + * Run the mixers. + */ + 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; /* - * Run the mixers if we have any control data at all. + * Update the servo outputs. */ - if (control_count > 0) { - 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 < IO_SERVO_COUNT; i++) { - if (i < mixed) { - /* scale to servo output */ - system_state.servos[i] = (outputs[i] * 500.0f) + 1500; - - } else { - /* set to zero to inhibit PWM pulse output */ - system_state.servos[i] = 0; - } - - /* - * If we are armed, update the servo output. - */ - if (system_state.armed && system_state.arm_ok) { - up_pwm_servo_set(i, system_state.servos[i]); - } - } - } + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servos[i]); /* * Decide whether the servos should be armed right now. - * A sufficient reason is armed state and either FMU or RC control inputs */ - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); + bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -226,21 +225,33 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - /* if the control index refers to an input that's not valid, we can't return it */ - if (control_index >= control_count) + if (control_group != 0) return -1; - /* scale from current PWM units (1000-2000) to mixer input values */ - if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) { - control = ((float)control_values[control_index] - 1000.0f) / 1000.0f; - } else { - control = ((float)control_values[control_index] - 1500.0f) / 500.0f; + 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: + /* XXX we could allow for configuration of per-output failsafe values */ + return -1; } return 0; } -static char mixer_text[256]; +static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; void @@ -267,6 +278,7 @@ mixer_handle_text(const void *buffer, size_t length) debug("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)) return; diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index aabc476dd7..4cb8a54ef3 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -55,7 +55,9 @@ * * 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. + * 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 @@ -66,6 +68,12 @@ #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 @@ -91,13 +99,15 @@ #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_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_AP_LOST (1 << 4) +#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) #define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ #define PX4IO_P_STATUS_TEMPERATURE 5 /* temperature in (units tbd) */ @@ -109,19 +119,26 @@ #define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of raw RC input values, microseconds */ -#define PX4IO_PAGE_RAW_RC_INPUT 4 /* 0..CONFIG_RC_INPUT_COUNT-1 */ +#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 /* 0..CONFIG_RC_INPUT_COUNT-1 */ +#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 */ /* setup page */ #define PX4IO_PAGE_SETUP 100 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK (1 << 0) /* OK to switch to manual override */ + #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 (1 << 1) /* request local override */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE (1 << 2) /* request switch to manual override */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ @@ -136,7 +153,17 @@ /* 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 */ /* * Old serial PX4FMU <-> PX4IO messaging protocol. diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index e51c1c73c1..13f46939a8 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -37,22 +37,22 @@ */ #include -#include + +#include // required for task_create #include -#include -#include -#include #include #include #include - -#include +#include #include #include +#include + #include +#define DEBUG #include "px4io.h" __EXPORT int user_start(int argc, char *argv[]); @@ -70,8 +70,6 @@ int user_start(int argc, char *argv[]) /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); - /* default to 50 Hz PWM outputs */ - system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); @@ -83,7 +81,7 @@ int user_start(int argc, char *argv[]) hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); /* print some startup info */ - lib_lowprintf("\nPX4IO: starting\n"); + debug("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); @@ -108,11 +106,19 @@ int user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); - /* start the i2c test code */ + /* start the i2c handler */ i2c_init(); - /* we're done here, go run the communications loop */ - comms_main(); + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* run the mixer at 100Hz (for now...) */ + for (;;) { + poll(NULL, 0, 10); + perf_begin(mixer_perf); + mixer_tick(); + perf_end(mixer_perf); + } } \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index fb06596023..2e2c50a3a8 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -75,6 +75,29 @@ 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 */ + +/* + * 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_lowrate r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] +#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] +#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] + +#define r_control_values (&r_page_controls[0]) /* * System state structure. @@ -84,9 +107,9 @@ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ */ struct sys_state_s { - bool armed; /* IO armed */ - bool arm_ok; /* FMU says OK to arm */ - uint16_t servo_rate; /* output rate of servos in Hz */ +// bool armed; /* IO armed */ +// bool arm_ok; /* FMU says OK to arm */ +// uint16_t servo_rate; /* output rate of servos in Hz */ /** * Remote control input(s) channel mappings @@ -105,39 +128,39 @@ struct sys_state_s { /** * Data from the remote control input(s) */ - unsigned rc_channels; - uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; +// unsigned rc_channels; +// uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; /** * Control signals from FMU. */ - uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; +// uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; /** * Mixed servo outputs */ - uint16_t servos[IO_SERVO_COUNT]; +// uint16_t servos[IO_SERVO_COUNT]; /** * Relay controls */ - bool relays[PX4IO_RELAY_CHANNELS]; +// bool relays[PX4IO_RELAY_CHANNELS]; /** * If true, we are using the FMU controls, else RC input if available. */ - bool mixer_manual_override; +// bool mixer_manual_override; /** * If true, FMU input is available. */ - bool mixer_fmu_available; +// bool mixer_fmu_available; /** * If true, state that should be reported to FMU has been updated. */ - bool fmu_report_due; +// bool fmu_report_due; /** * Last FMU receive time, in microseconds since system boot @@ -147,12 +170,12 @@ struct sys_state_s { /** * If true, the RC signal has been lost for more than a timeout interval */ - bool rc_lost; +// bool rc_lost; /** * If true, the connection to FMU has been lost for more than a timeout interval */ - bool fmu_lost; +// bool fmu_lost; /** * If true, FMU is ready for autonomous position control. Only used for LED indication @@ -162,41 +185,27 @@ struct sys_state_s { /** * If true, IO performs an on-board manual override with the RC channel values */ - bool manual_override_ok; +// bool manual_override_ok; /* * Measured battery voltage in mV */ - uint16_t battery_mv; +// uint16_t battery_mv; /* * ADC IN5 measurement */ - uint16_t adc_in5; +// uint16_t adc_in5; /* * Power supply overcurrent status bits. */ - uint8_t overcurrent; +// uint8_t overcurrent; }; extern struct sys_state_s system_state; -#if 0 -/* - * Software countdown timers. - * - * Each timer counts down to zero at one tick per ms. - */ -#define TIMER_BLINK_AMBER 0 -#define TIMER_BLINK_BLUE 1 -#define TIMER_STATUS_PRINT 2 -#define TIMER_SANITY 7 -#define TIMER_NUM_TIMERS 8 -extern volatile int timers[TIMER_NUM_TIMERS]; -#endif - /* * GPIO handling. */ @@ -249,9 +258,11 @@ 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_main(void); extern int dsm_init(const char *device); -extern bool dsm_input(void); +extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern int sbus_init(const char *device); -extern bool sbus_input(void); +extern bool sbus_input(uint16_t *values, uint16_t *num_values); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 1397dd2a44..59684f1ee8 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -37,6 +37,11 @@ * Implementation of the PX4IO register space. */ +#include + +#include +#include + #include "px4io.h" #include "protocol.h" @@ -45,8 +50,9 @@ static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); /** * Setup registers */ -uint16_t r_page_setup[] = +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_LOWRATE] = 50, @@ -54,14 +60,16 @@ uint16_t r_page_setup[] = [PX4IO_P_SETUP_RELAYS] = 0, }; -#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE) +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \ + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE) #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) -#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PXIO_RELAY_CHANNELS) - 1) +#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** * Control values from the FMU. */ -uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; +volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; /** * Static configuration parameters. @@ -108,13 +116,26 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; /** * Scaled/routed RC input */ -uint16_t r_page_rc_input[MAX_CONTROL_CHANNELS]; +uint16_t r_page_rc_input[] = { + [PX4IO_P_RC_VALID] = 0, + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 +}; /** * Raw RC input */ -uint16_t r_page_raw_rc_input[MAX_CONTROL_CHANNELS]; +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 +}; +/** + * R/C channel input configuration. + */ +uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; + +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -136,7 +157,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } /* XXX we should cause a mixer tick ASAP */ - system_state.mixer_fmu_available = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; break; /* handle text going to the mixer parser */ @@ -168,7 +189,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) switch (offset) { case PX4IO_P_STATUS_ALARMS: /* clear bits being written */ - r_page_status[PX4IO_P_STATUS_ALARMS] &= ~value; + r_status_alarms &= ~value; break; default: @@ -179,20 +200,30 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_SETUP: switch (offset) { + case PX4IO_P_SETUP_FEATURES: + + value &= PX4IO_P_SETUP_FEATURES_VALID; + r_setup_features = value; + + /* update manual override state - disable if no longer OK */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && !(value & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)) + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + + break; + case PX4IO_P_SETUP_ARMING: value &= PX4IO_P_SETUP_ARMING_VALID; - r_page_setup[PX4IO_P_SETUP_ARMING] = value; + r_setup_arming = value; /* update arming state - disarm if no longer OK */ - if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) - system_state.armed = false; - + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; break; case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; - r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value; + r_setup_pwm_rates = value; /* XXX re-configure timers */ break; @@ -201,7 +232,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value = 50; if (value > 400) value = 400; - r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value; + r_setup_pwm_lowrate = value; /* XXX re-configure timers */ break; @@ -210,13 +241,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value = 50; if (value > 400) value = 400; - r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value; + r_setup_pwm_highrate = value; /* XXX re-configure timers */ break; case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; - r_page_setup[PX4IO_P_SETUP_RELAYS] = value; + 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); @@ -228,6 +259,63 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) } break; + case PX4IO_PAGE_RC_CONFIG: { + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset % PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[offset * 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; + + /* 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) { + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) + break; + if (conf[PX4IO_P_RC_CONFIG_MAX] < 2500) + break; + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) + break; + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) + break; + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) + break; + if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) + break; + if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) + break; + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) + break; + + /* sanity checks pass, enable channel */ + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + break; + + } + } + default: return -1; } @@ -237,27 +325,27 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) { +#define SELECT_PAGE(_page_name) { *values = _page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); } switch (page) { - case PX4IO_PAGE_CONFIG: - *values = r_page_config; - *num_values = sizeof(r_page_config) / sizeof(r_page_config[0]); - break; + /* + * 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 */ - r_page_status[PX4IO_P_STATUS_FLAGS] = - (system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) | - (system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) | - ((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0)) - /* XXX specific receiver status */ - /* XXX PX4IO_P_STATUS_ALARMS] */ + /* 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 @@ -285,43 +373,42 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val unsigned counts = adc_measure(ADC_VBATT); r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10; } + /* XXX PX4IO_P_STATUS_TEMPERATURE */ - *values = r_page_status; - *num_values = sizeof(r_page_status) / sizeof(r_page_status[0]); + SELECT_PAGE(r_page_status); break; - case PX4IO_PAGE_ACTUATORS: - *values = r_page_actuators; - *num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]); - break; - - case PX4IO_PAGE_SERVOS: - *values = system_state.servos; - *num_values = IO_SERVO_COUNT; - break; - - case PX4IO_PAGE_RAW_RC_INPUT: - *values = r_page_raw_rc_input; - *num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]); - break; - - case PX4IO_PAGE_RC_INPUT: - *values = system_state.rc_channel_data; - *num_values = system_state.rc_channels; - return -1; - case PX4IO_PAGE_RAW_ADC_INPUT: r_page_adc[0] = adc_measure(ADC_VBATT); r_page_adc[1] = adc_measure(ADC_IN5); - *values = r_page_adc; - *num_values = ADC_CHANNEL_COUNT; + + SELECT_PAGE(r_page_adc); break; + /* + * Pages that are just a straight read of the register state. + */ +#define COPY_PAGE(_page_name, _page) case _page_name: SELECT_PAGE(_page); break; + + /* status pages */ + COPY_PAGE(PX4IO_PAGE_CONFIG, r_page_config); + COPY_PAGE(PX4IO_PAGE_ACTUATORS, r_page_actuators); + COPY_PAGE(PX4IO_PAGE_SERVOS, r_page_servos); + COPY_PAGE(PX4IO_PAGE_RAW_RC_INPUT, r_page_raw_rc_input); + COPY_PAGE(PX4IO_PAGE_RC_INPUT, r_page_rc_input); + + /* readback of input pages */ + COPY_PAGE(PX4IO_PAGE_SETUP, r_page_setup); + COPY_PAGE(PX4IO_PAGE_CONTROLS, r_page_controls); + COPY_PAGE(PX4IO_PAGE_RC_CONFIG, r_page_rc_input_config); + default: return -1; } +#undef SELECT_PAGE + /* if the offset is beyond the end of the page, we have no data */ if (*num_values <= offset) return -1; diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 0cae29ac98..a14051f76d 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -36,15 +36,8 @@ */ #include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include @@ -116,30 +109,28 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !system_state.armed) { + 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 armed state and notify the FMU */ - system_state.armed = true; + /* switch to armed state */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; counter++; - system_state.fmu_report_due = true; } /* Disarm quickly */ - } else if (safety_button_pressed && system_state.armed) { + } 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 */ - system_state.armed = false; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; counter++; - system_state.fmu_report_due = true; } } else { @@ -149,15 +140,15 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_SAFE; - if (system_state.armed) { - if (system_state.arm_ok) { + 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 (system_state.arm_ok) { + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { pattern = LED_PATTERN_FMU_ARMED; } else if (system_state.vector_flight_mode_ok) { @@ -188,7 +179,7 @@ failsafe_blink(void *arg) static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_fmu_available) { + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; } else { diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 568ef8091c..d199a9361c 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -66,13 +66,13 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +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); + sbus_fd = open(device, O_RDONLY | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(void) +sbus_input(uint16_t *values, uint16_t *num_values) { ssize_t ret; hrt_abstime now; @@ -134,7 +134,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - goto out; + return false; last_rx_time = now; @@ -147,20 +147,14 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + return false; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - sbus_decode(now); partial_frame_count = 0; - -out: - /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' - */ - return (now - last_frame_time) < 200000; + return sbus_decode(now, values, num_values); } /* @@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +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; + return false; } /* if the failsafe or connection lost bit is set, we consider the frame invalid */ @@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time) (frame[23] & (1 << 3))) { /* failsafe */ /* actively announce signal loss */ - system_state.rc_channels = 0; - return 1; + *values = 0; + return false; } /* we have received something we think is a frame */ @@ -241,23 +235,19 @@ sbus_decode(hrt_abstime frame_time) } /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - system_state.rc_channel_data[channel] = (value / 2) + 998; + values[channel] = (value / 2) + 998; } /* decode switch channels if data fields are wide enough */ if (chancount > 17) { /* channel 17 (index 16) */ - system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[23] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ - system_state.rc_channels = chancount; + *num_values = chancount; - /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return true; } From 6e291ddedc8b2d7bfeae8029a37df0b581262796 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 00:18:05 -0800 Subject: [PATCH 025/167] Add a mechanism for sending multi-part messages to the I2C driver base class. --- apps/drivers/device/i2c.cpp | 20 ++++++++++++++++++++ apps/drivers/device/i2c.h | 10 ++++++++++ 2 files changed, 30 insertions(+) diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 474190d839..c513ae2b67 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -166,4 +166,24 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } +int +I2C::transfer(i2c_msg_s *msgv, unsigned msgs) +{ + for (unsigned i = 0; i < msgs; i++) + msgv[i].addr = _address; + + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(_dev, _frequency); + ret = I2C_TRANSFER(_dev, msgv, msgs); + + if (ret != OK) + up_i2creset(_dev); + + return ret; +} + } // namespace device \ No newline at end of file diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 4d630b8a81..66c34dd7c4 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -100,6 +100,16 @@ protected: int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + /** + * Perform a multi-part I2C transaction to the device. + * + * @param msgv An I2C message vector. + * @param msgs The number of entries in the message vector. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(i2c_msg_s *msgv, unsigned msgs); + /** * Change the bus address. * From 2311e03379f717472a0c7cc0d990e08407d0cb5e Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 00:19:01 -0800 Subject: [PATCH 026/167] Start reworking the px4io driver to use the I2C interface instead. --- apps/drivers/px4io/px4io.cpp | 423 +++++++++++++++++++---------------- apps/px4io/protocol.h | 64 +----- 2 files changed, 236 insertions(+), 251 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2c2c236cae..301e6cc8f8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -85,7 +85,7 @@ #include "uploader.h" -class PX4IO : public device::CDev +class PX4IO : public device::I2C { public: PX4IO(); @@ -108,32 +108,23 @@ private: static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; unsigned _update_rate; ///< serial send rate in Hz - int _serial_fd; ///< serial interface to PX4IO - hx_stream_t _io_stream; ///< HX protocol stream - volatile int _task; ///< worker task volatile bool _task_should_exit; volatile bool _connected; ///< true once we have received a valid frame + /* subscribed topics */ int _t_actuators; ///< actuator output topic - actuator_controls_s _controls; ///< actuator outputs - - orb_advert_t _t_actuators_effective; ///< effective actuator controls topic - actuator_controls_effective_s _controls_effective; ///< effective controls - int _t_armed; ///< system armed control topic - actuator_armed_s _armed; ///< system armed state int _t_vstatus; ///< system / vehicle status - vehicle_status_s _vstatus; ///< overall system state + /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io - rc_input_values _input_rc; ///< rc input values - + orb_advert_t _to_actuators_effective; ///< effective actuator controls topic + orb_advert_t _to_outputs; ///< mixed servo outputs topic 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 + actuator_controls_effective_s _controls_effective; ///< effective controls const char *volatile _mix_buf; ///< mixer text buffer volatile unsigned _mix_buf_len; ///< size of the mixer text buffer @@ -142,12 +133,6 @@ private: uint32_t _relays; ///< state of the PX4IO relays, one bit per relay - volatile bool _switch_armed; ///< PX4IO switch armed state - // XXX how should this work? - - bool _send_needed; ///< If true, we need to send a packet to IO - bool _config_needed; ///< if true, we need to set a config update to IO - /** * Trampoline to the worker task */ @@ -159,43 +144,30 @@ private: void task_main(); /** - * Handle receiving bytes from PX4IO + * Send controls to IO */ - void io_recv(); + int io_set_control_state(); /** - * HX protocol callback trampoline. + * Update IO's arming-related state */ - static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); + int io_set_arming_state(); /** - * Callback invoked when we receive a whole packet from PX4IO + * write register(s) */ - void rx_callback(const uint8_t *buffer, size_t bytes_received); + int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); /** - * Send an update packet to PX4IO + * read register(s) */ - void io_send(); + int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); /** - * Send a config packet to PX4IO + * modify s register */ - void config_send(); + int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); - /** - * Send a buffer containing mixer text to PX4IO - */ - int mixer_send(const char *buf, unsigned buflen); - - /** - * Mixer control callback; invoked to fetch a control from a specific - * group/index during mixing. - */ - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); }; @@ -207,19 +179,19 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - CDev("px4io", "/dev/px4io"), + CDev("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), dump_one(false), _update_rate(50), - _serial_fd(-1), - _io_stream(nullptr), _task(-1), _task_should_exit(false), _connected(false), _t_actuators(-1), - _t_actuators_effective(-1), _t_armed(-1), _t_vstatus(-1), - _t_outputs(-1), + _to_input_rc(0), + _to_actuators_effective(0), + _to_outputs(0), + _to_battery(0), _mix_buf(nullptr), _mix_buf_len(0), _primary_pwm_device(false), @@ -260,8 +232,7 @@ PX4IO::init() ASSERT(_task == -1); /* do regular cdev init */ - ret = CDev::init(); - + ret = I2C::init(); if (ret != OK) return ret; @@ -320,46 +291,19 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { + hrt_abstime_t last_poll_time = 0; + unsigned poll_phase = 0; + log("starting"); unsigned update_rate_in_ms; - /* open the serial port */ - _serial_fd = ::open("/dev/ttyS2", O_RDWR); - - if (_serial_fd < 0) { - log("failed to open serial port: %d", errno); - goto out; - } - - /* 115200bps, no parity, one stop bit */ - { - struct termios t; - - tcgetattr(_serial_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(_serial_fd, TCSANOW, &t); - } - - /* protocol stream */ - _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); - - if (_io_stream == nullptr) { - log("failed to allocate HX protocol stream"); - goto out; - } - - hx_stream_set_counters(_io_stream, - perf_alloc(PC_COUNT, "PX4IO frames transmitted"), - perf_alloc(PC_COUNT, "PX4IO frames received"), - perf_alloc(PC_COUNT, "PX4IO receive errors")); - /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. */ _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); + /* convert the update rate in hz to milliseconds, rounding down if necessary */ update_rate_in_ms = 1000 / _update_rate; orb_set_interval(_t_actuators, update_rate_in_ms); @@ -389,27 +333,26 @@ PX4IO::task_main() _to_battery = -1; /* poll descriptor */ - pollfd fds[4]; - fds[0].fd = _serial_fd; + pollfd fds[3]; + fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuators; + fds[1].fd = _t_armed; fds[1].events = POLLIN; - fds[2].fd = _t_armed; + fds[2].fd = _t_vstatus; fds[2].events = POLLIN; - fds[3].fd = _t_vstatus; - fds[3].events = POLLIN; debug("ready"); /* lock against the ioctl handler */ lock(); - /* loop handling received serial bytes */ + /* loop talking to IO */ while (!_task_should_exit) { - /* sleep waiting for data, but no more than 100ms */ + /* sleep waiting for topic updates, but no more than 20ms */ + /* XXX should actually be calculated to keep the poller running tidily */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); + int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); lock(); /* this would be bad... */ @@ -418,63 +361,37 @@ PX4IO::task_main() continue; } - /* if we timed out waiting, we should send an update */ - if (ret == 0) - _send_needed = true; + /* if we have new control data from the ORB, handle it */ + if (fds[0].revents & POLLIN) + io_set_control_state(); - if (ret > 0) { - /* if we have new data from IO, go handle it */ - if (fds[0].revents & POLLIN) - io_recv(); + /* if we have an arming state update, handle it */ + if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) + io_set_arming_state(); - /* if we have new control data from the ORB, handle it */ - if (fds[1].revents & POLLIN) { + hrt_abstime_t now = hrt_absolute_time(); - /* get controls */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); + /* + * If this isn't time for the next tick of the polling state machine, + * go back to sleep. + */ + if ((now - last_poll_time) < 20000) + continue; - /* scale controls to PWM (temporary measure) */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _controls.control[i]); + /* + * Pull status and alarms from IO + */ + io_get_status(); - /* and flag for update */ - _send_needed = true; - } - - /* if we have an arming state update, handle it */ - if (fds[2].revents & POLLIN) { - - orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); - _send_needed = true; - } - - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus); - _send_needed = true; - } + switch (poll_phase) { + case 0: + /* XXX fetch raw RC values */ + break; + case 1: + /* XXX fetch servo outputs */ + break; } - /* send a config packet to IO if required */ - if (_config_needed) { - _config_needed = false; - config_send(); - } - - /* send a mixer update if needed */ - if (_mix_buf != nullptr) { - mixer_send(_mix_buf, _mix_buf_len); - - /* clear the buffer record so the ioctl handler knows we're done */ - _mix_buf = nullptr; - _mix_buf_len = 0; - } - - /* send an update to IO if required */ - if (_send_needed) { - _send_needed = false; - io_send(); - } } unlock(); @@ -482,12 +399,6 @@ PX4IO::task_main() out: debug("exiting"); - /* kill the HX stream */ - if (_io_stream != nullptr) - hx_stream_free(_io_stream); - - ::close(_serial_fd); - /* clean up the alternate device node */ if (_primary_pwm_device) unregister_driver(PWM_OUTPUT_DEVICE_PATH); @@ -498,17 +409,183 @@ out: } int -PX4IO::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +PX4IO::io_set_control_state() { - const actuator_controls_s *controls = (actuator_controls_s *)handle; + actuator_controls_s controls; ///< actuator outputs + uint16_t regs[_max_actuators]; - input = controls->control[control_index]; - return 0; + /* get controls */ + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); + + for (unsigned i = 0; i < _max_actuators; i++) + regs[i] = FLOAT_TO_REG(_controls.control[i]); + + /* copy values to registers in IO */ + io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators); } +int +PX4IO::io_set_arming_state() +{ + actuator_armed_s armed; ///< system armed state + vehicle_status_s vstatus; ///< overall system state + + orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + + uint16_t set = 0; + uint16_t clear = 0; + + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_ARM_OK; + } + if (vstatus.flag_vector_flight_mode_ok) { + set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + } + if (vstatus.flag_external_manual_override_ok) { + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE; + } else { + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE; + } + + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); +} + +int +PX4IO::io_get_status() +{ + struct { + uint16_t status; + uint16_t alarms; + uint16_t vbatt; + } state; + int ret; + bool rc_valid = false; + + /* get STATUS_FLAGS, STATUS_ALARMS and STATUS_VBATT in that order */ + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, state, 3); + + /* XXX handle status */ + + /* XXX handle alarms */ + + /* only publish if battery has a valid minimum voltage */ + if (state.vbatt > 3300) { + battery_status_s battery_status; + + battery_status.timestamp = hrt_absolute_time(); + battery_status.voltage_v = state.vbatt / 1000.0f; + + /* current and discharge are currently (ha) unknown */ + battery_status.current_a = -1.0f; + battery_status.discharged_mah = -1.0f; + + /* lazily publish the battery voltage */ + 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); + } + + /* + * If we have RC input, get it + */ + if (state.status & PX4IO_P_STATUS_FLAGS_RC_OK) { + rc_input_values input_rc; + + input_rc.timestamp = hrt_absolute_time(); + + if (state.status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + input_rc.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (state.status & RC_INPUT_SOURCE_PX4IO_DSM) { + input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (state.status & RC_INPUT_SOURCE_PX4IO_SBUS) { + input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { + input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + uint16_t channel_count; + io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1); + input_rc.channel_count = channel_count; + + if (channel_count > 0) + io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, channel_count); + + if (_to_input_rc > 0) { + orb_publish(ORB_ID(input_rc), _to_input_rc, &input_rc); + } else { + _to_input_rc = orb_advertise(ORB_ID(input_rc), &input_rc); + } + } + +} + +int +PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + i2c_msg_s msgv[2]; + t8_t hdr[2]; + + hdr[0] = page; + + hdr[1] = offset; + actuator_controls_effective_s _controls_effective; ///< effective controls + + mgsv[0].flags = 0; + msgv[0].buffer = hdr; + msgv[0].length = sizeof(hdr); + + msgv[1].flags = 0; + msgv[1].buffer = const_cast(values); + msgv[1].length = num_values * sizeof(*values); + + return transfer(msgv, 2); +} + +int +PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + i2c_msg_s msgv[2]; + uint8_t hdr[2]; + + hdr[0] = page; + hdr[1] = offset; + + mgsv[0].flags = 0; + msgv[0].buffer = hdr; + msgv[0].length = sizeof(hdr); + + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = values; + msgv[1].length = num_values * sizeof(*values); + + return transfer(msgv, 2); +} + +int +PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) +{ + int ret; + uint16_t value; + + ret = io_reg_get(page, offset, &value, 1); + if (ret) + return ret; + value &= ~clearbits; + value |= setbits; + + return io_reg_set(page, offset, &value, 1); +} + + + + void PX4IO::io_recv() { @@ -604,42 +681,7 @@ out: return; } -void -PX4IO::io_send() -{ - px4io_command cmd; - int ret; - - cmd.f2i_magic = F2I_MAGIC; - - /* set outputs */ - for (unsigned i = 0; i < _max_actuators; i++) { - cmd.output_control[i] = _outputs.output[i]; - } - /* publish as we send */ - _outputs.timestamp = hrt_absolute_time(); - /* XXX needs to be based off post-mix values from the IO side */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); - - /* update relays */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) - cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; - - /* armed and not locked down -> arming ok */ - cmd.arm_ok = (_armed.armed && !_armed.lockdown); - /* indicate that full autonomous position control / vector flight mode is available */ - cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok; - /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */ - cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; - /* set desired PWM output rate */ - cmd.servo_rate = _update_rate; - - ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); - - if (ret) - debug("send error %d", ret); -} - +#if 0 void PX4IO::config_send() { @@ -729,6 +771,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) return 0; } +#endif int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 4cb8a54ef3..682348a600 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -139,6 +139,7 @@ #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 (1 << 2) /* request switch to manual override */ +#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ @@ -165,65 +166,6 @@ #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ -/* - * Old serial PX4FMU <-> PX4IO messaging protocol. - * - * This initial version of the protocol is very simple; each side transmits a - * complete update with each frame. This avoids the sending of many small - * messages and the corresponding complexity involved. - */ - - -#define PX4IO_RELAY_CHANNELS 4 - -#pragma pack(push, 1) - -/** - * Periodic command from FMU to IO. - */ -struct px4io_command { - uint16_t f2i_magic; -#define F2I_MAGIC 0x636d - - uint16_t servo_rate; - uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */ - bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ - bool arm_ok; /**< FMU allows full arming */ - bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ - bool manual_override_ok; /**< if true, IO performs a direct manual override */ -}; - -/** - * Periodic report from IO to FMU - */ -struct px4io_report { - uint16_t i2f_magic; -#define I2F_MAGIC 0x7570 - - uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; - bool armed; - uint8_t channel_count; - - uint16_t battery_mv; - uint16_t adc_in; - uint8_t overcurrent; -}; - -/** - * As-needed config message from FMU to IO - */ -struct px4io_config { - uint16_t f2i_config_magic; -#define F2I_CONFIG_MAGIC 0x6366 - - uint8_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */ - uint16_t rc_min[4]; /**< min value for each channel */ - uint16_t rc_trim[4]; /**< trim value for each channel */ - uint16_t rc_max[4]; /**< max value for each channel */ - int8_t rc_rev[4]; /**< rev value for each channel */ - uint16_t rc_dz[4]; /**< dz value for each channel */ -}; - /** * As-needed mixer data upload. * @@ -241,7 +183,7 @@ struct px4io_mixdata { char text[0]; /* actual text size may vary */ }; -/* maximum size is limited by the HX frame size */ -#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata)) +/* maximum size is limited by the link frame size XXX use config values to set this */ +#define F2I_MIXER_MAX_TEXT (64 - sizeof(struct px4io_mixdata)) #pragma pack(pop) \ No newline at end of file From 2dc47160f4ac3b1dbd2e81c31237459b50497ca3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 00:30:18 -0800 Subject: [PATCH 027/167] Factoring and comments. --- apps/drivers/px4io/px4io.cpp | 56 +++++++++++++++++++++++++++++------- 1 file changed, 45 insertions(+), 11 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 301e6cc8f8..ad372081ad 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -153,6 +153,16 @@ private: */ int io_set_arming_state(); + /** + * Fetch status and alarms from IO + */ + int io_get_status(); + + /** + * Fetch RC inputs from IO + */ + int io_get_rc_input(rc_input_values &input_rc); + /** * write register(s) */ @@ -498,8 +508,8 @@ PX4IO::io_get_status() if (state.status & PX4IO_P_STATUS_FLAGS_RC_OK) { rc_input_values input_rc; - input_rc.timestamp = hrt_absolute_time(); - + io_get_rc_input(input_rc); + if (state.status & PX4IO_P_STATUS_FLAGS_RC_PPM) { input_rc.input_source = RC_INPUT_SOURCE_PX4IO_PPM; } else if (state.status & RC_INPUT_SOURCE_PX4IO_DSM) { @@ -510,13 +520,6 @@ PX4IO::io_get_status() input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; } - uint16_t channel_count; - io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1); - input_rc.channel_count = channel_count; - - if (channel_count > 0) - io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, channel_count); - if (_to_input_rc > 0) { orb_publish(ORB_ID(input_rc), _to_input_rc, &input_rc); } else { @@ -526,6 +529,39 @@ PX4IO::io_get_status() } +int +PX4IO::io_get_rc_input(rc_input_values &input_rc) +{ + uint16_t channel_count; + int ret; + + input_rc.timestamp = hrt_absolute_time(); + + /* we don't have the status bits, so input_source has to be set elsewhere */ + input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* + * XXX Because the channel count and channel data are fetched + * separately, there is a risk of a race between the two + * that could leave us with channel data and a count that + * are out of sync. + * Fixing this would require a guarantee of atomicity from + * IO, and a single fetch for both count and channels. + * + * XXX Since IO has the input calibration info, we ought to be + * able to get the pre-fixed-up controls directly. + */ + ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1); + if (ret) + return ret; + input_rc.channel_count = channel_count; + + if (channel_count > 0) + ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, channel_count); + + return ret; +} + int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -533,9 +569,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned t8_t hdr[2]; hdr[0] = page; - hdr[1] = offset; - actuator_controls_effective_s _controls_effective; ///< effective controls mgsv[0].flags = 0; msgv[0].buffer = hdr; From 06b66ad065f096060bfdd2e1f18cdc6704c70d2c Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 01:09:42 -0800 Subject: [PATCH 028/167] Don't advertise things we don't have anymore. --- apps/drivers/px4io/px4io.cpp | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index ad372081ad..968f433b2d 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -324,24 +324,6 @@ PX4IO::task_main() _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ - /* advertise the limited control inputs */ - memset(&_controls_effective, 0, sizeof(_controls_effective)); - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), - &_controls_effective); - - /* advertise the mixed control outputs */ - memset(&_outputs, 0, sizeof(_outputs)); - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &_outputs); - - /* advertise the rc inputs */ - 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(&_battery_status, 0, sizeof(_battery_status)); - _to_battery = -1; - /* poll descriptor */ pollfd fds[3]; fds[0].fd = _t_actuators; @@ -402,6 +384,20 @@ PX4IO::task_main() break; } +#if 0 + /* advertise the limited control inputs */ + memset(&_controls_effective, 0, sizeof(_controls_effective)); + _to_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), + &_controls_effective); + + /* advertise the mixed control outputs */ + memset(&_outputs, 0, sizeof(_outputs)); + _to_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &_outputs); +#endif + + + } unlock(); From 5c60ed9a9457e3ab0c51584e7e0db59bdbe4fd87 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 01:11:29 -0800 Subject: [PATCH 029/167] Fix up FMU input timeout handling. Fix the FMU auto OK LED status. Strip out unused fields from the system state structure. --- apps/px4io/mixer.cpp | 4 +- apps/px4io/protocol.h | 2 - apps/px4io/px4io.h | 91 ------------------------------------------ apps/px4io/registers.c | 5 +++ apps/px4io/safety.c | 2 +- 5 files changed, 8 insertions(+), 96 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 7ccf915b00..f394233f42 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -66,7 +66,7 @@ extern "C" { #define OVERRIDE 4 /* current servo arm/disarm state */ -bool mixer_servos_armed = false; +static bool mixer_servos_armed = false; /* selected control values and count for mixing */ enum mixer_source { @@ -89,7 +89,7 @@ mixer_tick(void) /* check that we are receiving fresh data from the FMU */ if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { - /* too many frames without FMU input, time to go to failsafe */ + /* too long without FMU input, time to go to failsafe */ r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 682348a600..501691cd26 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -185,5 +185,3 @@ struct px4io_mixdata { /* maximum size is limited by the link frame size XXX use config values to set this */ #define F2I_MIXER_MAX_TEXT (64 - sizeof(struct px4io_mixdata)) - -#pragma pack(pop) \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 2e2c50a3a8..d8cdafb4b5 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -101,107 +101,16 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ /* * System state structure. - * - * XXX note that many fields here are deprecated and replaced by - * registers. */ struct sys_state_s { -// bool armed; /* IO armed */ -// bool arm_ok; /* FMU says OK to arm */ -// uint16_t servo_rate; /* output rate of servos in Hz */ - - /** - * Remote control input(s) channel mappings - */ - uint8_t rc_map[4]; - - /** - * Remote control channel attributes - */ - uint16_t rc_min[4]; - uint16_t rc_trim[4]; - uint16_t rc_max[4]; - int16_t rc_rev[4]; - uint16_t rc_dz[4]; - - /** - * Data from the remote control input(s) - */ -// unsigned rc_channels; -// uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; - /** - * Control signals from FMU. - */ -// uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; - - /** - * Mixed servo outputs - */ -// uint16_t servos[IO_SERVO_COUNT]; - - /** - * Relay controls - */ -// bool relays[PX4IO_RELAY_CHANNELS]; - - /** - * If true, we are using the FMU controls, else RC input if available. - */ -// bool mixer_manual_override; - - /** - * If true, FMU input is available. - */ -// bool mixer_fmu_available; - - /** - * If true, state that should be reported to FMU has been updated. - */ -// bool fmu_report_due; - /** * Last FMU receive time, in microseconds since system boot */ uint64_t fmu_data_received_time; - /** - * If true, the RC signal has been lost for more than a timeout interval - */ -// bool rc_lost; - - /** - * If true, the connection to FMU has been lost for more than a timeout interval - */ -// bool fmu_lost; - - /** - * If true, FMU is ready for autonomous position control. Only used for LED indication - */ - bool vector_flight_mode_ok; - - /** - * If true, IO performs an on-board manual override with the RC channel values - */ -// bool manual_override_ok; - - /* - * Measured battery voltage in mV - */ -// 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; diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 59684f1ee8..0dd8fe28d6 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -42,6 +42,8 @@ #include #include +#include + #include "px4io.h" #include "protocol.h" @@ -140,6 +142,8 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { + system_state.fmu_data_received_time = hrt_absolute_time(); + switch (page) { /* handle bulk controls input */ @@ -157,6 +161,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } /* XXX we should cause a mixer tick ASAP */ + system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; break; diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index a14051f76d..540636e19d 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -151,7 +151,7 @@ safety_check_button(void *arg) } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { pattern = LED_PATTERN_FMU_ARMED; - } else if (system_state.vector_flight_mode_ok) { + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) { pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } From 2686344d585c2f4c4034357fcf3bf9be7b7b92e7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:40:15 -0800 Subject: [PATCH 030/167] Adjust the default deadzone for RC inputs per feedback. --- apps/px4io/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 072d296dab..2af62a369f 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -78,7 +78,7 @@ controls_main(void) 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] = 50; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; } From f3a587dfced54bfdfe3471e6099c3ea16bc33a31 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:40:41 -0800 Subject: [PATCH 031/167] Wire the I2C device code into the register handler. --- apps/px4io/i2c.c | 47 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 36 insertions(+), 11 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index d08da2ff14..cf69211791 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -77,16 +77,22 @@ static void i2c_tx_complete(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; -uint8_t rx_buf[64]; -unsigned rx_len; -uint8_t tx_buf[64]; +static uint8_t rx_buf[64]; +static unsigned rx_len; + +static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff }; + +static uint8_t *tx_buf = junk_buf; +static unsigned tx_len = sizeof(junk_buf); + +static uint8_t selected_page; +static uint8_t selected_offset; enum { DIR_NONE = 0, DIR_TX = 1, DIR_RX = 2 -}; -unsigned direction; +} direction; void i2c_init(void) @@ -223,10 +229,28 @@ i2c_rx_complete(void) rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); stm32_dmastop(rx_dma); - /* XXX handle reception */ - for (unsigned i = 0; i < rx_len; i++) { - tx_buf[i] = rx_buf[i] + 1; - rx_buf[i] = 0; + 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; + + 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); + } + } } /* prepare for the next transaction */ @@ -242,7 +266,7 @@ i2c_tx_setup(void) * 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], sizeof(tx_buf), + stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, DMA_CCR_DIR | DMA_CCR_CIRC | DMA_CCR_MINC | @@ -256,7 +280,8 @@ i2c_tx_complete(void) { stm32_dmastop(tx_dma); - /* XXX handle transmit-done */ + tx_buf = junk_buf; + tx_len = sizeof(junk_buf); /* prepare for the next transaction */ i2c_tx_setup(); From 112f5ea9697a2ada9e3852f9c2e7c10ab0e78a8a Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:41:13 -0800 Subject: [PATCH 032/167] Add support for raw PWM passthrough from FMU via IO. --- apps/px4io/mixer.cpp | 68 +++++++++++++++++++++++++++--------------- apps/px4io/protocol.h | 6 +++- apps/px4io/px4io.c | 1 + apps/px4io/registers.c | 22 +++++++++++++- 4 files changed, 71 insertions(+), 26 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index f394233f42..178b0bb431 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -81,6 +81,7 @@ static int mixer_callback(uintptr_t handle, uint8_t control_index, float &control); +static void mix(); static MixerGroup mixer_group(mixer_callback, 0); void @@ -91,7 +92,7 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; debug("AP RX timeout"); } @@ -100,13 +101,26 @@ mixer_tick(void) * Decide which set of controls we're using. */ if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { /* this is for planes, where manual override makes sense */ source = MIX_OVERRIDE; + + /* mix from the override controls */ + mix(); + } else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - source = MIX_FMU; + + if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) { + /* FMU has already provided PWM values */ + } else { + /* mix from FMU controls */ + source = MIX_FMU; + mix(); + } } else { source = MIX_FAILSAFE; + /* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */ } #if 0 @@ -173,27 +187,6 @@ mixer_tick(void) } } #endif - /* - * Run the mixers. - */ - 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; /* * Update the servo outputs. @@ -219,6 +212,33 @@ mixer_tick(void) } } +static void +mix() +{ + /* + * Run the mixers. + */ + 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; + +} + static int mixer_callback(uintptr_t handle, uint8_t control_group, diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 501691cd26..a2be4bfc4c 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -100,6 +100,7 @@ #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_PPM (1 << 7) /* raw PPM from FMU is bypassing the mixer */ #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 */ @@ -149,7 +150,7 @@ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 102 @@ -166,6 +167,9 @@ #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 */ + /** * As-needed mixer data upload. * diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 13f46939a8..c3dacb2a66 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -115,6 +115,7 @@ int user_start(int argc, char *argv[]) perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* run the mixer at 100Hz (for now...) */ + /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ for (;;) { poll(NULL, 0, 10); perf_begin(mixer_perf); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 0dd8fe28d6..2f411eebf7 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -152,7 +152,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { - /* XXX scaling - should be -10000..10000 */ + /* XXX range-check value? */ r_page_controls[offset] = *values; offset++; @@ -163,6 +163,26 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PPM; + 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++; + } + + /* XXX need to force these values to the servos */ + system_state.fmu_data_received_time = hrt_absolute_time(); + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM; break; /* handle text going to the mixer parser */ From 0eb5a070f165fe311dd5bdf4c40635276b000787 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:41:47 -0800 Subject: [PATCH 033/167] Checkpoint: more work on the px4io driver. Add raw PWM passthrough ioctl. --- apps/drivers/drv_pwm_output.h | 3 - apps/drivers/px4io/px4io.cpp | 152 ++++++++++++++++++++++++---------- 2 files changed, 110 insertions(+), 45 deletions(-) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index c110cd5cbc..fe08d3fe57 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -77,9 +77,6 @@ typedef uint16_t servo_position_t; * device. */ struct pwm_output_values { - /** desired servo update rate in Hz */ - uint32_t update_rate; - /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; }; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 968f433b2d..f97f4668d5 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -35,8 +35,7 @@ * @file px4io.cpp * Driver for the PX4IO board. * - * PX4IO is connected via serial (or possibly some other interface at a later - * point). + * PX4IO is connected via I2C. */ #include @@ -53,7 +52,6 @@ #include #include #include -#include #include #include @@ -94,6 +92,7 @@ public: 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); /** * Set the PWM via serial update rate @@ -105,7 +104,11 @@ public: private: // XXX - static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; + uint16_t _max_actuators; + uint16_t _max_servos; + uint16_t _max_rc_input; + uint16_t _max_relays; + unsigned _update_rate; ///< serial send rate in Hz volatile int _task; ///< worker task @@ -131,7 +134,6 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output - uint32_t _relays; ///< state of the PX4IO relays, one bit per relay /** * Trampoline to the worker task @@ -160,21 +162,50 @@ private: /** * Fetch RC inputs from IO + * + * @param input_rc Input structure to populate. */ int io_get_rc_input(rc_input_values &input_rc); /** * write register(s) + * + * @param page Register page to write to. + * @param offset Register offset to start writing at. + * @param values Pointer to array of values to write. + * @param num_values The number of values to write. + * @return Zero if all values were successfully written. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + /** + * write a register + * + * @param page Register page to write to. + * @param offset Register offset to write to. + * @param value Value to write. + * @return Zero if the value was written successfully. + */ + int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); + /** * read register(s) + * + * @param page Register page to read from. + * @param offset Register offset to start reading from. + * @param values Pointer to array where values should be stored. + * @param num_values The number of values to read. + * @return Zero if all values were successfully read. */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); /** - * modify s register + * modify a register + * + * @param page Register page to modify. + * @param offset Register offset to modify. + * @param clearbits Bits to clear in the register. + * @param setbits Bits to set in the register. */ int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); @@ -191,6 +222,8 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), dump_one(false), + _max_actuators(0), + _max_servos(0), _update_rate(50), _task(-1), _task_should_exit(false), @@ -246,6 +279,20 @@ PX4IO::init() if (ret != OK) return ret; + /* get some parameters */ + if ((ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT, &_max_actuators, 1)) || + (_max_actuators < 1) || + (ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SERVO_COUNT, &_max_servos, 1)) || + (_max_servos < 1) || + (ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT, &_max_relays, 1)) || + (_max_relays < 1) || + (ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT, &_max_rc_input, 1)) || + (_max_rc_input < 1)) { + + log("failed getting parameters from PX4IO"); + return ret; + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); @@ -268,14 +315,13 @@ PX4IO::init() debug("PX4IO connected"); break; } - usleep(100000); } if (!_connected) { /* error here will result in everything being torn down */ log("PX4IO not responding"); - return -EIO; + ret = -EIO; } return OK; @@ -813,66 +859,68 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: - /* fake an armed transition */ - _armed.armed = true; - _send_needed = true; + /* set the 'armed' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK); break; case PWM_SERVO_DISARM: - /* fake a disarmed transition */ - _armed.armed = false; - _send_needed = true; + /* clear the 'armed' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); break; case PWM_SERVO_SET_UPDATE_RATE: - // not supported yet - ret = -EINVAL; + /* set the requested rate */ + if ((arg >= 50) && (arg <= 400)) { + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE, arg); + } else { + ret = -EINVAL; + } break; - case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_servos - 1): { - /* fake an update to the selected 'servo' channel */ + unsigned channel = cmd - PWM_SERVO_SET(0); + + /* send a direct PWM value */ if ((arg >= 900) && (arg <= 2100)) { - _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; - _send_needed = true; - + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); } else { ret = -EINVAL; } break; + } - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): - /* copy the current output value from the channel */ - *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; - break; + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_servos - 1): { - case GPIO_RESET: - _relays = 0; - _send_needed = true; + unsigned channel = cmd - PWM_SERVO_GET(0); + + /* fetch a current PWM value */ + ret = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel, (uint16_t *)arg, 1); break; + } + + case GPIO_RESET: { + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg); + break; + } case GPIO_SET: + arg &= ((1 << _max_relays) - 1); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, value); + break; + case GPIO_CLEAR: - /* make sure only valid bits are being set */ - if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) { - ret = EINVAL; - break; - } - if (cmd == GPIO_SET) { - _relays |= arg; - } else { - _relays &= ~arg; - } - _send_needed = true; + arg &= ((1 << _max_relays) - 1); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, value, 0); break; case GPIO_GET: - *(uint32_t *)arg = _relays; + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, (uint16_t *)arg, 1); break; case MIXERIOCGETOUTPUTCOUNT: - *(unsigned *)arg = PX4IO_CONTROL_CHANNELS; + *(unsigned *)arg = _max_servos; break; case MIXERIOCRESET: @@ -881,6 +929,9 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case MIXERIOCLOADBUF: + +XXX + /* set the buffer up for transfer */ _mix_buf = (const char *)arg; _mix_buf_len = strnlen(_mix_buf, 1024); @@ -906,6 +957,23 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } +ssize_t +write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + int ret; + + if (count > 0) { + if (count > _max_servos) + count = _max_servos; + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, buffer, count); + } else { + ret = -EINVAL; + } + + return ret; +} + extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace From b4dcdae03d597b71f02850bec24c80134c10ae53 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 22:22:15 -0800 Subject: [PATCH 034/167] Add support for battery current scaling. Add feedback for mixer load operations. --- apps/px4io/mixer.cpp | 102 ++++++++++++++++++++++------------------- apps/px4io/protocol.h | 10 ++-- apps/px4io/registers.c | 22 +++++++-- 3 files changed, 79 insertions(+), 55 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 178b0bb431..932aab930f 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -70,6 +70,7 @@ static bool mixer_servos_armed = false; /* selected control values and count for mixing */ enum mixer_source { + MIX_NONE, MIX_FMU, MIX_OVERRIDE, MIX_FAILSAFE @@ -81,7 +82,6 @@ static int mixer_callback(uintptr_t handle, uint8_t control_index, float &control); -static void mix(); static MixerGroup mixer_group(mixer_callback, 0); void @@ -97,30 +97,56 @@ mixer_tick(void) debug("AP RX timeout"); } + source = MIX_FAILSAFE; + /* * Decide which set of controls we're using. */ - if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { - /* this is for planes, where manual override makes sense */ - source = MIX_OVERRIDE; + if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) { - /* mix from the override controls */ - mix(); + /* don't actually mix anything - we already have raw PWM values */ + source = MIX_NONE; - } else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + } else { + + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) { - /* FMU has already provided PWM values */ - } else { /* mix from FMU controls */ source = MIX_FMU; - mix(); } - } else { - source = MIX_FAILSAFE; - /* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */ + + if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + + /* if allowed, mix from RC inputs directly */ + source = MIX_OVERRIDE; + } + } + + /* + * Run the mixers. + */ + 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; } #if 0 @@ -196,9 +222,12 @@ mixer_tick(void) /* * 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. */ - - bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED); + bool should_arm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PPM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -212,33 +241,6 @@ mixer_tick(void) } } -static void -mix() -{ - /* - * Run the mixers. - */ - 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; - -} - static int mixer_callback(uintptr_t handle, uint8_t control_group, @@ -264,6 +266,7 @@ mixer_callback(uintptr_t handle, return -1; case MIX_FAILSAFE: + case MIX_NONE: /* XXX we could allow for configuration of per-output failsafe values */ return -1; } @@ -292,6 +295,7 @@ mixer_handle_text(const void *buffer, size_t length) debug("reset"); mixer_group.reset(); mixer_text_length = 0; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: @@ -299,8 +303,10 @@ mixer_handle_text(const void *buffer, size_t 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)) + 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); @@ -314,6 +320,10 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { + + /* ideally, this should test resid == 0 ? */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + debug("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index a2be4bfc4c..39ee4c0b1a 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -101,6 +101,7 @@ #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_PPM (1 << 7) /* raw PPM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #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 */ @@ -111,7 +112,7 @@ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) #define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_TEMPERATURE 5 /* temperature in (units tbd) */ +#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 */ @@ -143,11 +144,12 @@ #define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ - #define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_HIGHRATE 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 */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ @@ -187,5 +189,3 @@ struct px4io_mixdata { char text[0]; /* actual text size may vary */ }; -/* maximum size is limited by the link frame size XXX use config values to set this */ -#define F2I_MIXER_MAX_TEXT (64 - sizeof(struct px4io_mixdata)) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 2f411eebf7..0206e0db0e 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -60,6 +60,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_LOWRATE] = 50, [PX4IO_P_SETUP_PWM_HIGHRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, + [PX4IO_P_SETUP_VBATT_SCALE] = 10000, + [PX4IO_P_SETUP_IBATT_SCALE] = 0, + [PX4IO_P_SETUP_IBATT_BIAS] = 0 }; #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) @@ -80,7 +83,7 @@ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 0, [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0, [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 0, - [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, + [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, @@ -97,7 +100,7 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_TEMPERATURE] = 0 + [PX4IO_P_STATUS_IBATT] = 0 }; /** @@ -396,10 +399,21 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); - r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10; + unsigned mV = (4150 + (counts * 46)) / 10; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } - /* XXX PX4IO_P_STATUS_TEMPERATURE */ + /* 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; From d207d22a4f4c845ae9874846e7aa92dce9d1df20 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 22:59:57 -0800 Subject: [PATCH 035/167] compile fix --- apps/drivers/device/i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index c513ae2b67..a5b2fab7e2 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -178,7 +178,7 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) * preference. Really, this is pointless. */ I2C_SETFREQUENCY(_dev, _frequency); - ret = I2C_TRANSFER(_dev, msgv, msgs); + int ret = I2C_TRANSFER(_dev, msgv, msgs); if (ret != OK) up_i2creset(_dev); From 646b926ac98534004a0226dc491458728e61dabd Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 23:00:21 -0800 Subject: [PATCH 036/167] minor doc fix --- apps/uORB/topics/actuator_controls_effective.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index 40278c56c2..088c4fc8fe 100644 --- a/apps/uORB/topics/actuator_controls_effective.h +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -37,8 +37,8 @@ * Actuator control topics - mixer inputs. * * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. + * system and mixing process; they are the control-scale values that are + * then fed to the actual actuator driver. * * Each topic can be published by a single controller */ From 7b367c3eb30dac38c016e78c1a64897ff10f377c Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 23:01:04 -0800 Subject: [PATCH 037/167] Beat the px4io driver into compilable shape. Just missing RC input configuration now. --- apps/drivers/px4io/px4io.cpp | 596 +++++++++++++++++++---------------- apps/px4io/mixer.cpp | 6 + 2 files changed, 326 insertions(+), 276 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f97f4668d5..c78ce71c68 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -58,6 +58,7 @@ #include #include +#include #include #include #include @@ -66,7 +67,6 @@ #include #include -#include #include #include #include @@ -94,27 +94,25 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); - /** - * Set the PWM via serial update rate - * @warning this directly affects CPU load - */ - int set_pwm_rate(int hz); - - bool dump_one; - private: // XXX - uint16_t _max_actuators; - uint16_t _max_servos; - uint16_t _max_rc_input; - uint16_t _max_relays; + unsigned _max_actuators; + unsigned _max_rc_input; + unsigned _max_relays; + unsigned _max_transfer; - unsigned _update_rate; ///< serial send rate in Hz + unsigned _update_interval; ///< subscription interval limiting send rate volatile int _task; ///< worker task volatile bool _task_should_exit; volatile bool _connected; ///< true once we have received a valid frame + perf_counter_t _perf_update; + + /* cached IO state */ + uint16_t _status; + uint16_t _alarms; + /* subscribed topics */ int _t_actuators; ///< actuator output topic int _t_armed; ///< system armed control topic @@ -157,15 +155,33 @@ private: /** * Fetch status and alarms from IO + * + * Also publishes battery voltage/current. */ int io_get_status(); /** - * Fetch RC inputs from IO + * Fetch RC inputs from IO. * * @param input_rc Input structure to populate. + * @return OK if data was returned. */ - int io_get_rc_input(rc_input_values &input_rc); + int io_get_raw_rc_input(rc_input_values &input_rc); + + /** + * Fetch and publish raw RC input data. + */ + int io_publish_raw_rc(); + + /** + * Fetch and publish the mixed control values. + */ + int io_publish_mixed_controls(); + + /** + * Fetch and publish the PWM servo outputs. + */ + int io_publish_pwm_outputs(); /** * write register(s) @@ -199,6 +215,16 @@ private: */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + /** + * read a register + * + * @param page Register page to read from. + * @param offset Register offset to start reading from. + * @return Register value that was read, or _io_reg_get_error on error. + */ + uint32_t io_reg_get(uint8_t page, uint8_t offset); + static const uint32_t _io_reg_get_error = 0x80000000; + /** * modify a register * @@ -209,6 +235,11 @@ private: */ int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); + /** + * Send mixer definition text to IO + */ + int mixer_send(const char *buf, unsigned buflen); + }; @@ -220,14 +251,16 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - CDev("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), - dump_one(false), + I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), - _max_servos(0), - _update_rate(50), + _max_rc_input(0), + _max_relays(0), + _max_transfer(16), /* sensible default */ + _update_interval(0), _task(-1), _task_should_exit(false), _connected(false), + _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _t_actuators(-1), _t_armed(-1), _t_vstatus(-1), @@ -237,11 +270,7 @@ PX4IO::PX4IO() : _to_battery(0), _mix_buf(nullptr), _mix_buf_len(0), - _primary_pwm_device(false), - _relays(0), - _switch_armed(false), - _send_needed(false), - _config_needed(true) + _primary_pwm_device(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -280,19 +309,22 @@ PX4IO::init() return ret; /* get some parameters */ - if ((ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT, &_max_actuators, 1)) || - (_max_actuators < 1) || - (ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SERVO_COUNT, &_max_servos, 1)) || - (_max_servos < 1) || - (ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT, &_max_relays, 1)) || - (_max_relays < 1) || - (ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT, &_max_rc_input, 1)) || - (_max_rc_input < 1)) { + _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); + _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); + _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER); + _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); + if ((_max_actuators < 1) || (_max_actuators == _io_reg_get_error) || + (_max_relays < 1) || (_max_relays == _io_reg_get_error) || + (_max_relays < 16) || (_max_relays == _io_reg_get_error) || + (_max_rc_input < 1) || (_max_rc_input == _io_reg_get_error)) { log("failed getting parameters from PX4IO"); return ret; } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) + _max_rc_input = RC_INPUT_MAX_CHANNELS; + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); @@ -327,17 +359,6 @@ PX4IO::init() return OK; } -int -PX4IO::set_pwm_rate(int hz) -{ - if (hz > 0 && hz <= 400) { - _update_rate = hz; - return OK; - } else { - return -EINVAL; - } -} - void PX4IO::task_main_trampoline(int argc, char *argv[]) { @@ -347,11 +368,9 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - hrt_abstime_t last_poll_time = 0; - unsigned poll_phase = 0; + hrt_abstime last_poll_time = 0; log("starting"); - unsigned update_rate_in_ms; /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -359,10 +378,7 @@ PX4IO::task_main() */ _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); - - /* convert the update rate in hz to milliseconds, rounding down if necessary */ - update_rate_in_ms = 1000 / _update_rate; - orb_set_interval(_t_actuators, update_rate_in_ms); + orb_set_interval(_t_actuators, 20); /* default to 50Hz */ _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ @@ -387,10 +403,19 @@ PX4IO::task_main() /* loop talking to IO */ while (!_task_should_exit) { - /* sleep waiting for topic updates, but no more than 20ms */ - /* XXX should actually be calculated to keep the poller running tidily */ + /* adjust update interval */ + if (_update_interval != 0) { + if (_update_interval < 5) + _update_interval = 5; + if (_update_interval > 100) + _update_interval = 100; + orb_set_interval(_t_actuators, _update_interval); + _update_interval = 0; + } + + /* sleep waiting for topic updates, but no more than 100ms */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); + int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); lock(); /* this would be bad... */ @@ -407,7 +432,7 @@ PX4IO::task_main() if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) io_set_arming_state(); - hrt_abstime_t now = hrt_absolute_time(); + hrt_abstime now = hrt_absolute_time(); /* * If this isn't time for the next tick of the polling state machine, @@ -417,38 +442,27 @@ PX4IO::task_main() continue; /* - * Pull status and alarms from IO + * Pull status and alarms from IO. */ io_get_status(); - switch (poll_phase) { - case 0: - /* XXX fetch raw RC values */ - break; - case 1: - /* XXX fetch servo outputs */ - break; - } - -#if 0 - /* advertise the limited control inputs */ - memset(&_controls_effective, 0, sizeof(_controls_effective)); - _to_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), - &_controls_effective); - - /* advertise the mixed control outputs */ - memset(&_outputs, 0, sizeof(_outputs)); - _to_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &_outputs); -#endif - + /* + * Get R/C input from IO. + */ + io_publish_raw_rc(); + /* + * Fetch mixed servo controls and PWM outputs from IO. + * + * XXX We could do this at a reduced rate in many/most cases. + */ + io_publish_mixed_controls(); + io_publish_pwm_outputs(); } unlock(); -out: debug("exiting"); /* clean up the alternate device node */ @@ -468,13 +482,13 @@ PX4IO::io_set_control_state() /* get controls */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); + ORB_ID(actuator_controls_1), _t_actuators, &controls); for (unsigned i = 0; i < _max_actuators; i++) - regs[i] = FLOAT_TO_REG(_controls.control[i]); + regs[i] = FLOAT_TO_REG(controls.control[i]); /* copy values to registers in IO */ - io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators); + return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators); } int @@ -505,36 +519,40 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE; } - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); } int PX4IO::io_get_status() { - struct { - uint16_t status; - uint16_t alarms; - uint16_t vbatt; - } state; + uint16_t regs[4]; int ret; - bool rc_valid = false; - /* get STATUS_FLAGS, STATUS_ALARMS and STATUS_VBATT in that order */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, state, 3); + /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, regs, sizeof(regs) / sizeof(regs[0])); + if (ret != OK) + return ret; + + _status = regs[0]; + _alarms = regs[1]; /* XXX handle status */ /* XXX handle alarms */ /* only publish if battery has a valid minimum voltage */ - if (state.vbatt > 3300) { + if (regs[2] > 3300) { battery_status_s battery_status; battery_status.timestamp = hrt_absolute_time(); - battery_status.voltage_v = state.vbatt / 1000.0f; - /* current and discharge are currently (ha) unknown */ - battery_status.current_a = -1.0f; + /* voltage is scaled to mV */ + battery_status.voltage_v = regs[2] / 1000.0f; + + /* current scaling should be to cA in order to avoid limiting at 65A */ + battery_status.current_a = regs[3] / 100.f; + + /* this requires integration over time - not currently implemented */ battery_status.discharged_mah = -1.0f; /* lazily publish the battery voltage */ @@ -542,37 +560,13 @@ PX4IO::io_get_status() orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); } else { _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); - } - - /* - * If we have RC input, get it - */ - if (state.status & PX4IO_P_STATUS_FLAGS_RC_OK) { - rc_input_values input_rc; - - io_get_rc_input(input_rc); - - if (state.status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - input_rc.input_source = RC_INPUT_SOURCE_PX4IO_PPM; - } else if (state.status & RC_INPUT_SOURCE_PX4IO_DSM) { - input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; - } else if (state.status & RC_INPUT_SOURCE_PX4IO_SBUS) { - input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; - } else { - input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - } - - if (_to_input_rc > 0) { - orb_publish(ORB_ID(input_rc), _to_input_rc, &input_rc); - } else { - _to_input_rc = orb_advertise(ORB_ID(input_rc), &input_rc); } } - + return ret; } int -PX4IO::io_get_rc_input(rc_input_values &input_rc) +PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { uint16_t channel_count; int ret; @@ -592,38 +586,165 @@ PX4IO::io_get_rc_input(rc_input_values &input_rc) * * XXX Since IO has the input calibration info, we ought to be * able to get the pre-fixed-up controls directly. + * + * XXX can we do this more cheaply? If we knew we had DMA, it would + * almost certainly be better to just get all the inputs... */ - ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1); - if (ret) + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1); + if (ret != OK) return ret; input_rc.channel_count = channel_count; if (channel_count > 0) - ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, channel_count); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); return ret; } +int +PX4IO::io_publish_raw_rc() +{ + /* if no RC, just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + return OK; + + /* fetch values from IO */ + rc_input_values rc_val; + rc_val.timestamp = hrt_absolute_time(); + + int ret = io_get_raw_rc_input(rc_val); + if (ret != OK) + return ret; + + /* sort out the source of the values */ + if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { + rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* lazily advertise on first publication */ + if (_to_input_rc == 0) { + _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); + } else { + orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); + } + + return OK; +} + +int +PX4IO::io_publish_mixed_controls() +{ + /* if no FMU comms(!) just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) + return OK; + + /* if not taking raw PPM from us, must be mixing */ + if (_status & PX4IO_P_STATUS_FLAGS_RAW_PPM) + return OK; + + /* data we are going to fetch */ + actuator_controls_effective_s controls_effective; + controls_effective.timestamp = hrt_absolute_time(); + + /* get actuator controls from IO */ + uint16_t act[_max_actuators]; + int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + if (ret != OK) + return ret; + + /* convert from register format to float */ + for (unsigned i = 0; i < _max_actuators; i++) + controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + + /* laxily advertise on first publication */ + if (_to_actuators_effective == 0) { + _to_actuators_effective = + orb_advertise((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + &controls_effective); + } else { + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + _to_actuators_effective, &controls_effective); + } + + return OK; +} + +int +PX4IO::io_publish_pwm_outputs() +{ + /* if no FMU comms(!) just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) + return OK; + + /* data we are going to fetch */ + actuator_outputs_s outputs; + outputs.timestamp = hrt_absolute_time(); + + /* get servo values from IO */ + uint16_t ctl[_max_actuators]; + int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); + if (ret != OK) + return ret; + + /* convert from register format to float */ + for (unsigned i = 0; i < _max_actuators; i++) + outputs.output[i] = REG_TO_FLOAT(ctl[i]); + outputs.noutputs = _max_actuators; + + /* lazily advertise on first publication */ + if (_to_outputs == 0) { + _to_outputs = orb_advertise((_primary_pwm_device ? + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + &outputs); + } else { + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + _to_outputs, + &outputs); + } + + return OK; +} + + int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { i2c_msg_s msgv[2]; - t8_t hdr[2]; + uint8_t hdr[2]; hdr[0] = page; hdr[1] = offset; - mgsv[0].flags = 0; + msgv[0].flags = 0; msgv[0].buffer = hdr; msgv[0].length = sizeof(hdr); msgv[1].flags = 0; - msgv[1].buffer = const_cast(values); + msgv[1].buffer = (uint8_t *)(values); msgv[1].length = num_values * sizeof(*values); return transfer(msgv, 2); } +int +PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) +{ + return io_reg_set(page, offset, &value, 1); +} + int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { @@ -633,17 +754,28 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v hdr[0] = page; hdr[1] = offset; - mgsv[0].flags = 0; + msgv[0].flags = 0; msgv[0].buffer = hdr; msgv[0].length = sizeof(hdr); msgv[1].flags = I2C_M_READ; - msgv[1].buffer = values; + msgv[1].buffer = (uint8_t *)values; msgv[1].length = num_values * sizeof(*values); return transfer(msgv, 2); } +uint32_t +PX4IO::io_reg_get(uint8_t page, uint8_t offset) +{ + uint16_t value; + + if (io_reg_get(page, offset, &value, 1)) + return _io_reg_get_error; + + return value; +} + int PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) { @@ -659,104 +791,6 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t return io_reg_set(page, offset, &value, 1); } - - - -void -PX4IO::io_recv() -{ - uint8_t buf[32]; - int count; - - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_serial_fd, buf, sizeof(buf)); - - /* pass received bytes to the packet decoder */ - for (int i = 0; i < count; i++) - hx_stream_rx(_io_stream, buf[i]); -} - -void -PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received) -{ - g_dev->rx_callback((const uint8_t *)buffer, bytes_received); -} - -void -PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) -{ - const px4io_report *rep = (const px4io_report *)buffer; - -// lock(); - - /* sanity-check the received frame size */ - if (bytes_received != sizeof(px4io_report)) { - debug("got %u expected %u", bytes_received, sizeof(px4io_report)); - goto out; - } - - if (rep->i2f_magic != I2F_MAGIC) { - debug("bad magic"); - goto out; - } - - _connected = true; - - /* publish raw rc channel values from IO if valid channels are present */ - if (rep->channel_count > 0) { - _input_rc.timestamp = hrt_absolute_time(); - _input_rc.channel_count = rep->channel_count; - - for (int i = 0; i < rep->channel_count; i++) { - _input_rc.values[i] = rep->rc_channel[i]; - } - - orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); - } - - /* 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 */ - if (dump_one) { - dump_one = false; - - printf("IO: %s armed ", rep->armed ? "" : "not"); - - for (unsigned i = 0; i < rep->channel_count; i++) - printf("%d: %d ", i, rep->rc_channel[i]); - - printf("\n"); - } - -out: -// unlock(); - return; -} - #if 0 void PX4IO::config_send() @@ -812,12 +846,14 @@ PX4IO::config_send() if (ret) debug("config error %d", ret); } +#endif int PX4IO::mixer_send(const char *buf, unsigned buflen) { - uint8_t frame[HX_STREAM_MAX_FRAME]; + uint8_t frame[_max_transfer]; px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); msg->f2i_mixer_magic = F2I_MIXER_MAGIC; msg->action = F2I_MIXER_ACTION_RESET; @@ -825,8 +861,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) do { unsigned count = buflen; - if (count > F2I_MIXER_MAX_TEXT) - count = F2I_MIXER_MAX_TEXT; + if (count > max_len) + count = max_len; if (count > 0) { memcpy(&msg->text[0], buf, count); @@ -834,7 +870,20 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) buflen -= count; } - int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count); + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 1) { + msg->text[count] = '\0'; + total_len++; + } + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); if (ret) { log("mixer send error %d", ret); @@ -845,17 +894,19 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) } while (buflen > 0); - return 0; + /* check for the mixer-OK flag */ + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) + return 0; + + /* load must have failed for some reason */ + return -EINVAL; } -#endif int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { int ret = OK; - lock(); - /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: @@ -877,50 +928,59 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } break; - case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_servos - 1): { + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): { unsigned channel = cmd - PWM_SERVO_SET(0); - - /* send a direct PWM value */ - if ((arg >= 900) && (arg <= 2100)) { - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); - } else { + if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { ret = -EINVAL; + } else { + /* send a direct PWM value */ + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); } break; } - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_servos - 1): { + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): { unsigned channel = cmd - PWM_SERVO_GET(0); - /* fetch a current PWM value */ - ret = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel, (uint16_t *)arg, 1); + if (channel >= _max_actuators) { + ret = -EINVAL; + } else { + /* fetch a current PWM value */ + uint32_t value = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel); + if (value == _io_reg_get_error) { + ret = -EIO; + } else { + *(servo_position_t *)arg = value; + } + } break; } - case GPIO_RESET: { - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg); + case GPIO_RESET: + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); break; - } case GPIO_SET: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, value); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); break; case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, value, 0); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, (uint16_t *)arg, 1); + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; break; case MIXERIOCGETOUTPUTCOUNT: - *(unsigned *)arg = _max_servos; + *(unsigned *)arg = _max_actuators; break; case MIXERIOCRESET: @@ -928,23 +988,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case MIXERIOCLOADBUF: - - -XXX - - /* set the buffer up for transfer */ - _mix_buf = (const char *)arg; - _mix_buf_len = strnlen(_mix_buf, 1024); - - /* drop the lock and wait for the thread to clear the transmit */ - unlock(); - - while (_mix_buf != nullptr) - usleep(1000); - - lock(); - - ret = 0; + ret = mixer_send((const char *)arg, strnlen(_mix_buf, 1024)); break; default: @@ -952,21 +996,19 @@ XXX ret = -ENOTTY; } - unlock(); - return ret; } ssize_t -write(file *filp, const char *buffer, size_t len) +PX4IO::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; int ret; if (count > 0) { - if (count > _max_servos) - count = _max_servos; - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, buffer, count); + if (count > _max_actuators) + count = _max_actuators; + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); } else { ret = -EINVAL; } @@ -1034,8 +1076,10 @@ monitor(void) exit(0); } - if (g_dev != nullptr) - g_dev->dump_one = true; +#warning implement this + +// if (g_dev != nullptr) +// g_dev->dump_one = true; } } @@ -1063,7 +1107,7 @@ px4io_main(int argc, char *argv[]) /* look for the optional pwm update rate for the supported modes */ if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { if (argc > 2 + 1) { - g_dev->set_pwm_rate(atoi(argv[2 + 1])); +#warning implement this } else { fprintf(stderr, "missing argument for pwm update rate (-u)\n"); return 1; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 932aab930f..1c7a053439 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -274,6 +274,12 @@ mixer_callback(uintptr_t handle, 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; From 4b07a9abd36f3632cff1efeb993fd02a441f61d7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 16 Jan 2013 13:02:49 -0800 Subject: [PATCH 038/167] Add RC input configuration, update at startup and on parameter change (max 2 per second). --- apps/drivers/px4io/px4io.cpp | 119 +++++++++++++++++++++++++++++++++-- 1 file changed, 115 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c78ce71c68..c4d7ec4894 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -78,6 +78,7 @@ #include #include #include +#include #include #include "uploader.h" @@ -117,6 +118,7 @@ private: int _t_actuators; ///< actuator output topic int _t_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status + int _t_param; ///< parameter update topic /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io @@ -153,6 +155,11 @@ private: */ int io_set_arming_state(); + /** + * Push RC channel configuration to IO. + */ + int io_set_rc_config(); + /** * Fetch status and alarms from IO * @@ -321,10 +328,16 @@ PX4IO::init() log("failed getting parameters from PX4IO"); return ret; } - if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; + /* publish RC config to IO */ + ret = io_set_rc_config(); + if (ret != OK) { + log("failed to update RC input config"); + return ret; + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); @@ -353,7 +366,7 @@ PX4IO::init() if (!_connected) { /* error here will result in everything being torn down */ log("PX4IO not responding"); - ret = -EIO; + return -EIO; } return OK; @@ -386,14 +399,19 @@ PX4IO::task_main() _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + _t_param = orb_subscribe(ORB_ID(parameter_update)); + orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + /* poll descriptor */ - pollfd fds[3]; + pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_armed; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; + fds[3].fd = _t_param; + fds[3].events = POLLIN; debug("ready"); @@ -459,6 +477,20 @@ PX4IO::task_main() io_publish_mixed_controls(); io_publish_pwm_outputs(); + /* + * If parameters have changed, re-send RC mappings to IO + * + * XXX this may be a bit spammy + */ + if (fds[3].revents & POLLIN) { + parameter_update_s pupdate; + + /* copy to reset the notification */ + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); + + /* re-upload RC input config as it may have changed */ + io_set_rc_config(); + } } unlock(); @@ -522,6 +554,85 @@ PX4IO::io_set_arming_state() return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); } +int +PX4IO::io_set_rc_config() +{ + unsigned offset = 0; + int input_map[_max_rc_input]; + int32_t ichan; + int ret = OK; + + /* + * Generate the input channel -> control channel mapping table; + * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical + * controls. + */ + for (unsigned i = 0; i < _max_rc_input; i++) + input_map[i] = -1; + + param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 0; + + param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 1; + + param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 2; + + param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 3; + + ichan = 4; + for (unsigned i = 0; i < _max_rc_input; i++) + if (input_map[i] == -1) + input_map[i] = ichan++; + + /* + * Iterate all possible RC inputs. + */ + for (unsigned i = 0; i < _max_rc_input; i++) { + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + char pname[16]; + float fval; + + sprintf(pname, "RC%d_MIN", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MIN] = FLOAT_TO_REG(fval); + + sprintf(pname, "RC%d_TRIM", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_CENTER] = FLOAT_TO_REG(fval); + + sprintf(pname, "RC%d_MAX", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MAX] = FLOAT_TO_REG(fval); + + sprintf(pname, "RC%d_DZ", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_DEADZONE] = FLOAT_TO_REG(fval); + + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i]; + + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + sprintf(pname, "RC%d_REV", i + 1); + param_get(param_find(pname), &fval); + if (fval > 0) + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + + /* send channel config to IO */ + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + if (ret != OK) + break; + offset += PX4IO_P_RC_CONFIG_STRIDE; + } + + return ret; +} + int PX4IO::io_get_status() { @@ -718,7 +829,6 @@ PX4IO::io_publish_pwm_outputs() return OK; } - int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -791,6 +901,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t return io_reg_set(page, offset, &value, 1); } + #if 0 void PX4IO::config_send() From d7632b179426bc3544c4bc1a7c024555f3eaafd7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 19 Jan 2013 12:38:53 -0800 Subject: [PATCH 039/167] Drop some commented code now the functionality is implemented. --- apps/drivers/px4io/px4io.cpp | 58 ------------------------------------ 1 file changed, 58 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c4d7ec4894..681ac650e5 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -901,64 +901,6 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t return io_reg_set(page, offset, &value, 1); } - -#if 0 -void -PX4IO::config_send() -{ - px4io_config cfg; - int ret; - - cfg.f2i_config_magic = F2I_CONFIG_MAGIC; - - int val; - - /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */ - param_get(param_find("RC_MAP_ROLL"), &val); - cfg.rc_map[0] = val; - param_get(param_find("RC_MAP_PITCH"), &val); - cfg.rc_map[1] = val; - param_get(param_find("RC_MAP_YAW"), &val); - cfg.rc_map[2] = val; - param_get(param_find("RC_MAP_THROTTLE"), &val); - cfg.rc_map[3] = val; - - /* set the individual channel properties */ - char nbuf[16]; - float float_val; - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_MIN", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_min[i] = float_val; - } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_TRIM", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_trim[i] = float_val; - } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_MAX", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_max[i] = float_val; - } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_REV", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_rev[i] = float_val; - } - for (unsigned i = 0; i < 4; i++) { - sprintf(nbuf, "RC%d_DZ", i + 1); - param_get(param_find(nbuf), &float_val); - cfg.rc_dz[i] = float_val; - } - - ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); - - if (ret) - debug("config error %d", ret); -} -#endif - int PX4IO::mixer_send(const char *buf, unsigned buflen) { From 9a4d6455fc8964a928d94bac6f5db0b8d625b652 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:55:23 -0800 Subject: [PATCH 040/167] More debug macros. --- Debug/ARMv7M | 152 +++++++++++++++++++++++++++++++++++++++++++++++++++ Debug/PX4 | 1 + 2 files changed, 153 insertions(+) create mode 100644 Debug/ARMv7M diff --git a/Debug/ARMv7M b/Debug/ARMv7M new file mode 100644 index 0000000000..3d96da682d --- /dev/null +++ b/Debug/ARMv7M @@ -0,0 +1,152 @@ +# +# Assorted ARMv7M macros +# + +echo Loading ARMv7M GDB macros. Use 'help armv7m' for more information.\n + +define armv7m + echo Use 'help armv7m' for more information.\n +end + +document armv7m +. Various macros for working with the ARMv7M family of processors. +. +. vecstate +. Print information about the current exception handling state. +. +. Use 'help ' for more specific help. +end + + +define vecstate + set $icsr = *(uint32_t *)0xe000ed04 + set $vect = $icsr & 0x1ff + set $pend = ($icsr & 0x1ff000) >> 12 + set $shcsr = *(uint32_t *)0xe000ed24 + set $cfsr = *(uint32_t *)0xe000ed28 + set $mmfsr = $cfsr & 0xff + set $bfsr = ($cfsr >> 8) & 0xff + set $ufsr = ($cfsr >> 16) & 0xffff + set $hfsr = *(uint32_t *)0xe000ed2c + set $bfar = *(uint32_t *)0xe000ed38 + set $mmfar = *(uint32_t *)0xe000ed34 + + # XXX Currently, rather than look at $vect, we just decode the + # fault status registers directly. + + if $hfsr != 0 + printf "HardFault:" + if $hfsr & (1<<1) + printf " due to vector table read fault\n" + end + if $hfsr & (1<<30) + printf " forced ue to escalated or disabled configurable fault (see below)\n" + end + if $hfsr & (1<<31) + printf " due to an unexpected debug event\n" + end + end + if $mmfsr != 0 + printf "MemManage:" + if $mmfsr & (1<<5) + printf " during lazy FP state save" + end + if $mmfsr & (1<<4) + printf " during exception entry" + end + if $mmfsr & (1<<3) + printf " during exception return" + end + if $mmfsr & (1<<0) + printf " during data access" + end + if $mmfsr & (1<<0) + printf " during instruction prefetch" + end + if $mmfsr & (1<<7) + printf " accessing 0x%08x", $mmfar + end + printf "\n" + end + if $bfsr != 0 + printf "BusFault:" + if $bfsr & (1<<2) + printf " (imprecise)" + end + if $bfsr & (1<<1) + printf " (precise)" + end + if $bfsr & (1<<5) + printf " during lazy FP state save" + end + if $bfsr & (1<<4) + printf " during exception entry" + end + if $bfsr & (1<<3) + printf " during exception return" + end + if $bfsr & (1<<0) + printf " during instruction prefetch" + end + if $bfsr & (1<<7) + printf " accessing 0x%08x", $bfar + end + printf "\n" + end + if $ufsr != 0 + printf "UsageFault" + if $ufsr & (1<<9) + printf " due to divide-by-zero" + end + if $ufsr & (1<<8) + printf " due to unaligned memory access" + end + if $ufsr & (1<<3) + printf " due to access to disabled/absent coprocessor" + end + if $ufsr & (1<<2) + printf " due to a bad EXC_RETURN value" + end + if $ufsr & (1<<1) + printf " due to bad T or IT bits in EPSR" + end + if $ufsr & (1<<0) + printf " due to executing an undefined instruction" + end + printf "\n" + end + if ((uint32_t)$lr & 0xf0000000) == 0xf0000000 + if ($lr & 1) + set $frame_ptr = (uint32_t *)$msp + else + set $frame_ptr = (uint32_t *)$psp + end + printf " r0: %08x r1: %08x r2: %08x r3: %08x\n, $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] + printf " r4: %08x r5: %08x r6: %08x r7: %08x\n", $r4, $r5, $r6, $r7 + printf " r8: %08x r9: %08x r10: %08x r11: %08x\n", $r8, $r9, $r10, $r11 + printf " r12: $08x lr: %08x pc: %08xx PSR: %08x\n", $frame_ptr[4], $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] + + # Swap to the context of the faulting code and try to print a backtrace + set $saved_sp = $sp + if $lr & 0x10 + set $sp = $frame_ptr + (8 * 4) + else + set $sp = $frame_ptr + (26 * 4) + end + set $saved_lr = $lr + set $lr = $frame_ptr[5] + set $saved_pc = $pc + set $pc = $frame_ptr[6] + bt + set $sp = $saved_sp + set $lr = $saved_lr + set $pc = $saved_pc + else + printf "(not currently in exception state)\n" + end +end + +document vecstate +. vecstate +. Print information about the current exception handling state. +end diff --git a/Debug/PX4 b/Debug/PX4 index 7d59059398..806c495846 100644 --- a/Debug/PX4 +++ b/Debug/PX4 @@ -2,6 +2,7 @@ # Various PX4-specific macros # source Debug/NuttX +source Debug/ARMv7M echo Loading PX4 GDB macros. Use 'help px4' for more information.\n From 3244bb83ea2656a20cc486205dd77d48f525c05c Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:56:03 -0800 Subject: [PATCH 041/167] Better sanity checking and error handling. --- apps/drivers/px4io/px4io.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 681ac650e5..a866a37cf0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -320,13 +320,13 @@ PX4IO::init() _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER); _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); - if ((_max_actuators < 1) || (_max_actuators == _io_reg_get_error) || - (_max_relays < 1) || (_max_relays == _io_reg_get_error) || - (_max_relays < 16) || (_max_relays == _io_reg_get_error) || - (_max_rc_input < 1) || (_max_rc_input == _io_reg_get_error)) { + if ((_max_actuators < 1) || (_max_actuators > 255) || + (_max_relays < 1) || (_max_relays > 255) || + (_max_relays < 16) || (_max_relays > 255) || + (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); - return ret; + return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; @@ -640,7 +640,7 @@ PX4IO::io_get_status() int ret; /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, regs, sizeof(regs) / sizeof(regs[0])); + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) return ret; @@ -679,7 +679,7 @@ PX4IO::io_get_status() int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { - uint16_t channel_count; + uint32_t channel_count; int ret; input_rc.timestamp = hrt_absolute_time(); @@ -701,9 +701,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * XXX can we do this more cheaply? If we knew we had DMA, it would * almost certainly be better to just get all the inputs... */ - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1); - if (ret != OK) + channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + if (channel_count == _io_reg_get_error) return ret; + if (channel_count > RC_INPUT_MAX_CHANNELS) + channel_count = RC_INPUT_MAX_CHANNELS; input_rc.channel_count = channel_count; if (channel_count > 0) @@ -846,7 +848,10 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned msgv[1].buffer = (uint8_t *)(values); msgv[1].length = num_values * sizeof(*values); - return transfer(msgv, 2); + int ret = transfer(msgv, 2); + if (ret != OK) + debug("io_reg_set: %d", ret); + return ret; } int @@ -872,7 +877,10 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v msgv[1].buffer = (uint8_t *)values; msgv[1].length = num_values * sizeof(*values); - return transfer(msgv, 2); + int ret = transfer(msgv, 2); + if (ret != OK) + debug("io_reg_get: %d", ret); + return ret; } uint32_t @@ -898,7 +906,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t value &= ~clearbits; value |= setbits; - return io_reg_set(page, offset, &value, 1); + return io_reg_set(page, offset, value); } int From b34311915a04e9ec4d301930e4342f285b5c6cb4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:56:58 -0800 Subject: [PATCH 042/167] Safeguard against back-to-back transactions while setting up to handle a register read request. --- apps/px4io/i2c.c | 9 +++++++++ apps/px4io/registers.c | 6 +++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index cf69211791..174ec2813b 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -242,6 +242,7 @@ i2c_rx_complete(void) 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; @@ -250,6 +251,14 @@ i2c_rx_complete(void) 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); } } diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 0206e0db0e..6b7ef015f4 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -80,9 +80,9 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 0, - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0, - [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 0, + [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, From d8a013f8720e81afb637b8206fbe521ccb43ac8f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:57:16 -0800 Subject: [PATCH 043/167] Tinkering. --- apps/systemcmds/i2c/i2c.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 422d9f915f..e1babdc12b 100644 --- a/apps/systemcmds/i2c/i2c.c +++ b/apps/systemcmds/i2c/i2c.c @@ -80,12 +80,13 @@ int i2c_main(int argc, char *argv[]) usleep(100000); - uint32_t val = 0x12345678; - int ret = transfer(PX4_I2C_OBDEV_PX4IO, (uint8_t *)&val, sizeof(val), NULL, 0); + uint8_t buf[] = { 0, 4}; + int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0); if (ret) errx(1, "send failed - %d", ret); + uint32_t val; ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val)); if (ret) errx(1, "recive failed - %d", ret); @@ -128,7 +129,7 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig * if there are any devices on the bus with a different frequency * preference. Really, this is pointless. */ - I2C_SETFREQUENCY(i2c, 320000); + I2C_SETFREQUENCY(i2c, 400000); ret = I2C_TRANSFER(i2c, &msgv[0], msgs); // reset the I2C bus to unwedge on error From 51c47769f8a3a451e1d54852280046472fb65757 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 20:17:28 -0800 Subject: [PATCH 044/167] Restore the correct handling of the ACK flag at read completion. --- nuttx/arch/arm/src/stm32/stm32_i2c.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index 80f1575042..a66ac7e290 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -1225,11 +1225,11 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s *priv) /* Disable acknowledge when last byte is to be received */ + priv->dcnt--; if (priv->dcnt == 1) { stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_ACK, 0); } - priv->dcnt--; #ifdef CONFIG_I2C_POLLED irqrestore(state); From dce2afde0ffc01f8eba921212b819082b6f9297c Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 20:18:18 -0800 Subject: [PATCH 045/167] Rework the way that we handle the address phase for reads. Drop the _connected test as we talk to IO before starting the thread. --- apps/drivers/px4io/px4io.cpp | 72 +++++++++++++----------------------- 1 file changed, 26 insertions(+), 46 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a866a37cf0..bdddd1b1af 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -106,7 +106,6 @@ private: volatile int _task; ///< worker task volatile bool _task_should_exit; - volatile bool _connected; ///< true once we have received a valid frame perf_counter_t _perf_update; @@ -266,7 +265,6 @@ PX4IO::PX4IO() : _update_interval(0), _task(-1), _task_should_exit(false), - _connected(false), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _t_actuators(-1), _t_armed(-1), @@ -322,7 +320,7 @@ PX4IO::init() _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || (_max_relays < 1) || (_max_relays > 255) || - (_max_relays < 16) || (_max_relays > 255) || + (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); @@ -354,21 +352,6 @@ PX4IO::init() return -errno; } - /* wait a second for it to detect IO */ - for (unsigned i = 0; i < 10; i++) { - if (_connected) { - debug("PX4IO connected"); - break; - } - usleep(100000); - } - - if (!_connected) { - /* error here will result in everything being torn down */ - log("PX4IO not responding"); - return -EIO; - } - return OK; } @@ -680,7 +663,7 @@ int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { uint32_t channel_count; - int ret; + int ret = OK; input_rc.timestamp = hrt_absolute_time(); @@ -703,7 +686,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); if (channel_count == _io_reg_get_error) - return ret; + return -EIO; if (channel_count > RC_INPUT_MAX_CHANNELS) channel_count = RC_INPUT_MAX_CHANNELS; input_rc.channel_count = channel_count; @@ -834,23 +817,21 @@ PX4IO::io_publish_pwm_outputs() int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - i2c_msg_s msgv[2]; - uint8_t hdr[2]; + uint8_t buf[_max_transfer]; - hdr[0] = page; - hdr[1] = offset; + if (num_values > ((_max_transfer - 2) / sizeof(*values))) + return -EINVAL; + unsigned datalen = num_values * sizeof(*values); - msgv[0].flags = 0; - msgv[0].buffer = hdr; - msgv[0].length = sizeof(hdr); + buf[0] = page; + buf[1] = offset; - msgv[1].flags = 0; - msgv[1].buffer = (uint8_t *)(values); - msgv[1].length = num_values * sizeof(*values); + if (num_values > 0) + memcpy(&buf[2], values, datalen); - int ret = transfer(msgv, 2); + int ret = transfer(buf, datalen, nullptr, 0); if (ret != OK) - debug("io_reg_set: %d", ret); + debug("io_reg_set: error %d", ret); return ret; } @@ -863,23 +844,22 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - i2c_msg_s msgv[2]; - uint8_t hdr[2]; + uint8_t addr[2]; + int ret; - hdr[0] = page; - hdr[1] = offset; + /* send the address */ + addr[0] = page; + addr[1] = offset; + ret = transfer(addr, 2, nullptr, 0); + if (ret != OK) { + debug("io_reg_get: addr error %d", ret); + return ret; + } - msgv[0].flags = 0; - msgv[0].buffer = hdr; - msgv[0].length = sizeof(hdr); - - msgv[1].flags = I2C_M_READ; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - int ret = transfer(msgv, 2); + /* now read the data */ + ret = transfer(nullptr, 0, (uint8_t *)values, num_values * sizeof(*values)); if (ret != OK) - debug("io_reg_get: %d", ret); + debug("io_reg_get: data error %d", ret); return ret; } From 0bc836ae1d90b1805940ec8ae271639f0074a792 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 22:19:33 -0800 Subject: [PATCH 046/167] Implement fetching raw RC input values via the ioctl interface. --- apps/drivers/drv_rc_input.h | 7 +++++++ apps/drivers/px4io/px4io.cpp | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 927406108a..4decc324ef 100644 --- a/apps/drivers/drv_rc_input.h +++ b/apps/drivers/drv_rc_input.h @@ -100,4 +100,11 @@ struct rc_input_values { */ ORB_DECLARE(input_rc); +#define _RC_INPUT_BASE 0x2b00 + +/** Fetch R/C input values into (rc_input_values *)arg */ + +#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0) + + #endif /* _DRV_RC_INPUT_H */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index bdddd1b1af..51476ddd3b 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -313,6 +313,8 @@ PX4IO::init() if (ret != OK) return ret; + _retries = 2; + /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); @@ -1032,6 +1034,36 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = mixer_send((const char *)arg, strnlen(_mix_buf, 1024)); break; + case RC_INPUT_GET: { + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; + + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); + if (ret != OK) + break; + + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } + + /* sort out the source of the values */ + if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { + rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* read raw R/C input values */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); + break; + } + default: /* not a recognised value */ ret = -ENOTTY; From 82f72b96de81478fb5d627be30a2f81ef2c5135f Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:35:32 -0800 Subject: [PATCH 047/167] Move DMA start for tx/rx into the gap where SCL is still stretched so that there is no risk of receiving the first byte before DMA starts. --- apps/px4io/i2c.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 174ec2813b..bbb72360b4 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -158,6 +158,9 @@ i2c_interrupt(int irq, FAR void *context) if (sr1 & I2C_SR1_ADDR) { + stm32_dmastart(tx_dma, NULL, NULL, false); + stm32_dmastart(rx_dma, NULL, NULL, false); + /* clear ADDR to ack our selection and get direction */ (void)rSR1; /* as recommended, re-read SR1 */ uint16_t sr2 = rSR2; @@ -166,13 +169,11 @@ i2c_interrupt(int irq, FAR void *context) /* we are the transmitter */ direction = DIR_TX; - stm32_dmastart(tx_dma, NULL, NULL, false); } else { /* we are the receiver */ direction = DIR_RX; - stm32_dmastart(rx_dma, NULL, NULL, false); } } From 57d028fddd096cb3336aa1fbd60d406e2d44ec48 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 25 Jan 2013 10:10:47 +1100 Subject: [PATCH 048/167] px4io: fixed array reference bug --- apps/px4io/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 6b7ef015f4..1802433b34 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -290,7 +290,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset % PX4IO_P_RC_CONFIG_STRIDE; - uint16_t *conf = &r_page_rc_input_config[offset * 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; From f8bea6d07b3ca32e697617588df2c106b8fb375c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 25 Jan 2013 10:13:00 +1100 Subject: [PATCH 049/167] px4io: fixed cpp error --- apps/px4io/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 1802433b34..a4d81fd6e6 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -428,7 +428,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* * Pages that are just a straight read of the register state. */ -#define COPY_PAGE(_page_name, _page) case _page_name: SELECT_PAGE(_page); break; +#define COPY_PAGE(_page_name, _page) case _page_name: SELECT_PAGE(_page); break /* status pages */ COPY_PAGE(PX4IO_PAGE_CONFIG, r_page_config); From 8972843b14a86aae3bbc4aa84e0c0a446eb7d605 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 25 Jan 2013 10:15:41 +1100 Subject: [PATCH 050/167] px4io: fixed mixer load --- apps/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 51476ddd3b..4c1b5ddf25 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1030,9 +1030,11 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = 0; /* load always resets */ break; - case MIXERIOCLOADBUF: - ret = mixer_send((const char *)arg, strnlen(_mix_buf, 1024)); + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + ret = mixer_send(buf, strnlen(buf, 1024)); break; + } case RC_INPUT_GET: { uint16_t status; From 1b30cd2f938c05841470e5aedcbd535105ea3f36 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:40:18 -0800 Subject: [PATCH 051/167] Dump a couple of unused member variables. --- apps/drivers/px4io/px4io.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 4c1b5ddf25..9852448737 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -128,9 +128,6 @@ private: actuator_outputs_s _outputs; ///< mixed outputs actuator_controls_effective_s _controls_effective; ///< effective controls - const char *volatile _mix_buf; ///< mixer text buffer - volatile unsigned _mix_buf_len; ///< size of the mixer text buffer - bool _primary_pwm_device; ///< true if we are the default PWM output From 818e898a7e084d6529da549ca3ea7c7d53fe5c46 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:54:04 -0800 Subject: [PATCH 052/167] Fix the handling of max transfer size to leave room for the page/offset bytes. --- apps/drivers/px4io/px4io.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9852448737..fbbc0277ee 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -270,8 +270,6 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), - _mix_buf(nullptr), - _mix_buf_len(0), _primary_pwm_device(false) { /* we need this potentially before it could be set in task_main */ @@ -315,7 +313,7 @@ PX4IO::init() /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); - _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER); + _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || (_max_relays < 1) || (_max_relays > 255) || @@ -816,9 +814,9 @@ PX4IO::io_publish_pwm_outputs() int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - uint8_t buf[_max_transfer]; + uint8_t buf[_max_transfer + 2]; - if (num_values > ((_max_transfer - 2) / sizeof(*values))) + if (num_values > ((_max_transfer) / sizeof(*values))) return -EINVAL; unsigned datalen = num_values * sizeof(*values); @@ -928,16 +926,22 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) if (ret) { log("mixer send error %d", ret); return ret; + } else { + debug("mixer sent %u", total_len); } msg->action = F2I_MIXER_ACTION_APPEND; } while (buflen > 0); + debug("mixer upload OK"); + /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) return 0; + debug("mixer rejected"); + /* load must have failed for some reason */ return -EINVAL; } From 6c75c5909e7f72897ddb737c6459ea6bbfab3900 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:58:55 -0800 Subject: [PATCH 053/167] Move the DMA start to immediately after setting it up; less latency at interrupt time, and no chance of getting start/stop calls out of sync. --- apps/px4io/i2c.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index bbb72360b4..dca5ceb1d7 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -158,9 +158,6 @@ i2c_interrupt(int irq, FAR void *context) if (sr1 & I2C_SR1_ADDR) { - stm32_dmastart(tx_dma, NULL, NULL, false); - stm32_dmastart(rx_dma, NULL, NULL, false); - /* clear ADDR to ack our selection and get direction */ (void)rSR1; /* as recommended, re-read SR1 */ uint16_t sr2 = rSR2; @@ -222,6 +219,8 @@ i2c_rx_setup(void) DMA_CCR_PSIZE_32BITS | DMA_CCR_MSIZE_8BITS | DMA_CCR_PRIMED); + + stm32_dmastart(rx_dma, NULL, NULL, false); } static void @@ -283,6 +282,8 @@ i2c_tx_setup(void) DMA_CCR_PSIZE_8BITS | DMA_CCR_MSIZE_8BITS | DMA_CCR_PRIMED); + + stm32_dmastart(tx_dma, NULL, NULL, false); } static void From 900b0d58ef62d1ff1406a312ac88317152f0312a Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:59:31 -0800 Subject: [PATCH 054/167] Less debug output. --- apps/drivers/px4io/px4io.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index fbbc0277ee..9296aa0a57 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -926,8 +926,6 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) if (ret) { log("mixer send error %d", ret); return ret; - } else { - debug("mixer sent %u", total_len); } msg->action = F2I_MIXER_ACTION_APPEND; @@ -940,7 +938,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) return 0; - debug("mixer rejected"); + debug("mixer rejected by IO"); /* load must have failed for some reason */ return -EINVAL; From b46d05835b9ff55c1f21d37483202888eeea1656 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 22:58:33 -0800 Subject: [PATCH 055/167] Implement settable failsafe values for PWM outputs. By default in failsafe mode, PWM output pulses are not generated. --- apps/px4io/i2c.c | 2 +- apps/px4io/mixer.cpp | 17 +++++++++++++---- apps/px4io/protocol.h | 5 ++++- apps/px4io/px4io.h | 1 + apps/px4io/registers.c | 29 +++++++++++++++++++++++++---- 5 files changed, 44 insertions(+), 10 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index dca5ceb1d7..bbbbb6e46a 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -82,7 +82,7 @@ static unsigned rx_len; static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff }; -static uint8_t *tx_buf = junk_buf; +static const uint8_t *tx_buf = junk_buf; static unsigned tx_len = sizeof(junk_buf); static uint8_t selected_page; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 1c7a053439..91a9145759 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -92,7 +92,7 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; debug("AP RX timeout"); } @@ -102,7 +102,7 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; @@ -127,7 +127,13 @@ mixer_tick(void) /* * Run the mixers. */ - if (source != MIX_NONE) { + 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; @@ -225,9 +231,12 @@ mixer_tick(void) * * 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 = ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PPM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); + (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 39ee4c0b1a..6dbe162295 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -100,7 +100,7 @@ #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_PPM (1 << 7) /* raw PPM from FMU is bypassing the mixer */ +#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_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ @@ -172,6 +172,9 @@ /* 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. * diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index d8cdafb4b5..c90db5d780 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -76,6 +76,7 @@ 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. diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index a4d81fd6e6..22ed1de702 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -118,6 +118,11 @@ uint16_t r_page_actuators[IO_SERVO_COUNT]; */ uint16_t r_page_servos[IO_SERVO_COUNT]; +/** + * Servo PWM values + */ +uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; + /** * Scaled/routed RC input */ @@ -166,7 +171,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PPM; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; break; /* handle raw PWM input */ @@ -183,9 +188,24 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num values++; } - /* XXX need to force these values to the servos */ + /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM; + 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 */ @@ -353,7 +373,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) { -#define SELECT_PAGE(_page_name) { *values = _page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); } +#define SELECT_PAGE(_page_name) { *values = (uint16_t *)_page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); } switch (page) { @@ -441,6 +461,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val COPY_PAGE(PX4IO_PAGE_SETUP, r_page_setup); COPY_PAGE(PX4IO_PAGE_CONTROLS, r_page_controls); COPY_PAGE(PX4IO_PAGE_RC_CONFIG, r_page_rc_input_config); + COPY_PAGE(PX4IO_PAGE_FAILSAFE_PWM, r_page_servo_failsafe); default: return -1; From 5fe376c7b9bed861768680089bff3c62a030e2b6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 00:01:25 -0800 Subject: [PATCH 056/167] Correctness fixes from Tridge.; increased the minimum poll rate to 50Hz, don't set the input RC timestamp unless we get data. --- apps/drivers/px4io/px4io.cpp | 80 +++++++++++++++++++----------------- 1 file changed, 42 insertions(+), 38 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9296aa0a57..a0e9b18d23 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -411,9 +411,9 @@ PX4IO::task_main() _update_interval = 0; } - /* sleep waiting for topic updates, but no more than 100ms */ + /* sleep waiting for topic updates, but no more than 20ms */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); + int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); lock(); /* this would be bad... */ @@ -422,6 +422,9 @@ PX4IO::task_main() continue; } + perf_begin(_perf_update); + hrt_abstime now = hrt_absolute_time(); + /* if we have new control data from the ORB, handle it */ if (fds[0].revents & POLLIN) io_set_control_state(); @@ -430,47 +433,47 @@ PX4IO::task_main() if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) io_set_arming_state(); - hrt_abstime now = hrt_absolute_time(); - /* - * If this isn't time for the next tick of the polling state machine, - * go back to sleep. + * If it's time for another tick of the polling status machine, + * try it now. */ - if ((now - last_poll_time) < 20000) - continue; + if ((now - last_poll_time) >= 20000) { - /* - * Pull status and alarms from IO. - */ - io_get_status(); + /* + * Pull status and alarms from IO. + */ + io_get_status(); - /* - * Get R/C input from IO. - */ - io_publish_raw_rc(); + /* + * Get raw R/C input from IO. + */ + io_publish_raw_rc(); - /* - * Fetch mixed servo controls and PWM outputs from IO. - * - * XXX We could do this at a reduced rate in many/most cases. - */ - io_publish_mixed_controls(); - io_publish_pwm_outputs(); + /* + * Fetch mixed servo controls and PWM outputs from IO. + * + * XXX We could do this at a reduced rate in many/most cases. + */ + io_publish_mixed_controls(); + io_publish_pwm_outputs(); - /* - * If parameters have changed, re-send RC mappings to IO - * - * XXX this may be a bit spammy - */ - if (fds[3].revents & POLLIN) { - parameter_update_s pupdate; + /* + * If parameters have changed, re-send RC mappings to IO + * + * XXX this may be a bit spammy + */ + if (fds[3].revents & POLLIN) { + parameter_update_s pupdate; - /* copy to reset the notification */ - orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); + /* copy to reset the notification */ + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - /* re-upload RC input config as it may have changed */ - io_set_rc_config(); + /* re-upload RC input config as it may have changed */ + io_set_rc_config(); + } } + + perf_end(_perf_update); } unlock(); @@ -662,8 +665,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) uint32_t channel_count; int ret = OK; - input_rc.timestamp = hrt_absolute_time(); - /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; @@ -688,8 +689,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) channel_count = RC_INPUT_MAX_CHANNELS; input_rc.channel_count = channel_count; - if (channel_count > 0) + if (channel_count > 0) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); + if (ret == OK) + input_rc.timestamp = hrt_absolute_time(); + } return ret; } @@ -697,7 +701,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int PX4IO::io_publish_raw_rc() { - /* if no RC, just don't publish */ + /* if no raw RC, just don't publish */ if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) return OK; From f854e2f79133c93c56a40645fd37a103a26b4623 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 00:02:45 -0800 Subject: [PATCH 057/167] Fixes from/inspired by Tridge; enable all mapped R/C inputs, fix various logic errors, be more selective about clearing the RC input type flags for debugging purposes. --- apps/px4io/controls.c | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 2af62a369f..0b2c199ee7 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -70,7 +70,7 @@ controls_main(void) ASSERT(fds[0].fd >= 0); ASSERT(fds[1].fd >= 0); - /* default to a 1:1 input map */ + /* 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; @@ -80,6 +80,7 @@ controls_main(void) 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; } for (;;) { @@ -124,7 +125,7 @@ controls_main(void) /* * If we received a new frame from any of the RC sources, process it. */ - if (dsm_updated | sbus_updated | ppm_updated) { + if (dsm_updated || sbus_updated || ppm_updated) { /* update RC-received timestamp */ system_state.rc_channels_timestamp = hrt_absolute_time(); @@ -139,7 +140,7 @@ controls_main(void) /* 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) { + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint16_t raw = r_raw_rc_values[i]; @@ -193,6 +194,9 @@ controls_main(void) /* * 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; @@ -209,6 +213,12 @@ controls_main(void) */ if ((hrt_absolute_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); } /* @@ -216,13 +226,10 @@ controls_main(void) */ if (rc_input_lost) { - /* Clear the RC input status flags, clear manual override control */ + /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK | - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS); + PX4IO_P_STATUS_FLAGS_RC_OK); /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; @@ -247,6 +254,8 @@ controls_main(void) /* * Check mapped channel 5; if the value is 'high' then the pilot has * requested override. + * + * XXX This should be configurable. */ if ((r_rc_valid & (1 << 4)) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) override = true; @@ -283,12 +292,12 @@ ppm_input(uint16_t *values, uint16_t *num_values) irqstate_t state = irqsave(); /* - * Look for recent PPM input. + * If we have received a new PPM frame within the last 200ms, accept it + * and then invalidate it. */ if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { /* PPM data exists, copy it */ - result = true; *num_values = ppm_decoded_channels; if (*num_values > MAX_CONTROL_CHANNELS) *num_values = MAX_CONTROL_CHANNELS; @@ -298,6 +307,9 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* clear validity */ ppm_last_valid_decode = 0; + + /* good if we got any channels */ + result = (*num_values > 0); } irqrestore(state); From b20c050402dfe11e1f0d0002020205872b96172d Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 12:27:03 -0800 Subject: [PATCH 058/167] Fix two protocol-related typos; get the right status flag name for raw PWM; read back the correct page for PWM output. --- apps/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a0e9b18d23..1678ed0de2 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -742,7 +742,7 @@ PX4IO::io_publish_mixed_controls() return OK; /* if not taking raw PPM from us, must be mixing */ - if (_status & PX4IO_P_STATUS_FLAGS_RAW_PPM) + if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM) return OK; /* data we are going to fetch */ @@ -981,7 +981,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -EINVAL; } else { /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + ret = io_reg_set(PX4IO_PAGE_SERVOS, channel, arg); } break; From 984e68d76e6e199608aded66ed3c032a3db2fe32 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 12:27:42 -0800 Subject: [PATCH 059/167] Add an ioctl for fetching the number of PWM outputs --- apps/drivers/drv_pwm_output.h | 3 +++ apps/drivers/px4io/px4io.cpp | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index fe08d3fe57..23883323e2 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm); /** set update rate in Hz */ #define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +/** get the number of servos in *(unsigned *)arg */ +#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 1678ed0de2..61c9a793d9 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -974,6 +974,10 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_GET_COUNT: + *(unsigned *)arg = _max_actuators; + break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): { unsigned channel = cmd - PWM_SERVO_SET(0); From 3a8bbe837e6c49e6fd92e47f889a2e7925173611 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:14:15 -0800 Subject: [PATCH 060/167] Allow readback of the direct PWM outputs (this mirrors the PWM servo outputs) --- apps/px4io/registers.c | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 22ed1de702..1593ed2c89 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -461,6 +461,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val COPY_PAGE(PX4IO_PAGE_SETUP, r_page_setup); COPY_PAGE(PX4IO_PAGE_CONTROLS, r_page_controls); COPY_PAGE(PX4IO_PAGE_RC_CONFIG, r_page_rc_input_config); + COPY_PAGE(PX4IO_PAGE_DIRECT_PWM, r_page_servos); COPY_PAGE(PX4IO_PAGE_FAILSAFE_PWM, r_page_servo_failsafe); default: From 899fbcc7cf13fbcdfb371663fef7782dd9ea1456 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:14:52 -0800 Subject: [PATCH 061/167] Fix cut and paste so that we send direct PWM and read back servo values from the right pages. --- apps/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 61c9a793d9..257b15685c 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -985,7 +985,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -EINVAL; } else { /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_SERVOS, channel, arg); + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); } break; @@ -999,7 +999,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -EINVAL; } else { /* fetch a current PWM value */ - uint32_t value = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel); + uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); if (value == _io_reg_get_error) { ret = -EIO; } else { From 72fcc8aad32936692814f4e9521a462bedc6c723 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:15:27 -0800 Subject: [PATCH 062/167] Tidy up the write path. --- apps/drivers/px4io/px4io.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 257b15685c..769363b4a8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1085,18 +1085,16 @@ ssize_t PX4IO::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - int ret; - if (count > 0) { if (count > _max_actuators) count = _max_actuators; - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); - } else { - ret = -EINVAL; - } - + if (count > 0) { + int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + if (ret != OK) return ret; } + return count * 2; +} extern "C" __EXPORT int px4io_main(int argc, char *argv[]); From 4ea8a64b395026d7df8739bc6c76ea121ee6b977 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:15:57 -0800 Subject: [PATCH 063/167] Correct the length calculation for register write transfers so that we send all of the requested registers. --- apps/drivers/px4io/px4io.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 769363b4a8..2d7495aaa8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -820,17 +820,24 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned { uint8_t buf[_max_transfer + 2]; - if (num_values > ((_max_transfer) / sizeof(*values))) + /* range check the transfer */ + if (num_values > ((_max_transfer) / sizeof(*values))) { + debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; - unsigned datalen = num_values * sizeof(*values); + } + /* set page/offset address */ buf[0] = page; buf[1] = offset; - if (num_values > 0) + /* copy data into our local transfer buffer */ + /* XXX we could use a msgv and two transactions, maybe? */ + unsigned datalen = num_values * sizeof(*values); + if (datalen > 0) memcpy(&buf[2], values, datalen); - int ret = transfer(buf, datalen, nullptr, 0); + /* perform the transfer */ + int ret = transfer(buf, datalen + 2, nullptr, 0); if (ret != OK) debug("io_reg_set: error %d", ret); return ret; From 4ab490bd5017c6dd132b4422aecda597a5c8cd38 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:16:24 -0800 Subject: [PATCH 064/167] Only update the servo output values when we are armed. --- apps/px4io/mixer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 91a9145759..79b32009e4 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -220,12 +220,6 @@ mixer_tick(void) } #endif - /* - * Update the servo outputs. - */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) - up_pwm_servo_set(i, r_page_servos[i]); - /* * Decide whether the servos should be armed right now. * @@ -243,6 +237,12 @@ mixer_tick(void) up_pwm_servo_arm(true); mixer_servos_armed = true; + /* + * Update the servo outputs. + */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servos[i]); + } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); From 11796e27f25ae9a5d3cb39d2d43f0ad53d388dcc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 14:25:22 -0800 Subject: [PATCH 065/167] Simplify and tidy the handling of page buffer selection on the readout path. --- apps/px4io/registers.c | 62 +++++++++++++++++++++++++++++++----------- 1 file changed, 46 insertions(+), 16 deletions(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 1593ed2c89..1c6fbf4fbf 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -370,10 +370,17 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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) { *values = (uint16_t *)_page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); } +#define SELECT_PAGE(_page_name) \ + do { \ + *values = &_page_name[0]; \ + *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ + } while(0) switch (page) { @@ -448,35 +455,58 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* * Pages that are just a straight read of the register state. */ -#define COPY_PAGE(_page_name, _page) case _page_name: SELECT_PAGE(_page); break /* status pages */ - COPY_PAGE(PX4IO_PAGE_CONFIG, r_page_config); - COPY_PAGE(PX4IO_PAGE_ACTUATORS, r_page_actuators); - COPY_PAGE(PX4IO_PAGE_SERVOS, r_page_servos); - COPY_PAGE(PX4IO_PAGE_RAW_RC_INPUT, r_page_raw_rc_input); - COPY_PAGE(PX4IO_PAGE_RC_INPUT, r_page_rc_input); + 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 */ - COPY_PAGE(PX4IO_PAGE_SETUP, r_page_setup); - COPY_PAGE(PX4IO_PAGE_CONTROLS, r_page_controls); - COPY_PAGE(PX4IO_PAGE_RC_CONFIG, r_page_rc_input_config); - COPY_PAGE(PX4IO_PAGE_DIRECT_PWM, r_page_servos); - COPY_PAGE(PX4IO_PAGE_FAILSAFE_PWM, r_page_servo_failsafe); + 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 - /* if the offset is beyond the end of the page, we have no data */ - if (*num_values <= offset) +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; - /* adjust value count and pointer for the page offset */ - *num_values -= offset; + /* correct the data pointer and count for the offset */ *values += offset; + *num_values -= offset; return 0; } From 6ba4cd04fecd8600dbea7b94ccf0706eac40a089 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 14:26:02 -0800 Subject: [PATCH 066/167] Handle the completion of an in-progress transaction (STOPF/AF bits) before accepting the start of a new transaction (ADDR). --- apps/px4io/i2c.c | 45 ++++++++++++++++++++++++--------------------- 1 file changed, 24 insertions(+), 21 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index bbbbb6e46a..c16d5700cb 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -84,6 +84,7 @@ 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; @@ -156,24 +157,6 @@ i2c_interrupt(int irq, FAR void *context) { uint16_t sr1 = rSR1; - 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; - } - } - if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF)) { if (sr1 & I2C_SR1_STOPF) { @@ -197,6 +180,24 @@ i2c_interrupt(int irq, FAR void *context) 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; @@ -246,7 +247,7 @@ i2c_rx_complete(void) int ret = registers_get(selected_page, selected_offset, ®s, ®_count); if (ret == 0) { tx_buf = (uint8_t *)regs; - tx_len = reg_count *2; + tx_len = reg_count * 2; } else { tx_buf = junk_buf; tx_len = sizeof(junk_buf); @@ -289,10 +290,12 @@ i2c_tx_setup(void) static void i2c_tx_complete(void) { + tx_count = tx_len - stm32_dmaresidual(tx_dma); stm32_dmastop(tx_dma); - tx_buf = junk_buf; - tx_len = sizeof(junk_buf); + /* 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(); From 2a18d6466c36c50244851d225a5319207b08b0bf Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 14:36:31 -0800 Subject: [PATCH 067/167] Add a bus saturation test for px4io. --- apps/drivers/px4io/px4io.cpp | 73 ++++++++++++++++++++++++------------ 1 file changed, 48 insertions(+), 25 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2d7495aaa8..2a860e5c14 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1112,34 +1112,57 @@ void test(void) { int fd; + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + int ret; - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + fd = open("/dev/px4io", O_WRONLY); - if (fd < 0) { - puts("open fail"); - exit(1); + if (fd < 0) + err(1, "failed to open device"); + + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) + err(1, "failed to get servo count"); + + if (ioctl(fd, PWM_SERVO_ARM, 0)) + err(1, "failed to arm servos"); + + for (;;) { + + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; + + ret = write(fd, servos, sizeof(servos)); + if (ret != sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } + + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; + + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + err(1, "error reading PWM servo %d", i); + if (value != servos[i]) + errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } } - - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); - ioctl(fd, PWM_SERVO_SET(1), 1100); - ioctl(fd, PWM_SERVO_SET(2), 1200); - ioctl(fd, PWM_SERVO_SET(3), 1300); - ioctl(fd, PWM_SERVO_SET(4), 1400); - ioctl(fd, PWM_SERVO_SET(5), 1500); - ioctl(fd, PWM_SERVO_SET(6), 1600); - ioctl(fd, PWM_SERVO_SET(7), 1700); - - close(fd); - - actuator_armed_s aa; - - aa.armed = true; - aa.lockdown = false; - - orb_advertise(ORB_ID(actuator_armed), &aa); - - exit(0); } void From e0f83af96fdab2cd5b239dec3a842c4a2a92ad85 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 15:13:30 -0800 Subject: [PATCH 068/167] Reset the collection state machine on all I2C errors, increase the retry count. --- apps/drivers/ms5611/ms5611.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index b8845aaf18..b215166dfe 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -331,7 +331,7 @@ MS5611::probe() if ((OK == probe_address(MS5611_ADDRESS_1)) || (OK == probe_address(MS5611_ADDRESS_2))) { - _retries = 1; + _retries = 2; return OK; } @@ -574,13 +574,15 @@ MS5611::cycle_trampoline(void *arg) void MS5611::cycle() { + int ret; /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { - log("collection error"); + ret = collect(); + if (ret != OK) { + log("collection error %d", ret); /* reset the collection state machine and try again */ start(); return; @@ -609,8 +611,13 @@ MS5611::cycle() } /* measurement phase */ - if (OK != measure()) - log("measure error"); + ret = measure(); + if (ret != OK) { + log("measure error %d", ret); + /* reset the collection state machine and try again */ + start(); + return; + } /* next phase is collection */ _collect_phase = true; @@ -647,6 +654,7 @@ MS5611::measure() int MS5611::collect() { + int ret; uint8_t cmd; uint8_t data[3]; union { @@ -662,9 +670,10 @@ MS5611::collect() /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - if (OK != transfer(&cmd, 1, &data[0], 3)) { + ret = transfer(&cmd, 1, &data[0], 3); + if (ret != OK) { perf_count(_comms_errors); - return -EIO; + return ret; } /* fetch the raw value */ From 6bd662cfb2d90628eb03c0bec1ebac54e47a7090 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:11:31 -0800 Subject: [PATCH 069/167] In the case of a repeated start, we won't get a STOPF/AF status, but we still need to complete the old transaction before handling ADDR. --- apps/px4io/i2c.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index c16d5700cb..2b8c502c7c 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -157,7 +157,7 @@ i2c_interrupt(int irq, FAR void *context) { uint16_t sr1 = rSR1; - if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF)) { + if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) { if (sr1 & I2C_SR1_STOPF) { /* write to CR1 to clear STOPF */ @@ -165,7 +165,7 @@ i2c_interrupt(int irq, FAR void *context) rCR1 |= I2C_CR1_PE; } - /* it's likely that the DMA hasn't stopped, so we have to do it here */ + /* DMA never stops, so we should do that now */ switch (direction) { case DIR_TX: i2c_tx_complete(); From fd28217e5982cbb848a2ddf2d118ffa3d69a4746 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:15:23 -0800 Subject: [PATCH 070/167] Implement the retry counter for message-vector based transfers. --- apps/drivers/device/i2c.cpp | 30 +++++++++++++++++++++--------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index a5b2fab7e2..d2cd5f19bd 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -169,19 +169,31 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re int I2C::transfer(i2c_msg_s *msgv, unsigned msgs) { + int ret; + + /* force the device address into the message vector */ for (unsigned i = 0; i < msgs; i++) msgv[i].addr = _address; - /* - * I2C architecture means there is an unavoidable race here - * if there are any devices on the bus with a different frequency - * preference. Really, this is pointless. - */ - I2C_SETFREQUENCY(_dev, _frequency); - int ret = I2C_TRANSFER(_dev, msgv, msgs); + unsigned tries = 0; - if (ret != OK) - up_i2creset(_dev); + do { + + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(_dev, _frequency); + ret = I2C_TRANSFER(_dev, msgv, msgs); + + if (ret == OK) + break; + + if (ret != OK) + up_i2creset(_dev); + + } while (tries++ < _retries); return ret; } From 52ff9b7d433fed007a62fe2de375f685aa1b6b8a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:16:19 -0800 Subject: [PATCH 071/167] Use multi-part transactions rather than separate transfers to avoid racing between the ioctl and thread-side interfaces. --- apps/drivers/px4io/px4io.cpp | 60 +++++++++++++++++++----------------- 1 file changed, 32 insertions(+), 28 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2a860e5c14..e48c84774e 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -818,26 +818,28 @@ PX4IO::io_publish_pwm_outputs() int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - uint8_t buf[_max_transfer + 2]; - /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } - /* set page/offset address */ - buf[0] = page; - buf[1] = offset; + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; - /* copy data into our local transfer buffer */ - /* XXX we could use a msgv and two transactions, maybe? */ - unsigned datalen = num_values * sizeof(*values); - if (datalen > 0) - memcpy(&buf[2], values, datalen); + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); /* perform the transfer */ - int ret = transfer(buf, datalen + 2, nullptr, 0); + int ret = transfer(msgv, 2); if (ret != OK) debug("io_reg_set: error %d", ret); return ret; @@ -852,20 +854,22 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - uint8_t addr[2]; - int ret; + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; - /* send the address */ - addr[0] = page; - addr[1] = offset; - ret = transfer(addr, 2, nullptr, 0); - if (ret != OK) { - debug("io_reg_get: addr error %d", ret); - return ret; - } + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); - /* now read the data */ - ret = transfer(nullptr, 0, (uint8_t *)values, num_values * sizeof(*values)); + /* perform the transfer */ + int ret = transfer(msgv, 2); if (ret != OK) debug("io_reg_get: data error %d", ret); return ret; @@ -1093,13 +1097,13 @@ PX4IO::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - if (count > _max_actuators) - count = _max_actuators; + if (count > _max_actuators) + count = _max_actuators; if (count > 0) { int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); if (ret != OK) - return ret; -} + return ret; + } return count * 2; } @@ -1111,7 +1115,7 @@ namespace void test(void) { - int fd; + int fd; unsigned servo_count = 0; unsigned pwm_value = 1000; int direction = 1; From 5ee52138c4ac5c807888e3973099fd0f9a29aa59 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 08:47:21 +1100 Subject: [PATCH 072/167] px4io: ensure RC_OK status flag is set on good input --- apps/px4io/controls.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 0b2c199ee7..4782dc742c 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -198,8 +198,11 @@ controls_main(void) * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0) + if (assigned_channels == 0) { rc_input_lost = true; + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + } /* * Export the valid channel bitmap From c0a46c4b93d29a438a6207ce49589bddc026c900 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 10:58:30 +1100 Subject: [PATCH 073/167] px4io: fixed logical vs bitwise typo --- apps/px4io/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 4782dc742c..8cd5235b73 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -275,7 +275,7 @@ controls_main(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated | sbus_updated | ppm_updated) + if (dsm_updated || sbus_updated || ppm_updated) mixer_tick(); } else { From 7864176b5a1f2ac9cde4ac29ef19c5a72f87b3d3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:37:35 -0800 Subject: [PATCH 074/167] A couple of logic fixes from Tridge. --- apps/px4io/mixer.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 79b32009e4..848a88621a 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -91,10 +91,12 @@ mixer_tick(void) if ((hrt_absolute_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) { + debug("AP RX timeout"); + } r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; - debug("AP RX timeout"); } source = MIX_FAILSAFE; @@ -237,17 +239,17 @@ mixer_tick(void) up_pwm_servo_arm(true); mixer_servos_armed = true; - /* - * Update the servo outputs. - */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) - up_pwm_servo_set(i, r_page_servos[i]); - } 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 From 33c12d13ad42582745989794bf1d966d2a9e070f Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 17:07:58 -0800 Subject: [PATCH 075/167] Defer I2C bus resets for the first couple of retries to avoid transient slave errors causing massive retry spam. --- apps/drivers/device/i2c.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index d2cd5f19bd..a416801eb7 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -120,7 +120,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re struct i2c_msg_s msgv[2]; unsigned msgs; int ret; - unsigned tries = 0; + unsigned retry_count = 0; do { // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); @@ -154,13 +154,15 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re I2C_SETFREQUENCY(_dev, _frequency); ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + /* success */ if (ret == OK) break; - // reset the I2C bus to unwedge on error - up_i2creset(_dev); + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) + up_i2creset(_dev); - } while (tries++ < _retries); + } while (retry_count++ < _retries); return ret; @@ -170,15 +172,14 @@ int I2C::transfer(i2c_msg_s *msgv, unsigned msgs) { int ret; + unsigned retry_count = 0; /* force the device address into the message vector */ for (unsigned i = 0; i < msgs; i++) msgv[i].addr = _address; - unsigned tries = 0; do { - /* * I2C architecture means there is an unavoidable race here * if there are any devices on the bus with a different frequency @@ -187,13 +188,15 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) I2C_SETFREQUENCY(_dev, _frequency); ret = I2C_TRANSFER(_dev, msgv, msgs); + /* success */ if (ret == OK) break; - if (ret != OK) + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) up_i2creset(_dev); - } while (tries++ < _retries); + } while (retry_count++ < _retries); return ret; } From 621063ac084954bba11189c8566776aff25bfaeb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 17:10:04 -0800 Subject: [PATCH 076/167] Increase the number of I2C retries. --- apps/drivers/hmc5883/hmc5883.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 3734d77552..4a201b98c6 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -465,7 +465,7 @@ HMC5883::probe() read_reg(ADDR_ID_C, data[2])) debug("read_reg fail"); - _retries = 1; + _retries = 2; if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || From 666d3a401b04c259f930d6614130de97feed0034 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 18:55:26 -0800 Subject: [PATCH 077/167] Rename ::start to ::start_cycle to avoid confusion with the other start function. Only enable I2C retries on operations that have no side-effects. --- apps/drivers/ms5611/ms5611.cpp | 46 +++++++++++++++++++++++----------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index b215166dfe..9b1d8d5b71 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -162,12 +162,12 @@ private: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start_cycle(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop_cycle(); /** * Perform a poll cycle; collect from the previous measurement @@ -287,7 +287,7 @@ MS5611::MS5611(int bus) : MS5611::~MS5611() { /* make sure we are truly inactive */ - stop(); + stop_cycle(); /* free any existing reports */ if (_reports != nullptr) @@ -331,7 +331,11 @@ MS5611::probe() if ((OK == probe_address(MS5611_ADDRESS_1)) || (OK == probe_address(MS5611_ADDRESS_2))) { - _retries = 2; + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; return OK; } @@ -436,7 +440,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); + stop_cycle(); _measure_ticks = 0; return OK; @@ -458,7 +462,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -480,7 +484,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -508,11 +512,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop(); + stop_cycle(); delete[] _reports; _num_reports = arg; _reports = buf; - start(); + start_cycle(); return OK; } @@ -545,7 +549,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start() +MS5611::start_cycle() { /* reset the report ring and state machine */ @@ -558,7 +562,7 @@ MS5611::start() } void -MS5611::stop() +MS5611::stop_cycle() { work_cancel(HPWORK, &_work); } @@ -582,9 +586,17 @@ MS5611::cycle() /* perform collection */ ret = collect(); if (ret != OK) { - log("collection error %d", ret); + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + log("collection error %d", ret); + } /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } @@ -615,7 +627,7 @@ MS5611::cycle() if (ret != OK) { log("measure error %d", ret); /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } @@ -642,7 +654,11 @@ MS5611::measure() /* * Send the command to begin measuring. + * + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the write. */ + _retries = 0; ret = transfer(&cmd_data, 1, nullptr, 0); if (OK != ret) @@ -670,6 +686,8 @@ MS5611::collect() /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); + /* it's OK to retry on collection, as it has no side-effects */ + _retries = 3; ret = transfer(&cmd, 1, &data[0], 3); if (ret != OK) { perf_count(_comms_errors); From 981477c7856c5d7694561e0a13ebb0747518370e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 19:24:18 -0800 Subject: [PATCH 078/167] Re-order register page variables to match the order registers are defined in the protocol header. --- apps/px4io/protocol.h | 2 +- apps/px4io/registers.c | 156 ++++++++++++++++++++++++----------------- 2 files changed, 94 insertions(+), 64 deletions(-) diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 6dbe162295..54f2fa27cf 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -109,7 +109,7 @@ #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) +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ #define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ #define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */ diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 1c6fbf4fbf..c60491f93c 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -50,6 +50,81 @@ static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); /** + * 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 +}; + +/** + * PAGE 6 + * + * Raw ADC input. + */ +uint16_t r_page_adc[ADC_CHANNEL_COUNT]; + +/** + * PAGE 100 + * * Setup registers */ volatile uint16_t r_page_setup[] = @@ -72,85 +147,40 @@ volatile uint16_t r_page_setup[] = #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]; -/** - * Static configuration parameters. +/* + * PAGE 102 does not have a buffer. */ -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, -}; - -/** - * 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 -}; - -/** - * ADC input buffer. - */ -uint16_t r_page_adc[ADC_CHANNEL_COUNT]; - -/** - * Post-mixed actuator values. - */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; - -/** - * Servo PWM values - */ -uint16_t r_page_servos[IO_SERVO_COUNT]; - -/** - * Servo PWM values - */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; - -/** - * 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 -}; - -/** - * 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 103 + * * R/C channel input configuration. */ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +/* valid options excluding ENABLE */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE +/* + * 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) { - system_state.fmu_data_received_time = hrt_absolute_time(); switch (page) { From 6d0363faff7f5a59264198f04bae6f5b61c54510 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 19:43:23 -0800 Subject: [PATCH 079/167] Disarm IO at driver startup time. --- apps/drivers/px4io/px4io.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index e48c84774e..f948fec2c4 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -308,6 +308,12 @@ PX4IO::init() if (ret != OK) return ret; + /* + * Enable a couple of retries for operations to IO. + * + * Register read/write operations are intentionally idempotent + * so this is safe as designed. + */ _retries = 2; /* get some parameters */ @@ -326,6 +332,12 @@ PX4IO::init() if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; + /* dis-arm IO before touching anything */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, + PX4IO_P_SETUP_ARMING_ARM_OK | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE | + PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); + /* publish RC config to IO */ ret = io_set_rc_config(); if (ret != OK) { From 636e0cc56ad251318614f05d49b53f677456d2c1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 20:23:19 -0800 Subject: [PATCH 080/167] It looks like retrying reads from the ms5611 is not safe either. --- apps/drivers/ms5611/ms5611.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 9b1d8d5b71..231006ae3d 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -332,8 +332,7 @@ MS5611::probe() if ((OK == probe_address(MS5611_ADDRESS_1)) || (OK == probe_address(MS5611_ADDRESS_2))) { /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. + * Disable retries; the device gets confused if we retry some of the commands. */ _retries = 0; return OK; @@ -654,11 +653,7 @@ MS5611::measure() /* * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. */ - _retries = 0; ret = transfer(&cmd_data, 1, nullptr, 0); if (OK != ret) @@ -686,8 +681,6 @@ MS5611::collect() /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - /* it's OK to retry on collection, as it has no side-effects */ - _retries = 3; ret = transfer(&cmd, 1, &data[0], 3); if (ret != OK) { perf_count(_comms_errors); From 43ead720a711c8e29733eb4559f6ec69e17b69cb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 21:17:10 -0800 Subject: [PATCH 081/167] Now that we're mostly done with I2C, the old serial interface can be cleaned out. --- apps/px4io/comms.c | 329 --------------------------------------------- 1 file changed, 329 deletions(-) delete mode 100644 apps/px4io/comms.c diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c deleted file mode 100644 index e51c2771af..0000000000 --- a/apps/px4io/comms.c +++ /dev/null @@ -1,329 +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 comms.c - * - * FMU communication for the PX4IO module. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#define DEBUG -#include "px4io.h" - -#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */ -#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */ - -#define FMU_STATUS_INTERVAL 1000000 /* 100ms */ - -static int fmu_fd; -static hx_stream_t stream; -static struct px4io_report report; - -static void comms_handle_frame(void *arg, const void *buffer, size_t length); - -perf_counter_t comms_rx_errors; - -static void -comms_init(void) -{ - /* initialise the FMU interface */ - fmu_fd = open("/dev/ttyS1", O_RDWR); - stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); - - comms_rx_errors = perf_alloc(PC_COUNT, "rx_err"); - hx_stream_set_counters(stream, 0, 0, comms_rx_errors); - - /* default state in the report to FMU */ - report.i2f_magic = I2F_MAGIC; - - struct termios t; - - /* 115200bps, no parity, one stop bit */ - tcgetattr(fmu_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(fmu_fd, TCSANOW, &t); - - /* init the ADC */ - adc_init(); -} - -void -comms_main(void) -{ - comms_init(); - - struct pollfd fds; - fds.fd = fmu_fd; - fds.events = POLLIN; - debug("FMU: ready"); - - for (;;) { - /* wait for serial data, but no more than 10ms */ - poll(&fds, 1, 10); - - /* - * Pull bytes from FMU and feed them to the HX engine. - * Limit the number of bytes we actually process on any one iteration. - */ - if (fds.revents & POLLIN) { - char buf[32]; - ssize_t count = read(fmu_fd, buf, sizeof(buf)); - - for (int i = 0; i < count; i++) - hx_stream_rx(stream, buf[i]); - } - - /* - * Decide if it's time to send an update to the FMU. - */ - static hrt_abstime last_report_time; - hrt_abstime now, delta; - - /* should we send a report to the FMU? */ - now = hrt_absolute_time(); - delta = now - last_report_time; - - if ((delta > FMU_MIN_REPORT_INTERVAL) && - (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { - - system_state.fmu_report_due = false; - last_report_time = now; - - /* populate the report */ - for (unsigned i = 0; i < system_state.rc_channels; i++) { - report.rc_channel[i] = system_state.rc_channel_data[i]; - } - - 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; - } - } -} - -static void -comms_handle_config(const void *buffer, size_t length) -{ - const struct px4io_config *cfg = (struct px4io_config *)buffer; - - if (length != sizeof(*cfg)) - return; - - /* fetch the rc mappings */ - for (unsigned i = 0; i < 4; i++) { - system_state.rc_map[i] = cfg->rc_map[i]; - } - - /* fetch the rc channel attributes */ - for (unsigned i = 0; i < 4; i++) { - system_state.rc_min[i] = cfg->rc_min[i]; - system_state.rc_trim[i] = cfg->rc_trim[i]; - system_state.rc_max[i] = cfg->rc_max[i]; - system_state.rc_rev[i] = cfg->rc_rev[i]; - system_state.rc_dz[i] = cfg->rc_dz[i]; - } -} - -static void -comms_handle_command(const void *buffer, size_t length) -{ - const struct px4io_command *cmd = (struct px4io_command *)buffer; - - if (length != sizeof(*cmd)) - return; - - irqstate_t flags = irqsave(); - - /* fetch new PWM output values */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) - system_state.fmu_channel_data[i] = cmd->output_control[i]; - - /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ - if (system_state.arm_ok && !cmd->arm_ok) - system_state.armed = false; - - system_state.arm_ok = cmd->arm_ok; - system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; - system_state.manual_override_ok = cmd->manual_override_ok; - system_state.mixer_fmu_available = true; - system_state.fmu_data_received_time = hrt_absolute_time(); - - /* set PWM update rate if changed (after limiting) */ - uint16_t new_servo_rate = cmd->servo_rate; - - /* reject faster than 500 Hz updates */ - if (new_servo_rate > 500) { - new_servo_rate = 500; - } - - /* reject slower than 50 Hz updates */ - if (new_servo_rate < 50) { - new_servo_rate = 50; - } - - if (system_state.servo_rate != new_servo_rate) { - up_pwm_servo_set_rate(new_servo_rate); - system_state.servo_rate = new_servo_rate; - } - - /* - * update servo values immediately. - * the updates are done in addition also - * in the mainloop, since this function will only - * update with a connected FMU. - */ - mixer_tick(); - - /* handle relay state changes here */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { - if (system_state.relays[i] != cmd->relay_state[i]) { - switch (i) { - case 0: - POWER_ACC1(cmd->relay_state[i]); - break; - - case 1: - POWER_ACC2(cmd->relay_state[i]); - break; - - case 2: - POWER_RELAY1(cmd->relay_state[i]); - break; - - case 3: - POWER_RELAY2(cmd->relay_state[i]); - break; - } - } - - system_state.relays[i] = cmd->relay_state[i]; - } - - irqrestore(flags); -} - -static void -comms_handle_frame(void *arg, const void *buffer, size_t length) -{ - const uint16_t *type = (const uint16_t *)buffer; - - - /* make sure it's what we are expecting */ - if (length > 2) { - switch (*type) { - case F2I_MAGIC: - comms_handle_command(buffer, length); - break; - - case F2I_CONFIG_MAGIC: - comms_handle_config(buffer, length); - break; - - case F2I_MIXER_MAGIC: - mixer_handle_text(buffer, length); - break; - - default: - break; - } - } -} - From a196e73842259152595d524b150a611076ca91d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 18:11:59 +0100 Subject: [PATCH 082/167] Fixed arm ok flag typo --- apps/drivers/px4io/px4io.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f948fec2c4..2a0d72c310 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -541,9 +541,9 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; } if (vstatus.flag_external_manual_override_ok) { - set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE; + set |= PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK; } else { - clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE; + clear |= PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK; } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); @@ -1214,6 +1214,10 @@ monitor(void) int px4io_main(int argc, char *argv[]) { + /* check for sufficient number of arguments */ + if (argc < 2) + goto out; + if (!strcmp(argv[1], "start")) { if (g_dev != nullptr) @@ -1330,5 +1334,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); + out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'"); } From 167ec25c4f04ceed699a20fd6490db6cdf74e223 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 18:28:41 +0100 Subject: [PATCH 083/167] Fixed altitude jump issue, hunted down and fix by Andrew Tridgell. --- apps/drivers/ms5611/ms5611.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 231006ae3d..30166828a5 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -331,8 +331,9 @@ MS5611::probe() if ((OK == probe_address(MS5611_ADDRESS_1)) || (OK == probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; the device gets confused if we retry some of the commands. + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. */ _retries = 0; return OK; @@ -653,7 +654,11 @@ MS5611::measure() /* * Send the command to begin measuring. + * + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the write. */ + _retries = 0; ret = transfer(&cmd_data, 1, nullptr, 0); if (OK != ret) From a645a388bcb8c598f59d11c4b4a68265f64cb022 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 9 Feb 2013 00:53:51 -0800 Subject: [PATCH 084/167] Fix a sign error --- apps/px4io/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index c60491f93c..3824c64b06 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -370,7 +370,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) break; - if (conf[PX4IO_P_RC_CONFIG_MAX] < 2500) + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) break; if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) break; From 3c8da27d72e81da923106dcc667a5e644de90b86 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 9 Feb 2013 00:57:23 -0800 Subject: [PATCH 085/167] Fix a misleading comment. --- apps/px4io/i2c.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 2b8c502c7c..93dfd21dd1 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -174,7 +174,7 @@ i2c_interrupt(int irq, FAR void *context) i2c_rx_complete(); break; default: - /* spurious stop, ignore */ + /* not currently transferring - must be a new txn */ break; } direction = DIR_NONE; From aa16a63a10bb43d8614f01915cdf33996fe0d0e4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 11 Feb 2013 20:40:06 -0800 Subject: [PATCH 086/167] Fix assignment of relay GPIOs. --- apps/drivers/boards/px4io/px4io_internal.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h index f77d237a77..44bb98513b 100644 --- a/apps/drivers/boards/px4io/px4io_internal.h +++ b/apps/drivers/boards/px4io/px4io_internal.h @@ -74,8 +74,8 @@ #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_PIN13) -#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) +#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 ********************************************************************/ From 857fe5d405312508439a91493802e58e547d7940 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 09:20:11 +0100 Subject: [PATCH 087/167] Fixes to RC config transmission from Simon Wilks --- apps/px4io/registers.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 3824c64b06..815563daa1 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -339,7 +339,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; - unsigned index = 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) @@ -390,8 +390,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ } + break; + /* case PX4IO_RC_PAGE_CONFIG */ } default: From 01ada7f74f38f84fd19b74313c21f3dd13ca420d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 09:31:43 +0100 Subject: [PATCH 088/167] Fixed mixer transmission between FMU and IO --- apps/px4io/i2c.c | 2 +- apps/px4io/protocol.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 93dfd21dd1..235f8affa6 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -77,7 +77,7 @@ static void i2c_tx_complete(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; -static uint8_t rx_buf[64]; +static uint8_t rx_buf[68]; static unsigned rx_len; static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff }; diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 54f2fa27cf..6278e05d46 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -181,6 +181,7 @@ * 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 @@ -191,4 +192,5 @@ struct px4io_mixdata { char text[0]; /* actual text size may vary */ }; +#pragma pack(pop) From 163257f3bd4ebe13f6f3eda537793602035baf28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 09:33:52 +0100 Subject: [PATCH 089/167] Fixed scaling of RC calibration in IO driver, fixed interpretation of (odd, but APM-compatible) channel reverse flag --- apps/drivers/px4io/px4io.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2a0d72c310..c65855b834 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -594,34 +594,52 @@ PX4IO::io_set_rc_config() char pname[16]; float fval; + /* + * RC params are floats, but do only + * contain integer values. Do not scale + * or cast them, let the auto-typeconversion + * do its job here. + * Channels: 500 - 2500 + * Inverted flag: -1 (inverted) or 1 (normal) + */ + sprintf(pname, "RC%d_MIN", i + 1); param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_MIN] = FLOAT_TO_REG(fval); + regs[PX4IO_P_RC_CONFIG_MIN] = fval; sprintf(pname, "RC%d_TRIM", i + 1); param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_CENTER] = FLOAT_TO_REG(fval); + regs[PX4IO_P_RC_CONFIG_CENTER] = fval; sprintf(pname, "RC%d_MAX", i + 1); param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_MAX] = FLOAT_TO_REG(fval); + regs[PX4IO_P_RC_CONFIG_MAX] = fval; sprintf(pname, "RC%d_DZ", i + 1); param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_DEADZONE] = FLOAT_TO_REG(fval); + regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval; regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i]; regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; sprintf(pname, "RC%d_REV", i + 1); param_get(param_find(pname), &fval); - if (fval > 0) + + /* + * This has been taken for the sake of compatibility + * with APM's setup / mission planner: normal: 1, + * inverted: -1 + */ + if (fval < 0) { regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); - if (ret != OK) + if (ret != OK) { + log("RC config update failed"); break; + } offset += PX4IO_P_RC_CONFIG_STRIDE; } From d4ca6a29a19c96d359aa1458a4e6f3d9c86b01ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 22:19:53 +0100 Subject: [PATCH 090/167] Ensured that the mixer output obeys the FMU and IO armed state --- apps/px4io/mixer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 848a88621a..740d8aeb00 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -231,8 +231,9 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); + 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))); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ From 72de5b5ea7ac81a8b772bb9a2d11d0453fa0cc97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Feb 2013 07:43:51 +0100 Subject: [PATCH 091/167] Reworked manual override flag, reworked arming slightly. Pending testing --- apps/drivers/px4io/px4io.cpp | 6 +++--- apps/px4io/protocol.h | 5 ++--- apps/px4io/registers.c | 2 +- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c65855b834..51ff3fb837 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -335,7 +335,7 @@ PX4IO::init() /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); /* publish RC config to IO */ @@ -541,9 +541,9 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; } if (vstatus.flag_external_manual_override_ok) { - set |= PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK; + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { - clear |= PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK; + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 6278e05d46..29a287e45c 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -136,12 +136,11 @@ /* setup page */ #define PX4IO_PAGE_SETUP 100 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK (1 << 0) /* OK to switch to manual override */ #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 (1 << 2) /* request switch to manual override */ -#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) +#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_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 815563daa1..d0796ba295 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -284,7 +284,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_setup_features = value; /* update manual override state - disable if no longer OK */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && !(value & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && !(value & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)) r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; break; From 4595cc65b8bd30ba084783660a541c57c3921dbb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Feb 2013 07:43:51 +0100 Subject: [PATCH 092/167] Reworked manual override flag, reworked arming slightly. Pending testing --- apps/drivers/px4io/px4io.cpp | 6 +++--- apps/px4io/controls.c | 14 ++++---------- apps/px4io/protocol.h | 5 ++--- apps/px4io/registers.c | 8 +++----- 4 files changed, 12 insertions(+), 21 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c65855b834..51ff3fb837 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -335,7 +335,7 @@ PX4IO::init() /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); /* publish RC config to IO */ @@ -541,9 +541,9 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; } if (vstatus.flag_external_manual_override_ok) { - set |= PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK; + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { - clear |= PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK; + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 8cd5235b73..b4a18bae6b 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -245,11 +245,12 @@ controls_main(void) /* * Check for manual override. * - * The OVERRIDE_OK feature must be set, and we must have R/C input. + * 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_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { bool override = false; @@ -260,14 +261,7 @@ controls_main(void) * * XXX This should be configurable. */ - if ((r_rc_valid & (1 << 4)) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) - override = true; - - /* - * Check for an explicit manual override request from the AP. - */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) override = true; if (override) { diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 6278e05d46..29a287e45c 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -136,12 +136,11 @@ /* setup page */ #define PX4IO_PAGE_SETUP 100 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK (1 << 0) /* OK to switch to manual override */ #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 (1 << 2) /* request switch to manual override */ -#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) +#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_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 815563daa1..6229a6cc13 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -140,9 +140,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_IBATT_BIAS] = 0 }; -#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) +#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) + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -283,9 +283,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_SETUP_FEATURES_VALID; r_setup_features = value; - /* update manual override state - disable if no longer OK */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && !(value & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)) - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + /* no implemented feature selection at this point */ break; From bfecfbf5eef422e0ee069a17ff2482b352e29386 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 08:46:41 +1100 Subject: [PATCH 093/167] px4io: added isr_debug() this is useful for debugging px4io internals --- apps/px4io/px4io.c | 62 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index c3dacb2a66..8e0e52f04a 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -63,6 +63,54 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; +// global debug level for isr_debug() +volatile uint8_t debug_level = 0; +volatile uint32_t i2c_resets = 0; +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; +#define NUM_MSG 6 +static char msg[NUM_MSG][60]; + +/* + add a debug message to be printed on the console + */ +void isr_debug(uint8_t level, const char *fmt, ...) +{ + if (level > debug_level) { + 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 */ @@ -108,6 +156,8 @@ int user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + debug("debug_level=%u\n", (unsigned)debug_level); + /* start the i2c handler */ i2c_init(); @@ -116,10 +166,20 @@ int user_start(int argc, char *argv[]) /* run the mixer at 100Hz (for now...) */ /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ + uint8_t counter=0; for (;;) { poll(NULL, 0, 10); perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); + show_debug_messages(); + if (counter++ == 200) { + counter = 0; + isr_debug(0, "tick debug=%u status=0x%x resets=%u loop_resets=%u", + (unsigned)debug_level, + (unsigned)r_status_flags, + (unsigned)i2c_resets, + (unsigned)i2c_loop_resets); + } } -} \ No newline at end of file +} From 6eb69b07a8fd44fff1bfcf77bf4622f4dd044eef Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 11:06:13 +1100 Subject: [PATCH 094/167] Merged debug level commits from Tridge --- apps/px4io/i2c.c | 41 ++++++++++++++++++++++++++++++++++------- apps/px4io/px4io.c | 27 ++++++++++++++++++++++----- apps/px4io/px4io.h | 9 +++++++++ 3 files changed, 65 insertions(+), 12 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 235f8affa6..4485daa5b1 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -65,10 +65,6 @@ #define rTRISE REG(STM32_I2C_TRISE_OFFSET) static int i2c_interrupt(int irq, void *context); -#ifdef DEBUG -static void i2c_dump(void); -#endif - static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); @@ -152,6 +148,39 @@ i2c_init(void) #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) { @@ -301,8 +330,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } -#ifdef DEBUG -static void +void i2c_dump(void) { debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); @@ -310,4 +338,3 @@ i2c_dump(void) debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); } -#endif \ No newline at end of file diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 8e0e52f04a..b7e6ccb215 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -63,11 +63,12 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -// global debug level for isr_debug() +/* global debug level for isr_debug() */ volatile uint8_t debug_level = 0; -volatile uint32_t i2c_resets = 0; volatile uint32_t i2c_loop_resets = 0; +struct hrt_call loop_overtime_call; + /* a set of debug buffers to allow us to send debug information from ISRs @@ -111,6 +112,19 @@ static void show_debug_messages(void) } } +/* + catch I2C lockups + */ +static void loop_overtime(void *arg) +{ + debug("RESETTING\n"); + i2c_loop_resets++; + i2c_dump(); + i2c_reset(); + hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL); +} + + int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ @@ -152,7 +166,6 @@ int user_start(int argc, char *argv[]) (main_t)controls_main, NULL); - struct mallinfo minfo = mallinfo(); debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); @@ -168,6 +181,11 @@ int user_start(int argc, char *argv[]) /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ uint8_t counter=0; for (;;) { + /* + if we are not scheduled for 100ms then reset the I2C bus + */ + hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL); + poll(NULL, 0, 10); perf_begin(mixer_perf); mixer_tick(); @@ -175,10 +193,9 @@ int user_start(int argc, char *argv[]) show_debug_messages(); if (counter++ == 200) { counter = 0; - isr_debug(0, "tick debug=%u status=0x%x resets=%u loop_resets=%u", + isr_debug(1, "tick debug=%u status=0x%x resets=%u", (unsigned)debug_level, (unsigned)r_status_flags, - (unsigned)i2c_resets, (unsigned)i2c_loop_resets); } } diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index c90db5d780..92d55a47c5 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -176,3 +176,12 @@ 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); From 598622a00f71fac330c7bc919382466cfa059601 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 Feb 2013 18:16:29 +0100 Subject: [PATCH 095/167] Slightly adjusted battery voltage measurement after calibration against B&K Precision lab supply with beefy wiring. Needs more cross-validation. --- apps/px4io/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 6229a6cc13..f14cd8309d 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -457,7 +457,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); - unsigned mV = (4150 + (counts * 46)) / 10; + 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; From caade93ae409595352db40d62828009f93752346 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Feb 2013 14:28:54 +1100 Subject: [PATCH 096/167] px4io: enable signals signals will be used to wakeup the mixer on a new set of pwm values --- nuttx/configs/px4io/io/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index d2a4f38487..ccb5b1ec44 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -400,7 +400,7 @@ CONFIG_SCHED_ATEXIT=n CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_SIGNALS=n CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y From 4216982d54a858cb8ad8dbf68d2b7499348cd5e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 Feb 2013 21:28:43 +0100 Subject: [PATCH 097/167] Made timeouts configurable, untested --- nuttx/arch/arm/src/stm32/stm32_i2c.c | 2 +- nuttx/configs/px4fmu/nsh/defconfig | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index a66ac7e290..6332759068 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -2013,7 +2013,7 @@ int up_i2creset(FAR struct i2c_dev_s * dev) /* Give up if we have tried too hard */ - if (clock_count++ > 1000) + if (clock_count++ > CONFIG_STM32_I2CTIMEOTICKS) { goto out; } diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index fd783dec5d..b579096efb 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -372,7 +372,8 @@ CONFIG_I2C_POLLED=y CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n CONFIG_I2C_RESET=y - +# XXX fixed per-transaction timeout +CONFIG_STM32_I2CTIMEOMS=10 # XXX re-enable after integration testing From a33f314a2500e207d7aa566e2ea4fe8ace7e6300 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 14:47:43 +1100 Subject: [PATCH 098/167] More output --- apps/px4io/px4io.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index b7e6ccb215..e50022a0ca 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -193,9 +193,11 @@ int user_start(int argc, char *argv[]) show_debug_messages(); if (counter++ == 200) { counter = 0; - isr_debug(1, "tick debug=%u status=0x%x resets=%u", + isr_debug(1, "tick debug=%u status=0x%x arming=0x%x features=0x%x resets=%u", (unsigned)debug_level, (unsigned)r_status_flags, + (unsigned)r_setup_arming, + (unsigned)r_setup_features, (unsigned)i2c_loop_resets); } } From 04bea8678e98d58145d18c44bd3069433f528019 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 08:16:39 +1100 Subject: [PATCH 099/167] Merged debuglevel command from Tridge --- apps/drivers/drv_pwm_output.h | 3 +++ apps/drivers/px4io/px4io.cpp | 28 +++++++++++++++++++++++++++- apps/px4io/protocol.h | 1 + apps/px4io/px4io.h | 1 + apps/px4io/registers.c | 5 +++++ 5 files changed, 37 insertions(+), 1 deletion(-) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 23883323e2..64bb540b42 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -106,6 +106,9 @@ ORB_DECLARE(output_pwm); /** get the number of servos in *(unsigned *)arg */ #define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) +/** set debug level for servo IO */ +#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 51ff3fb837..8bcf4bacbf 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1019,6 +1019,11 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(unsigned *)arg = _max_actuators; break; + case PWM_SERVO_SET_DEBUG: + /* set the debug level */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); + break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): { unsigned channel = cmd - PWM_SERVO_SET(0); @@ -1287,6 +1292,27 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "debug")) { + if (argc <= 2) { + printf("usage: px4io debug LEVEL\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + uint8_t level = atoi(argv[2]); + // we can cheat and call the driver directly, as it + // doesn't reference filp in ioctl() + int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); + if (ret != 0) { + printf("SET_DEBUG failed - %d\n", ret); + exit(1); + } + printf("SET_DEBUG %u OK\n", (unsigned)level); + exit(0); + } + /* note, stop not currently implemented */ if (!strcmp(argv[1], "update")) { @@ -1353,5 +1379,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug' or 'update'"); } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 29a287e45c..f64b82ad17 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -149,6 +149,7 @@ #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 */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 92d55a47c5..9c5989df7b 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -185,3 +185,4 @@ extern void isr_debug(uint8_t level, const char *fmt, ...); void i2c_dump(void); void i2c_reset(void); + diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index f14cd8309d..0146084167 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -330,6 +330,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) POWER_ACC2(value & (1 << 3) ? 1 : 0); break; + case PX4IO_P_SETUP_SET_DEBUG: + debug_level = value; + isr_debug(0, "set debug %u\n", (unsigned)debug_level); + break; + default: return -1; } From 6bd18e46bb3b250213bb64d9b0da2e71ddc912ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 15:25:59 +0100 Subject: [PATCH 100/167] Disable signals again, it is too early (needs NuttX merge) --- nuttx/configs/px4io/io/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index ccb5b1ec44..d2a4f38487 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -400,7 +400,7 @@ CONFIG_SCHED_ATEXIT=n CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y From 5d92e0195fde9905d8d92fb22e41b52ce6b3f523 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 15:32:46 +0100 Subject: [PATCH 101/167] Re-enabled signals --- nuttx/configs/px4io/io/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 78e868b4f3..5db94c6f11 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -401,7 +401,7 @@ CONFIG_SCHED_ATEXIT=n CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_SIGNALS=n CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y From 038037d67656b92947a976cf647dfd4f748ec3ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 16:06:33 +0100 Subject: [PATCH 102/167] Allow to in-air restore the FMU and IO arming state if only one of the two fails --- apps/commander/commander.c | 7 ++ apps/drivers/px4io/px4io.cpp | 177 +++++++++++++++++++++++++++--- apps/px4io/protocol.h | 42 +++---- apps/px4io/registers.c | 15 ++- apps/uORB/topics/vehicle_status.h | 7 +- 5 files changed, 207 insertions(+), 41 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b1bc0f9b5..f917b7275d 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1260,6 +1260,8 @@ int commander_thread_main(int argc, char *argv[]) param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); /* welcome user */ warnx("I am in command now!\n"); @@ -1444,6 +1446,7 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + /* update parameters */ if (!current_status.flag_system_armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { @@ -1460,6 +1463,10 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_external_manual_override_ok = true; } + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + } else { warnx("ARMED, rejecting sys type change\n"); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 8bcf4bacbf..069028c14e 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -243,6 +244,24 @@ private: */ int mixer_send(const char *buf, unsigned buflen); + /** + * Handle a status update from IO. + * + * Publish IO status information if necessary. + * + * @param status The status register + */ + int io_handle_status(uint16_t status); + + /** + * Handle an alarm update from IO. + * + * Publish IO alarm information if necessary. + * + * @param alarm The status register + */ + int io_handle_alarms(uint16_t alarms); + }; @@ -332,17 +351,113 @@ PX4IO::init() if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; - /* dis-arm IO before touching anything */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_ARM_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | - PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); + /* + * Check for IO flight state - if FMU was flagged to be in + * armed state, FMU is recovering from an in-air reset. + * Read back status and request the commander to arm + * in this case. + */ - /* publish RC config to IO */ - ret = io_set_rc_config(); - if (ret != OK) { - log("failed to update RC input config"); + uint16_t reg; + + /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®, sizeof(reg)); + if (ret != OK) return ret; + + if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) { + + /* WARNING: COMMANDER app/vehicle status must be initialized. + * If this fails (or the app is not started), worst-case IO + * remains untouched (so manual override is still available). + */ + + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + /* fill with initial values, clear updated flag */ + vehicle_status_s status; + uint64_t try_start_time = hrt_absolute_time(); + bool updated = false; + + /* keep checking for an update, ensure we got a recent state, + not something that was published a long time ago. */ + do { + orb_check(vstatus_sub, &updated); + + if (updated) { + /* got data, copy and exit loop */ + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + break; + } + + /* wait 10 ms */ + usleep(10000); + + /* abort after 10s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + log("failed to recover from in-air restart (1), aborting IO driver init."); + return 1; + } + + } while (true); + + /* send command to arm system via command API */ + vehicle_command_s cmd; + /* request arming */ + cmd.param1 = 1.0f; + cmd.param2 = 0; + cmd.param3 = 0; + cmd.param4 = 0; + cmd.param5 = 0; + cmd.param6 = 0; + cmd.param7 = 0; + cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; + cmd.target_system = status.system_id; + cmd.target_component = status.component_id; + cmd.source_system = status.system_id; + cmd.source_component = status.component_id; + /* ask to confirm command */ + cmd.confirmation = 1; + + /* send command once */ + (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + + /* spin here until IO's state has propagated into the system */ + do { + orb_check(vstatus_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + } + + /* wait 10 ms */ + usleep(10000); + + /* abort after 10s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + log("failed to recover from in-air restart (2), aborting IO driver init."); + return 1; + } + + /* keep waiting for state change for 10 s */ + } while (!status.flag_system_armed); + + /* regular boot, no in-air restart, init IO */ + } else { + + + /* dis-arm IO before touching anything */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, + PX4IO_P_SETUP_ARMING_ARM_OK | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); + + /* publish RC config to IO */ + ret = io_set_rc_config(); + if (ret != OK) { + log("failed to update RC input config"); + return ret; + } + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ @@ -646,6 +761,42 @@ PX4IO::io_set_rc_config() return ret; } +int +PX4IO::io_handle_status(uint16_t status) +{ + int ret = 1; + /** + * WARNING: This section handles in-air resets. + */ + + /* check for IO reset - force it back to armed if necessary */ + if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* set the arming flag */ + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED); + + /* set new status */ + _status = status; + _status &= PX4IO_P_STATUS_FLAGS_ARMED; + } else { + ret = 0; + + /* set new status */ + _status = status; + } + return ret; +} + +int +PX4IO::io_handle_alarms(uint16_t alarms) +{ + + /* XXX handle alarms */ + + + /* set new alarms state */ + _alarms = alarms; +} + int PX4IO::io_get_status() { @@ -657,12 +808,8 @@ PX4IO::io_get_status() if (ret != OK) return ret; - _status = regs[0]; - _alarms = regs[1]; - - /* XXX handle status */ - - /* XXX handle alarms */ + io_handle_status(regs[0]); + io_handle_alarms(regs[1]); /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index f64b82ad17..4f1b067bd2 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -64,9 +64,9 @@ * packed. */ -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 +#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)) @@ -76,7 +76,7 @@ #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) /* static configuration page */ -#define PX4IO_PAGE_CONFIG 0 +#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? */ @@ -88,11 +88,11 @@ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ /* dynamic status page */ -#define PX4IO_PAGE_STATUS 1 +#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 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 */ @@ -103,7 +103,7 @@ #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_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#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 */ @@ -115,29 +115,29 @@ #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 */ +#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 */ +#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_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_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 */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 100 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#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) */ @@ -152,13 +152,13 @@ #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 */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 102 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#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 */ @@ -170,10 +170,10 @@ #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 */ +#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 */ +#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. @@ -187,8 +187,8 @@ struct px4io_mixdata { #define F2I_MIXER_MAGIC 0x6d74 uint8_t action; -#define F2I_MIXER_ACTION_RESET 0 -#define F2I_MIXER_ACTION_APPEND 1 +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 char text[0]; /* actual text size may vary */ }; diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 0146084167..be3bebada9 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -290,11 +290,20 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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; - /* update arming state - disarm if no longer OK */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; break; case PX4IO_P_SETUP_PWM_RATES: diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5c..893e34537c 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -156,7 +156,9 @@ struct vehicle_status_s enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ - int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ + int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ + int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ + int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ /* system flags - these represent the state predicates */ @@ -171,7 +173,7 @@ struct vehicle_status_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; bool rc_signal_found_once; @@ -209,6 +211,7 @@ struct vehicle_status_s bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_valid_launch_position; /**< indicates a valid launch position */ + bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ }; /** From f689f0abb0832c3d68e462e291a7a4d6dd43e216 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 16:38:19 +0100 Subject: [PATCH 103/167] Fixed excessive debug buffer size --- apps/px4io/px4io.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index e50022a0ca..96f49492f2 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -77,8 +77,14 @@ struct hrt_call loop_overtime_call; static volatile uint32_t msg_counter; static volatile uint32_t last_msg_counter; static volatile uint8_t msg_next_out, msg_next_in; -#define NUM_MSG 6 -static char msg[NUM_MSG][60]; + +/* + * 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 @@ -193,7 +199,7 @@ int user_start(int argc, char *argv[]) show_debug_messages(); if (counter++ == 200) { counter = 0; - isr_debug(1, "tick debug=%u status=0x%x arming=0x%x features=0x%x resets=%u", + isr_debug(1, "tick dbg=%u stat=0x%x arm=0x%x feat=0x%x rst=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, From 56bf9855a8f8140a8a5edeeb08f4246249b27085 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 17:47:26 +0100 Subject: [PATCH 104/167] Finished and tested in-air restore of arming state, as long as both boards reset at the same time armings state is now retained --- apps/drivers/px4io/px4io.cpp | 27 +++++++++++++++++++-------- apps/px4io/protocol.h | 1 + apps/px4io/px4io.c | 16 ++++++++-------- apps/px4io/registers.c | 10 ++++++++++ 4 files changed, 38 insertions(+), 16 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 069028c14e..b2e0c6ee45 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -279,6 +279,8 @@ PX4IO::PX4IO() : _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), + _status(0), + _alarms(0), _task(-1), _task_should_exit(false), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), @@ -360,8 +362,8 @@ PX4IO::init() uint16_t reg; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®, sizeof(reg)); + /* get IO's last seen FMU state */ + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); if (ret != OK) return ret; @@ -392,8 +394,8 @@ PX4IO::init() /* wait 10 ms */ usleep(10000); - /* abort after 10s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } @@ -432,8 +434,8 @@ PX4IO::init() /* wait 10 ms */ usleep(10000); - /* abort after 10s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } @@ -770,19 +772,28 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) { + if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; _status &= PX4IO_P_STATUS_FLAGS_ARMED; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + + /* set the sync flag */ + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); + /* set new status */ + _status = status; + } else { ret = 0; /* set new status */ _status = status; } + return ret; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 4f1b067bd2..a957a9e79a 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -102,6 +102,7 @@ #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_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 */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 96f49492f2..c102bf479a 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -84,7 +84,7 @@ static volatile uint8_t msg_next_out, msg_next_in; * output. */ #define NUM_MSG 2 -static char msg[NUM_MSG][40]; +static char msg[NUM_MSG][50]; /* add a debug message to be printed on the console @@ -183,23 +183,23 @@ int user_start(int argc, char *argv[]) /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - /* run the mixer at 100Hz (for now...) */ + /* run the mixer at ~300Hz (for now...) */ /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ - uint8_t counter=0; + uint16_t counter=0; for (;;) { /* - if we are not scheduled for 100ms then reset the I2C bus + if we are not scheduled for 10ms then reset the I2C bus */ - hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL); + hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL); - poll(NULL, 0, 10); + poll(NULL, 0, 3); perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); show_debug_messages(); - if (counter++ == 200) { + if (counter++ == 800) { counter = 0; - isr_debug(1, "tick dbg=%u stat=0x%x arm=0x%x feat=0x%x rst=%u", + isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index be3bebada9..40bf724821 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -270,6 +270,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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; From 9f15f38e5705d73e1dfdf381c8d3b458a8a1557b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Feb 2013 14:31:26 +1100 Subject: [PATCH 105/167] Merged, removed unneeded line --- apps/px4io/px4io.c | 31 +++++++++++++++++++++++++++++++ apps/px4io/px4io.h | 18 +++++++++++------- apps/px4io/registers.c | 8 ++++++-- 3 files changed, 48 insertions(+), 9 deletions(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index fec5eed236..122f007543 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -69,6 +70,9 @@ volatile uint32_t i2c_loop_resets = 0; struct hrt_call loop_overtime_call; +// this allows wakeup of the main task via a signal +static pid_t daemon_pid; + /* a set of debug buffers to allow us to send debug information from ISRs @@ -130,9 +134,24 @@ static void loop_overtime(void *arg) hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL); } +static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) +{ + // nothing to do - we just want poll() to return +} + + +/* + wakeup the main task using a signal + */ +void daemon_wakeup(void) +{ + kill(daemon_pid, SIGUSR1); +} int user_start(int argc, char *argv[]) { + daemon_pid = getpid(); + /* run C++ ctors before we go any further */ up_cxxinitialize(); @@ -183,6 +202,18 @@ int user_start(int argc, char *argv[]) /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + /* + * setup a null handler for SIGUSR1 - we will use this for wakeup from poll() + */ + struct sigaction sa; + memset(&sa, 0, sizeof(sa)); + sa.sa_sigaction = wakeup_handler; + sigfillset(&sa.sa_mask); + sigdelset(&sa.sa_mask, SIGUSR1); + if (sigaction(SIGUSR1, &sa, NULL) != OK) { + debug("Failed to setup SIGUSR1 handler\n"); + } + /* run the mixer at ~300Hz (for now...) */ /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ uint16_t counter=0; diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 6944776a91..cd5977258d 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -143,30 +143,29 @@ extern struct sys_state_s system_state; 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 comms_main(void) __attribute__((noreturn)); 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. @@ -177,9 +176,14 @@ 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() */ +/** global debug level for isr_debug() */ extern volatile uint8_t debug_level; +/** + * Wake up mixer. + */ +void daemon_wakeup(void); + /* send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 40bf724821..23ad4aa884 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -198,10 +198,12 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num values++; } - /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; + + // wake up daemon to trigger mixer + daemon_wakeup(); break; /* handle raw PWM input */ @@ -218,9 +220,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num values++; } - /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; + + // wake up the main thread to trigger mixer + daemon_wakeup(); break; /* handle setup for servo failsafe values */ From d6c108d870034a8dfc328487dc3477738937894d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Feb 2013 16:20:23 +1100 Subject: [PATCH 106/167] px4fmu: added publication of input_rc ORB values this allows for PPM input with no IO board --- apps/drivers/px4fmu/fmu.cpp | 34 ++++++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 430d18c6d5..ed3635fc95 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -64,12 +64,14 @@ #include #include #include +#include #include #include #include #include +#include class PX4FMU : public device::CDev { @@ -338,6 +340,13 @@ PX4FMU::task_main() unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + log("starting"); /* loop until killed */ @@ -363,8 +372,9 @@ PX4FMU::task_main() _current_update_rate = _update_rate; } - /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 10); /* this would be bad... */ if (ret < 0) { @@ -429,6 +439,26 @@ PX4FMU::task_main() /* update PWM servo armed status if armed and not locked down */ up_pwm_servo_arm(aa.armed && !aa.lockdown); } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; i Date: Sun, 17 Feb 2013 16:43:45 +1100 Subject: [PATCH 107/167] px4fmu: add support for write() interface for PWM output this matches the PX4IO interface --- apps/drivers/px4fmu/fmu.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index ed3635fc95..c29fe0ba3d 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -85,6 +85,7 @@ public: ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); virtual int init(); @@ -651,6 +652,30 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) return ret; } +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +PX4FMU::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + uint16_t values[4]; + + if (count > 4) { + // we only have 4 PWM outputs on the FMU + count = 4; + } + + // allow for misaligned values + memcpy(values, buffer, count*2); + + for (uint8_t i=0; i Date: Tue, 22 Jan 2013 07:48:30 +1100 Subject: [PATCH 108/167] appconfig: disable mathlib and associated examples on APM these are far too large (777 kbyte) and we can't fit them with the ArduCopter flight code --- nuttx/configs/px4fmu/nsh/appconfig | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index a904d90ac5..cb9ddee5f4 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -46,14 +46,18 @@ CONFIGURED_APPS += systemlib CONFIGURED_APPS += systemlib/mixer # Math library +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += mathlib CONFIGURED_APPS += mathlib/CMSIS CONFIGURED_APPS += examples/math_demo +endif # Control library +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += controllib CONFIGURED_APPS += examples/control_demo CONFIGURED_APPS += examples/kalman_demo +endif # System utility commands CONFIGURED_APPS += systemcmds/reboot From 1670b8afe13ba6c845800228d1c8829aa1bf31c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 10:19:38 +1100 Subject: [PATCH 109/167] nshlib: added cmp command to nsh this is useful for startup scripts testing for auto-upgrade of add-on board firmware --- apps/nshlib/Kconfig | 4 ++ apps/nshlib/nsh.h | 3 ++ apps/nshlib/nsh_fscmds.c | 81 ++++++++++++++++++++++++++++++++++++++++ apps/nshlib/nsh_parse.c | 3 ++ 4 files changed, 91 insertions(+) diff --git a/apps/nshlib/Kconfig b/apps/nshlib/Kconfig index 92bc83cfd0..d7a7b8a991 100644 --- a/apps/nshlib/Kconfig +++ b/apps/nshlib/Kconfig @@ -55,6 +55,10 @@ config NSH_DISABLE_CP bool "Disable cp" default n +config NSH_DISABLE_CMP + bool "Disable cmp" + default n + config NSH_DISABLE_DD bool "Disable dd" default n diff --git a/apps/nshlib/nsh.h b/apps/nshlib/nsh.h index 23209dba52..83cf25aa7f 100644 --- a/apps/nshlib/nsh.h +++ b/apps/nshlib/nsh.h @@ -603,6 +603,9 @@ void nsh_usbtrace(void); # ifndef CONFIG_NSH_DISABLE_CP int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif +# ifndef CONFIG_NSH_DISABLE_CMP + int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif # ifndef CONFIG_NSH_DISABLE_DD int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif diff --git a/apps/nshlib/nsh_fscmds.c b/apps/nshlib/nsh_fscmds.c index f47dca896f..83717e416c 100644 --- a/apps/nshlib/nsh_fscmds.c +++ b/apps/nshlib/nsh_fscmds.c @@ -1232,3 +1232,84 @@ int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) } #endif #endif + + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_CMP +int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *path1 = NULL; + char *path2 = NULL; + int fd1 = -1, fd2 = -1; + int ret = ERROR; + unsigned total_read = 0; + + /* Get the full path to the two files */ + + path1 = nsh_getfullpath(vtbl, argv[1]); + if (!path1) + { + goto errout; + } + + path2 = nsh_getfullpath(vtbl, argv[2]); + if (!path2) + { + goto errout; + } + + /* Open the files for reading */ + fd1 = open(path1, O_RDONLY); + if (fd1 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout; + } + + fd2 = open(path2, O_RDONLY); + if (fd2 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout; + } + + for (;;) + { + char buf1[128]; + char buf2[128]; + + int nbytesread1 = read(fd1, buf1, sizeof(buf1)); + int nbytesread2 = read(fd2, buf2, sizeof(buf2)); + + if (nbytesread1 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO); + goto errout; + } + + if (nbytesread2 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO); + goto errout; + } + + total_read += nbytesread1>nbytesread2?nbytesread2:nbytesread1; + + if (nbytesread1 != nbytesread2 || memcmp(buf1, buf2, nbytesread1) != 0) + { + nsh_output(vtbl, "files differ: byte %u\n", total_read); + goto errout; + } + + if (nbytesread1 < sizeof(buf1)) break; + } + + ret = OK; + +errout: + if (fd1 != -1) close(fd1); + if (fd2 != -1) close(fd2); + return ret; +} +#endif +#endif diff --git a/apps/nshlib/nsh_parse.c b/apps/nshlib/nsh_parse.c index f679d9b320..4d8f04b23f 100644 --- a/apps/nshlib/nsh_parse.c +++ b/apps/nshlib/nsh_parse.c @@ -175,6 +175,9 @@ static const struct cmdmap_s g_cmdmap[] = # ifndef CONFIG_NSH_DISABLE_CP { "cp", cmd_cp, 3, 3, " " }, # endif +# ifndef CONFIG_NSH_DISABLE_CMP + { "cmp", cmd_cmp, 3, 3, " " }, +# endif #endif #if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) From 317515fb6a3d6e469681b53316d69c7669efdaf0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 14:16:09 +1100 Subject: [PATCH 110/167] px4io: added INAIR_RESTART enable/disable flags the autopilot code needs to know that in-air restart may happen, so it should be something that is enabled, rather than on by default. --- apps/drivers/drv_pwm_output.h | 6 ++++++ apps/drivers/px4io/px4io.cpp | 18 +++++++++++++++++- apps/px4io/protocol.h | 1 + apps/px4io/registers.c | 3 ++- 4 files changed, 26 insertions(+), 2 deletions(-) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 64bb540b42..f3f753ebe6 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm); /** set debug level for servo IO */ #define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4) +/** enable in-air restart */ +#define PWM_SERVO_INAIR_RESTART_ENABLE _IOC(_PWM_SERVO_BASE, 5) + +/** disable in-air restart */ +#define PWM_SERVO_INAIR_RESTART_DISABLE _IOC(_PWM_SERVO_BASE, 6) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index b2e0c6ee45..adb894371f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -367,7 +367,12 @@ PX4IO::init() if (ret != OK) return ret; - if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) { + /* + * in-air restart is only tried if the IO board reports it is + * already armed, and has been configured for in-air restart + */ + if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO @@ -450,6 +455,7 @@ PX4IO::init() /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK | + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); @@ -1164,6 +1170,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); break; + case PWM_SERVO_INAIR_RESTART_ENABLE: + /* set the 'in-air restart' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + break; + + case PWM_SERVO_INAIR_RESTART_DISABLE: + /* unset the 'in-air restart' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); + break; + case PWM_SERVO_SET_UPDATE_RATE: /* set the requested rate */ if ((arg >= 50) && (arg <= 400)) { diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index a957a9e79a..0ee6d2c4b4 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -142,6 +142,7 @@ #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_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 23ad4aa884..ec1980aaaf 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -142,7 +142,8 @@ volatile uint16_t r_page_setup[] = #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_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) From a68300941f4492553d865d63b20683adb292c2fb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 14:16:29 +1100 Subject: [PATCH 111/167] px4fmu: enable BINFS needed for APM startup --- nuttx/configs/px4fmu/nsh/defconfig | 1 + 1 file changed, 1 insertion(+) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 3fd27db113..5d6900ab35 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -779,6 +779,7 @@ CONFIG_NXFFS_MAXNAMLEN=32 CONFIG_NXFFS_TAILTHRESHOLD=2048 CONFIG_NXFFS_PREALLOCATED=y CONFIG_FS_ROMFS=y +CONFIG_FS_BINFS=y # # SPI-based MMC/SD driver From 3c6d6f0ef1cf67ebcb1c28f142da73f8c6fcf91d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 14:17:06 +1100 Subject: [PATCH 112/167] px4fmu: disable a bunch of code when built for APM this leaves us enough flash to fit APM --- nuttx/configs/px4fmu/nsh/appconfig | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index cb9ddee5f4..517399a051 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -87,6 +87,8 @@ CONFIGURED_APPS += mavlink_onboard CONFIGURED_APPS += commander CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors + +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control @@ -96,6 +98,7 @@ CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry +endif # Hacking tools #CONFIGURED_APPS += system/i2c From 4a15eef602528bb79a62838e033be989e5fa2b3f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Feb 2013 09:01:31 +1100 Subject: [PATCH 113/167] px4io: fixed signals for lower latency PWM output poll() is not interrupted by signals, whereas usleep() is --- apps/px4io/px4io.c | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 122f007543..56923a674b 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -214,28 +214,34 @@ int user_start(int argc, char *argv[]) debug("Failed to setup SIGUSR1 handler\n"); } - /* run the mixer at ~300Hz (for now...) */ - /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ - uint16_t counter=0; + /* + run the mixer at ~50Hz, using signals to run it early if + need be + */ + uint64_t last_debug_time = 0; for (;;) { /* - if we are not scheduled for 10ms then reset the I2C bus + if we are not scheduled for 30ms then reset the I2C bus */ - hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL); + hrt_call_after(&loop_overtime_call, 30000, (hrt_callout)loop_overtime, NULL); + + // we use usleep() instead of poll() as poll() is not + // interrupted by signals in nuttx, whereas usleep() is + usleep(20000); - poll(NULL, 0, 3); perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); + show_debug_messages(); - if (counter++ == 800) { - counter = 0; - isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u", + if (hrt_absolute_time() - last_debug_time > 1000000) { + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)i2c_loop_resets); + last_debug_time = hrt_absolute_time(); } } } From e896944adcce3d0d5e333186a76b35850e5f9bc9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Feb 2013 14:45:45 +1100 Subject: [PATCH 114/167] ms5611: try to measure the performance cost of I2C timeouts --- apps/drivers/ms5611/ms5611.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 30166828a5..44014d969b 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -144,6 +144,7 @@ private: orb_advert_t _baro_topic; perf_counter_t _sample_perf; + perf_counter_t _measure_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; @@ -274,6 +275,7 @@ MS5611::MS5611(int bus) : _msl_pressure(101325), _baro_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { @@ -647,6 +649,8 @@ MS5611::measure() { int ret; + perf_begin(_measure_perf); + /* * In phase zero, request temperature; in other phases, request pressure. */ @@ -664,6 +668,8 @@ MS5611::measure() if (OK != ret) perf_count(_comms_errors); + perf_end(_measure_perf); + return ret; } @@ -689,6 +695,7 @@ MS5611::collect() ret = transfer(&cmd, 1, &data[0], 3); if (ret != OK) { perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } From 4cde2754666cea6332bb8069da518d28226c9477 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Feb 2013 09:47:08 +0100 Subject: [PATCH 115/167] Switched I2C to interrupt driven mode --- nuttx/configs/px4fmu/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 5d6900ab35..130886aac2 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -369,7 +369,7 @@ CONFIG_I2C_WRITEREAD=y # I2C configuration # CONFIG_I2C=y -CONFIG_I2C_POLLED=y +CONFIG_I2C_POLLED=n CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n CONFIG_I2C_RESET=y From be408451779dc53220ec94499a7acbe5ff3b8e7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Feb 2013 12:19:03 +0100 Subject: [PATCH 116/167] Switched to debug statement which is more efficient regarding stack usage, only printing at debug level 2 or higher. --- apps/px4io/mixer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 93bd4470f2..3ae2a3115e 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -301,7 +301,7 @@ mixer_handle_text(const void *buffer, size_t length) px4io_mixdata *msg = (px4io_mixdata *)buffer; - debug("mixer text %u", length); + isr_debug(2, "mixer text %u", length); if (length < sizeof(px4io_mixdata)) return; @@ -310,14 +310,14 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: - debug("reset"); + isr_debug(2, "reset"); mixer_group.reset(); mixer_text_length = 0; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: - debug("append %d", length); + 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... */ @@ -330,7 +330,7 @@ mixer_handle_text(const void *buffer, size_t length) memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; - debug("buflen %u", mixer_text_length); + 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; @@ -342,7 +342,7 @@ mixer_handle_text(const void *buffer, size_t length) /* ideally, this should test resid == 0 ? */ r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; - debug("used %u", mixer_text_length - resid); + 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) From a704acc2a20936d7e6d6828ae0ddf2cf7dc3578b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Feb 2013 12:02:00 +0100 Subject: [PATCH 117/167] Out of memory warning, flash and RAM optimizations --- apps/px4io/controls.c | 1 - apps/px4io/dsm.c | 6 +-- apps/px4io/mixer.cpp | 67 +------------------------------- apps/px4io/px4io.c | 31 +++++++++++---- apps/px4io/sbus.c | 6 ++- nuttx/configs/px4io/io/defconfig | 6 +-- 6 files changed, 34 insertions(+), 83 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index b4a18bae6b..829cc76716 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -45,7 +45,6 @@ #include #include -//#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index f0e8e0f322..1719cf58cb 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -248,18 +248,18 @@ dsm_guess_format(bool reset) if ((votes11 == 1) && (votes10 == 0)) { channel_shift = 11; - debug("DSM: detected 11-bit format"); + debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { channel_shift = 10; - debug("DSM: detected 10-bit format"); + debug("DSM: 10-bit format"); return; } /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); dsm_guess_format(true); } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 3ae2a3115e..505bc8a699 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -157,71 +157,6 @@ mixer_tick(void) r_page_servos[i] = 0; } -#if 0 - /* if everything is ok */ - - if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { - /* we have recent control data from the FMU */ - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - } else if (system_state.rc_channels > 0) { - /* when override is on or the fmu is not available, but RC is present */ - control_count = system_state.rc_channels; - - sched_lock(); - - /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */ - rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1]; - rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1]; - rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1]; - rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1]; - //rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1]; - - /* get the remaining channels, no remapping needed */ - for (unsigned i = 4; i < system_state.rc_channels; i++) { - rc_channel_data[i] = system_state.rc_channel_data[i]; - } - - /* scale the control inputs */ - rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / - (float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000; - - if (rc_channel_data[THROTTLE] > 2000) { - rc_channel_data[THROTTLE] = 2000; - } - - if (rc_channel_data[THROTTLE] < 1000) { - rc_channel_data[THROTTLE] = 1000; - } - - // lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", - // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), - // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); - - control_values = &rc_channel_data[0]; - sched_unlock(); - } else { - /* we have no control input (no FMU, no RC) */ - - // XXX builtin failsafe would activate here - control_count = 0; - } - //lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); - - /* this is for multicopters, etc. where manual override does not make sense */ - } else { - /* if the fmu is available whe are good */ - if (system_state.mixer_fmu_available) { - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - /* we better shut everything off */ - } else { - control_count = 0; - } - } -#endif - /* * Decide whether the servos should be armed right now. * @@ -301,7 +236,7 @@ mixer_handle_text(const void *buffer, size_t length) px4io_mixdata *msg = (px4io_mixdata *)buffer; - isr_debug(2, "mixer text %u", length); + isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) return; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 56923a674b..b4135aba5a 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -83,12 +83,12 @@ 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 + * 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][50]; +static char msg[NUM_MSG][40]; /* add a debug message to be printed on the console @@ -127,7 +127,7 @@ static void show_debug_messages(void) */ static void loop_overtime(void *arg) { - debug("RESETTING\n"); + lowsyslog("I2C RESET\n"); i2c_loop_resets++; i2c_dump(); i2c_reset(); @@ -136,7 +136,7 @@ static void loop_overtime(void *arg) static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) { - // nothing to do - we just want poll() to return + /* nothing to do - we just want poll() to return */ } @@ -185,16 +185,31 @@ int user_start(int argc, char *argv[]) up_pwm_servo_init(0xff); /* start the flight control signal handler */ - task_create("FCon", + int ret = task_create("FCon", SCHED_PRIORITY_DEFAULT, 1024, (main_t)controls_main, NULL); struct mallinfo minfo = mallinfo(); - lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); - debug("debug_level=%u\n", (unsigned)debug_level); + /* not enough memory, lock down */ + if (ret != OK || 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); + } /* start the i2c handler */ i2c_init(); @@ -211,7 +226,7 @@ int user_start(int argc, char *argv[]) sigfillset(&sa.sa_mask); sigdelset(&sa.sa_mask, SIGUSR1); if (sigaction(SIGUSR1, &sa, NULL) != OK) { - debug("Failed to setup SIGUSR1 handler\n"); + lowsyslog("SIGUSR1 init fail\n"); } /* diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index d199a9361c..073ddeabae 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -53,7 +53,7 @@ #include "debug.h" #define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 18 +#define SBUS_INPUT_CHANNELS 16 static int sbus_fd = -1; @@ -239,7 +239,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* decode switch channels if data fields are wide enough */ - if (chancount > 17) { + 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) */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 5db94c6f11..87d5001615 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -475,12 +475,12 @@ CONFIG_ARCH_BZERO=n # timer structures to minimize dynamic allocations. Set to # zero for all dynamic allocations. # -CONFIG_MAX_TASKS=8 +CONFIG_MAX_TASKS=4 CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=4 +CONFIG_NPTHREAD_KEYS=2 CONFIG_NFILE_DESCRIPTORS=8 CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=32 +CONFIG_NAME_MAX=12 CONFIG_STDIO_BUFFER_SIZE=32 CONFIG_STDIO_LINEBUFFER=n CONFIG_NUNGET_CHARS=2 From 5aa5645fb060c13997dc6458b530acb551c6c53e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Feb 2013 12:02:58 +0100 Subject: [PATCH 118/167] Disabled MAVLink debug app --- nuttx/configs/px4fmu/nsh/appconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 289a721a1a..2bf3283c81 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -81,7 +81,7 @@ CONFIGURED_APPS += systemcmds/delay_test # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/debug_values -CONFIGURED_APPS += examples/px4_mavlink_debug +# CONFIGURED_APPS += examples/px4_mavlink_debug # Shared object broker; required by many parts of the system. CONFIGURED_APPS += uORB From 8c7e2546ed5222145a6d1745e77d01f7c21c24fc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 00:09:37 -0800 Subject: [PATCH 119/167] Simplify the PX4IO main loop to cut down on memory consumption. --- apps/px4io/controls.c | 361 ++++++++++++++++--------------- apps/px4io/px4io.c | 115 ++++------ apps/px4io/px4io.h | 8 +- apps/px4io/registers.c | 4 - nuttx/configs/px4io/io/defconfig | 6 +- 5 files changed, 227 insertions(+), 267 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 829cc76716..cad368ae43 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -39,7 +39,6 @@ #include #include -#include #include #include @@ -53,21 +52,18 @@ static bool ppm_input(uint16_t *values, uint16_t *num_values); -void -controls_main(void) -{ - struct pollfd fds[2]; +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 */ - fds[0].fd = dsm_init("/dev/ttyS0"); - fds[0].events = POLLIN; + dsm_init("/dev/ttyS0"); /* S.bus input */ - fds[1].fd = sbus_init("/dev/ttyS2"); - fds[1].events = POLLIN; - - ASSERT(fds[0].fd >= 0); - ASSERT(fds[1].fd >= 0); + sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { @@ -82,200 +78,207 @@ controls_main(void) r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } - for (;;) { - /* run this loop at ~100Hz */ - int result = poll(fds, 2, 10); + 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"); +} - ASSERT(result >= 0); +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. - */ + /* + * 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. + */ - 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_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); - 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_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. - */ - 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; + /* + * 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); + 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) { + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); + /* + * If we received a new frame from any of the RC sources, process it. + */ + if (dsm_updated || sbus_updated || ppm_updated) { - /* record a bitmask of channels assigned */ - unsigned assigned_channels = 0; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp = hrt_absolute_time(); - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - uint16_t raw = r_raw_rc_values[i]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - /* implement the deadzone */ - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } + uint16_t raw = r_raw_rc_values[i]; - /* constrain to min/max values */ - 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]; - - int16_t scaled = raw; - - /* adjust to zero-relative (-500..500) */ - scaled -= 1500; - - /* scale to fixed-point representation (-10000..10000) */ - scaled *= 20; - - ASSERT(scaled >= -15000); - ASSERT(scaled <= 15000); - - 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); - - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* implement the deadzone */ + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; + } + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; } - } - /* 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; - } + /* constrain to min/max values */ + 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]; - /* - * 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 { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - } + int16_t scaled = raw; - /* - * Export the valid channel bitmap - */ - r_rc_valid = assigned_channels; + /* adjust to zero-relative (-500..500) */ + scaled -= 1500; + + /* scale to fixed-point representation (-10000..10000) */ + scaled *= 20; + + ASSERT(scaled >= -15000); + ASSERT(scaled <= 15000); + + 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); + + 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 haven't seen any new control data in 200ms, assume we - * have lost input. - */ - if ((hrt_absolute_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. + * If we got an update with zero channels, treat it as + * a loss of input. * - * 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. + * This might happen if a protocol-based receiver returns an update + * that contains no channels that we have mapped. */ - 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; if the value is 'high' then the pilot has - * requested override. - * - * XXX This should be configurable. - */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (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; - } + if (assigned_channels == 0) { + rc_input_lost = true; + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; } + /* + * 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_absolute_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; if the value is 'high' then the pilot has + * requested override. + * + * XXX This should be configurable. + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (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; + } } } diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index b4135aba5a..5892646612 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -68,14 +68,8 @@ static struct hrt_call serial_dma_call; volatile uint8_t debug_level = 0; volatile uint32_t i2c_loop_resets = 0; -struct hrt_call loop_overtime_call; - -// this allows wakeup of the main task via a signal -static pid_t daemon_pid; - - /* - a set of debug buffers to allow us to send debug information from ISRs + * a set of debug buffers to allow us to send debug information from ISRs */ static volatile uint32_t msg_counter; @@ -91,9 +85,10 @@ static volatile uint8_t msg_next_out, msg_next_in; static char msg[NUM_MSG][40]; /* - add a debug message to be printed on the console + * add a debug message to be printed on the console */ -void isr_debug(uint8_t level, const char *fmt, ...) +void +isr_debug(uint8_t level, const char *fmt, ...) { if (level > debug_level) { return; @@ -107,9 +102,10 @@ void isr_debug(uint8_t level, const char *fmt, ...) } /* - show all pending debug messages + * show all pending debug messages */ -static void show_debug_messages(void) +static void +show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; @@ -122,36 +118,9 @@ static void show_debug_messages(void) } } -/* - catch I2C lockups - */ -static void loop_overtime(void *arg) +int +user_start(int argc, char *argv[]) { - lowsyslog("I2C RESET\n"); - i2c_loop_resets++; - i2c_dump(); - i2c_reset(); - hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL); -} - -static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) -{ - /* nothing to do - we just want poll() to return */ -} - - -/* - wakeup the main task using a signal - */ -void daemon_wakeup(void) -{ - kill(daemon_pid, SIGUSR1); -} - -int user_start(int argc, char *argv[]) -{ - daemon_pid = getpid(); - /* run C++ ctors before we go any further */ up_cxxinitialize(); @@ -184,18 +153,27 @@ int user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* start the flight control signal handler */ - int ret = task_create("FCon", - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)controls_main, - NULL); + /* 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 (ret != OK || minfo.mxordblk < 500) { + if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); bool phase = false; @@ -210,46 +188,33 @@ int user_start(int argc, char *argv[]) phase = !phase; usleep(300000); } +#endif - /* start the i2c handler */ - i2c_init(); - - /* add a performance counter for mixing */ - perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - - /* - * setup a null handler for SIGUSR1 - we will use this for wakeup from poll() + /* + * Run everything in a tight loop. */ - struct sigaction sa; - memset(&sa, 0, sizeof(sa)); - sa.sa_sigaction = wakeup_handler; - sigfillset(&sa.sa_mask); - sigdelset(&sa.sa_mask, SIGUSR1); - if (sigaction(SIGUSR1, &sa, NULL) != OK) { - lowsyslog("SIGUSR1 init fail\n"); - } - /* - run the mixer at ~50Hz, using signals to run it early if - need be - */ uint64_t last_debug_time = 0; for (;;) { - /* - if we are not scheduled for 30ms then reset the I2C bus - */ - hrt_call_after(&loop_overtime_call, 30000, (hrt_callout)loop_overtime, NULL); - // we use usleep() instead of poll() as poll() is not - // interrupted by signals in nuttx, whereas usleep() is - usleep(20000); + /* 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(); - if (hrt_absolute_time() - last_debug_time > 1000000) { + + /* post debug state at ~1Hz */ + if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", (unsigned)debug_level, (unsigned)r_status_flags, diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index cd5977258d..22993fb52f 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -170,7 +170,8 @@ extern uint16_t adc_measure(unsigned channel); * * Input functions return true when they receive an update from the RC controller. */ -extern void controls_main(void); +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); @@ -179,11 +180,6 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/** - * Wake up mixer. - */ -void daemon_wakeup(void); - /* send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index ec1980aaaf..5fb90b9b0c 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -203,8 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up daemon to trigger mixer - daemon_wakeup(); break; /* handle raw PWM input */ @@ -224,8 +222,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up the main thread to trigger mixer - daemon_wakeup(); break; /* handle setup for servo failsafe values */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 87d5001615..1145fb349f 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -401,11 +401,11 @@ CONFIG_SCHED_ATEXIT=n CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=n +CONFIG_DISABLE_POLL=y # # Misc libc settings @@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=800 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE= From a11a71ec9c1b04cd5ca515862605008038804c8d Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 10:54:22 -0800 Subject: [PATCH 120/167] Hotfix: discard NUL characters in readline, rather than faking EOF on the console. --- apps/system/readline/readline.c | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/apps/system/readline/readline.c b/apps/system/readline/readline.c index bac7eee8c5..a8240a62a9 100644 --- a/apps/system/readline/readline.c +++ b/apps/system/readline/readline.c @@ -126,7 +126,7 @@ static inline int readline_rawgetc(int infd) * error occurs). */ - do + for (;;) { /* Read one character from the incoming stream */ @@ -154,13 +154,21 @@ static inline int readline_rawgetc(int infd) { return -errcode; } + + continue; } + + else if (buffer == '\0') + { + /* Ignore NUL characters, since they look like EOF to our caller */ + + continue; + } + + /* Success, return the character that was read */ + + return (int)buffer; } - while (nread < 1); - - /* On success, return the character that was read */ - - return (int)buffer; } /**************************************************************************** From 8740349545ab7fea4cbb231535de234bb0939764 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Feb 2013 20:13:45 +0100 Subject: [PATCH 121/167] Removed 1 Hz output --- apps/examples/kalman_demo/KalmanNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index db851221b2..955e77b3e4 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -247,8 +247,8 @@ void KalmanNav::update() // output if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, miss #: %4d\n", - _navFrames / 10, _miss / 10); + //printf("nav: %4d Hz, miss #: %4d\n", + // _navFrames / 10, _miss / 10); _navFrames = 0; _miss = 0; } From 35369471db820ba79e9803d4a48ea74ad6843e86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Feb 2013 20:24:21 +0100 Subject: [PATCH 122/167] working on better status reporting, removed unneeded fake PWM generation from FMU --- apps/drivers/px4io/px4io.cpp | 32 +++++++++++++++++++++++--------- apps/sensors/sensors.cpp | 30 ------------------------------ 2 files changed, 23 insertions(+), 39 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index adb894371f..a06a2575eb 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -96,6 +96,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + void print_status(); + private: // XXX unsigned _max_actuators; @@ -459,7 +461,7 @@ PX4IO::init() PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); - /* publish RC config to IO */ + /* publish RC config to IO */ ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); @@ -1141,18 +1143,28 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) } while (buflen > 0); - debug("mixer upload OK"); - /* check for the mixer-OK flag */ - if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { + debug("mixer upload OK"); return 0; - - debug("mixer rejected by IO"); + } else { + debug("mixer rejected by IO"); + } /* load must have failed for some reason */ return -EINVAL; } +void +PX4IO::print_status() +{ + printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "LOST"); + if (_status & PX4IO_P_STATUS_FLAGS_RC_OK) { + printf("\tRC type:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? "S.BUS" : ((_status & PX4IO_P_STATUS_FLAGS_RC_DSM) ? "DSM" : "PPM")); + // printf("\tRC chans:\t%d\n", xxx); + } +} + int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { @@ -1294,7 +1306,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } default: - /* not a recognised value */ + /* not a recognized value */ ret = -ENOTTY; } @@ -1458,10 +1470,12 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) + if (g_dev != nullptr) { printf("[px4io] loaded\n"); - else + g_dev->print_status(); + } else { printf("[px4io] not loaded\n"); + } exit(0); } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9d..edff8828f7 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1074,36 +1074,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - /* fake low-level driver, directly pulling from driver variables */ - static orb_advert_t rc_input_pub = -1; - struct rc_input_values raw; - - raw.timestamp = ppm_last_valid_decode; - /* we are accepting this message */ - _ppm_last_valid = ppm_last_valid_decode; - - /* - * relying on two decoded channels is very noise-prone, - * in particular if nothing is connected to the pins. - * requiring a minimum of four channels - */ - if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - raw.values[i] = ppm_buffer[i]; - } - - raw.channel_count = ppm_decoded_channels; - - /* publish to object request broker */ - if (rc_input_pub <= 0) { - rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); - - } else { - orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); - } - } - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ bool rc_updated; From 923a7cc505971ab6d04115d18108152f62d46283 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:41:26 -0800 Subject: [PATCH 123/167] Add an interrupt-safe way of comparing a timestamp with the current time. Add an interrupt-safe way of storing the current time into a timestamp. --- apps/drivers/drv_hrt.h | 16 ++++++++++++++++ apps/drivers/stm32/drv_hrt.c | 30 ++++++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 3b493a81a8..2e30725bdc 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -91,6 +91,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts); */ __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); +/* + * Compute the delta between a timestamp taken in the past + * and now. + * + * This function is safe to use even if the timestamp is updated + * by an interrupt during execution. + */ +__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); + +/* + * Store the absolute time in an interrupt-safe fashion. + * + * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. + */ +__EXPORT extern hrt_absolute_time hrt_store_absolute_time(volatile hrt_abstime *now); + /* * Call callout(arg) after delay has elapsed. * diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index cd9cb45b1c..0df2a8b748 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -645,6 +645,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) 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_absolute_time +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. */ From f245d6b1a7edaa3b403007b704b4d54ecb7f3737 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:42:34 -0800 Subject: [PATCH 124/167] Use hrt_elapsed_time() in cases where we can't be sure the timestamp won't change under us. --- apps/px4io/adc.c | 2 +- apps/px4io/controls.c | 4 ++-- apps/px4io/mixer.cpp | 2 +- apps/px4io/px4io.h | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c index e06b269dc0..670b8d635d 100644 --- a/apps/px4io/adc.c +++ b/apps/px4io/adc.c @@ -154,7 +154,7 @@ adc_measure(unsigned channel) while (!(rSR & ADC_SR_EOC)) { /* never spin forever - this will give a bogus result though */ - if ((hrt_absolute_time() - now) > 1000) { + if (hrt_elapsed_time(&now) > 1000) { debug("adc timeout"); break; } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index cad368ae43..6b51647569 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -218,7 +218,7 @@ controls_tick() { * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ @@ -294,7 +294,7 @@ ppm_input(uint16_t *values, uint16_t *num_values) * If we have received a new PPM frame within the last 200ms, accept it * and then invalidate it. */ - if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { + if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) { /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 505bc8a699..77f28cd7a6 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -88,7 +88,7 @@ void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ - if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + 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) { diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 22993fb52f..7b4b07c2c7 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -105,12 +105,12 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ */ struct sys_state_s { - uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp; /** * Last FMU receive time, in microseconds since system boot */ - uint64_t fmu_data_received_time; + volatile uint64_t fmu_data_received_time; }; From 186d3297228e4fbf34bb71545d0cdbac08e78fb3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:47:56 -0800 Subject: [PATCH 125/167] Fix search-and-replace error. --- apps/drivers/drv_hrt.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 2e30725bdc..0a64d69c34 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -105,7 +105,7 @@ __EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ -__EXPORT extern hrt_absolute_time hrt_store_absolute_time(volatile hrt_abstime *now); +__EXPORT extern hrt_abstime_time hrt_store_absolute_time(volatile hrt_abstime *now); /* * Call callout(arg) after delay has elapsed. From ccbd5a6372ff2ec40b1f014571712c4554b01e71 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:48:52 -0800 Subject: [PATCH 126/167] No, really fix it this time. --- apps/drivers/drv_hrt.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 0a64d69c34..8a99eeca79 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -105,7 +105,7 @@ __EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ -__EXPORT extern hrt_abstime_time hrt_store_absolute_time(volatile hrt_abstime *now); +__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); /* * Call callout(arg) after delay has elapsed. From 3d53b1d551d2ff901b68503e55167be03123b067 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:50:57 -0800 Subject: [PATCH 127/167] Fix it here, too. --- apps/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 0df2a8b748..bb67d5e6d2 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -663,7 +663,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then) /* * Store the absolute time in an interrupt-safe fashion */ -hrt_absolute_time +hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) { irqstate_t flags = irqsave(); From 72603207a1b16b628f153d195788171054cfc047 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Feb 2013 21:12:25 +0100 Subject: [PATCH 128/167] Fixed formatting of status printing --- apps/drivers/px4io/px4io.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a06a2575eb..d695c4f825 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1158,11 +1158,13 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) void PX4IO::print_status() { - printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "LOST"); + printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "FAIL"); if (_status & PX4IO_P_STATUS_FLAGS_RC_OK) { printf("\tRC type:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? "S.BUS" : ((_status & PX4IO_P_STATUS_FLAGS_RC_DSM) ? "DSM" : "PPM")); // printf("\tRC chans:\t%d\n", xxx); } + //printf("\tRC Config:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_CONFIG_OK) ? "OK" : "FAIL"); + printf("\tFMU link:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_FMU_OK) ? "OK" : "FAIL"); } int From 858460c863ca496e774a616698acf0eb94431dea Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 13:40:46 -0800 Subject: [PATCH 129/167] Extended PX4IO status dump --- apps/drivers/px4io/px4io.cpp | 101 ++++++++++++++++++++++++++++++++--- 1 file changed, 93 insertions(+), 8 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index d695c4f825..2f5d4cf89a 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -281,14 +281,15 @@ PX4IO::PX4IO() : _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), - _status(0), - _alarms(0), _task(-1), _task_should_exit(false), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), + _status(0), + _alarms(0), _t_actuators(-1), _t_armed(-1), _t_vstatus(-1), + _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), @@ -814,6 +815,8 @@ PX4IO::io_handle_alarms(uint16_t alarms) /* set new alarms state */ _alarms = alarms; + + return 0; } int @@ -1158,13 +1161,95 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) void PX4IO::print_status() { - printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "FAIL"); - if (_status & PX4IO_P_STATUS_FLAGS_RC_OK) { - printf("\tRC type:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? "S.BUS" : ((_status & PX4IO_P_STATUS_FLAGS_RC_DSM) ? "DSM" : "PPM")); - // printf("\tRC chans:\t%d\n", xxx); + /* basic configuration */ + printf("protocol %u software %u bootloader %u buffer %uB\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + + /* status */ + printf("%u bytes free\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); + uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n", + flags, + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC")); + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + printf("alarms 0x%04x%s%s%s%s%s%s\n", + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : "")); + printf("vbatt %u ibatt %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); + printf("actuators"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf("\n"); + printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); + uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + printf("%d R/C inputs", raw_inputs); + for (unsigned i = 0; i < raw_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + printf("\n"); + uint16_t valid_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RC_VALID); + printf("valid R/C inputs 0x%04x", valid_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { + if (valid_inputs && (1 << i)) + printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); } - //printf("\tRC Config:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_CONFIG_OK) ? "OK" : "FAIL"); - printf("\tFMU link:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_FMU_OK) ? "OK" : "FAIL"); + printf("\n"); + uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); + printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); + + /* setup */ + printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + printf("arming 0x%04x%s%s%s\n", + ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? "ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? "MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? "VECTOR_FLIGHT_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? "INAIR_RESTART_OK" : "")); + printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); + printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); } int From e818bcbfc2a91b7716ef782d15f2b8bf9f644419 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 13:58:52 -0800 Subject: [PATCH 130/167] Fix a wrong register read for the mapped channel mask --- apps/drivers/px4io/px4io.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2f5d4cf89a..18350718ca 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1211,14 +1211,14 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); - printf("%d R/C inputs", raw_inputs); + printf("%d raw R/C inputs", raw_inputs); for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); printf("\n"); - uint16_t valid_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RC_VALID); - printf("valid R/C inputs 0x%04x", valid_inputs); + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); + printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { - if (valid_inputs && (1 << i)) + if (mapped_inputs && (1 << i)) printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); } printf("\n"); @@ -1231,11 +1231,12 @@ PX4IO::print_status() /* setup */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s\n", - ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? "ARM_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? "MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? "VECTOR_FLIGHT_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? "INAIR_RESTART_OK" : "")); + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), From 776cf6093c97a6bb4c8fe9543b91c7039708fc85 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:06:28 -0800 Subject: [PATCH 131/167] && -> & --- 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 18350718ca..2488f8022b 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1218,7 +1218,7 @@ PX4IO::print_status() uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { - if (mapped_inputs && (1 << i)) + if (mapped_inputs & (1 << i)) printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); } printf("\n"); From 93f6edfe64780f1132cf8cb44afe3c7f1477f94e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:30:56 -0800 Subject: [PATCH 132/167] Fix reporting of R/C input config --- apps/drivers/px4io/px4io.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2488f8022b..3872d7201f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1228,7 +1228,7 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); printf("\n"); - /* setup */ + /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s\n", @@ -1251,6 +1251,20 @@ PX4IO::print_status() for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } } int From f35c5d600aa1d936207e3e6988058093dccacdf7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:32:04 -0800 Subject: [PATCH 133/167] Don't mask out the enable bit when accepting R/C input config updates. --- apps/px4io/registers.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 5fb90b9b0c..5ec2f7258b 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -165,8 +165,8 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; */ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; -/* valid options excluding ENABLE */ -#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE +/* 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. From dc74eeb421bce204a3064bcc60d524bf3fb53ab2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:31:01 -0800 Subject: [PATCH 134/167] Report the control values from the FMU in the status output. Count them separately from the actuators. --- apps/drivers/px4io/px4io.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 3872d7201f..a3b9957dd5 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -101,6 +101,7 @@ public: private: // XXX unsigned _max_actuators; + unsigned _max_controls; unsigned _max_rc_input; unsigned _max_relays; unsigned _max_transfer; @@ -277,6 +278,7 @@ PX4IO *g_dev; PX4IO::PX4IO() : I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), + _max_controls(0), _max_rc_input(0), _max_relays(0), _max_transfer(16), /* sensible default */ @@ -342,6 +344,7 @@ PX4IO::init() /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); @@ -637,11 +640,11 @@ PX4IO::io_set_control_state() orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &controls); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators); + return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } int @@ -1247,9 +1250,9 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - printf("failsafe"); - for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); printf("\n"); for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; @@ -1265,6 +1268,10 @@ PX4IO::print_status() ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); } int From 3d9901dfaf687e375569cbc3256b818ff01721c6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:31:40 -0800 Subject: [PATCH 135/167] If we have seen control input from FMU, update the FMU_OK status flag. --- apps/px4io/mixer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 77f28cd7a6..ed74cb3d30 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -94,9 +94,13 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { debug("AP RX timeout"); } - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + + /* XXX this is questionable - vehicle may not make sense for direct control */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } source = MIX_FAILSAFE; From 345b1a091554c92aa2d3e8e8df2b91cba2431aa5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:55:38 -0800 Subject: [PATCH 136/167] Print mapped R/C inputs as signed values (since they are zero-relative) --- 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 a3b9957dd5..4006f88cf0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1222,7 +1222,7 @@ PX4IO::print_status() printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) - printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); + printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); From 6ac7e8b7e4662c297e02ffc43e2cd52126753fa2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:56:02 -0800 Subject: [PATCH 137/167] Scale R/C inputs around the preset center, not the nominal center. --- apps/px4io/controls.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 6b51647569..37872d3569 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -119,7 +119,6 @@ controls_tick() { ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - /* * In some cases we may have received a frame, but input has still * been lost. @@ -168,8 +167,8 @@ controls_tick() { int16_t scaled = raw; - /* adjust to zero-relative (-500..500) */ - scaled -= 1500; + /* adjust to zero-relative around center */ + scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; /* scale to fixed-point representation (-10000..10000) */ scaled *= 20; From e6228355557aa7e06551711fc5a5a2127ca683f3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 16:20:04 -0800 Subject: [PATCH 138/167] Bump the task stack up to 1200 bytes to give the mixer loader some headroom. This addresses the last reported issue with this branch. --- apps/px4io/controls.c | 2 +- nuttx/configs/px4io/io/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 37872d3569..21b4edcc37 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -167,7 +167,7 @@ controls_tick() { int16_t scaled = raw; - /* adjust to zero-relative around center */ + /* adjust to zero-relative around center (nominal -500..500) */ scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; /* scale to fixed-point representation (-10000..10000) */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 1145fb349f..bb937cf4ee 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=800 +CONFIG_USERMAIN_STACKSIZE=1200 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE= From 5cc1e30e4fea92f32004288d8511de7e63c0c506 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Feb 2013 08:31:43 +0100 Subject: [PATCH 139/167] Corrected assertion range --- apps/px4io/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 21b4edcc37..3cc7140de5 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -173,8 +173,8 @@ controls_tick() { /* scale to fixed-point representation (-10000..10000) */ scaled *= 20; - ASSERT(scaled >= -15000); - ASSERT(scaled <= 15000); + ASSERT(scaled >= -50000); + ASSERT(scaled <= 50000); if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; From 2284e668ebab8fd452ecb5ca86d386e599f19ff1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Feb 2013 08:53:00 +0100 Subject: [PATCH 140/167] Removed bound checking assertions --- apps/px4io/controls.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 3cc7140de5..dabdde0af9 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -173,9 +173,6 @@ controls_tick() { /* scale to fixed-point representation (-10000..10000) */ scaled *= 20; - ASSERT(scaled >= -50000); - ASSERT(scaled <= 50000); - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; From ca794265c6a33f5aa9de87d97a222ed331c59aec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Feb 2013 09:07:13 +0100 Subject: [PATCH 141/167] Fixed input indexing, stupid 1-based indices on the GCS side (MP/QGC) caused confusion --- apps/drivers/px4io/px4io.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 4006f88cf0..2611c4e9c7 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -694,21 +694,26 @@ PX4IO::io_set_rc_config() for (unsigned i = 0; i < _max_rc_input; i++) input_map[i] = -1; + /* + * NOTE: The indices for mapped channels are 1-based + * for compatibility reasons with existing + * autopilots / GCS'. + */ param_get(param_find("RC_MAP_ROLL"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 0; + input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 1; + input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 2; + input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 3; + input_map[ichan - 1] = 3; ichan = 4; for (unsigned i = 0; i < _max_rc_input; i++) From 6eca4ba462440c8b3d45a2e44ac1f2085554d638 Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 09:20:00 -0800 Subject: [PATCH 142/167] Maxbotix I2C Sonar Support --- apps/drivers/device/i2c.cpp | 24 + apps/drivers/device/i2c.h | 20 + apps/drivers/drv_range_finder.h | 81 +++ apps/drivers/mb12xx/Makefile | 42 ++ apps/drivers/mb12xx/mb12xx.cpp | 842 +++++++++++++++++++++++++++++ apps/uORB/objects_common.cpp | 3 + apps/uORB/topics/subsystem_info.h | 3 +- nuttx/configs/px4fmu/nsh/appconfig | 1 + 8 files changed, 1015 insertions(+), 1 deletion(-) create mode 100644 apps/drivers/drv_range_finder.h create mode 100644 apps/drivers/mb12xx/Makefile create mode 100644 apps/drivers/mb12xx/mb12xx.cpp diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index a416801eb7..c6102a6c11 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -114,6 +114,30 @@ I2C::probe() return OK; } +int +I2C::write(const uint8_t *send, unsigned send_len) +{ + int ret = OK; + + I2C_SETFREQUENCY(_dev, _frequency); + I2C_SETADDRESS(_dev, _address, 7); + ret = I2C_WRITE(_dev, send, send_len); + + return ret; +} + +int +I2C::read(uint8_t *recv, unsigned recv_len) +{ + int ret = OK; + + I2C_SETFREQUENCY(_dev, _frequency); + I2C_SETADDRESS(_dev, _address, 7); + ret = I2C_READ(_dev, recv, recv_len); + + return ret; +} + int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 66c34dd7c4..874b22301b 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -85,6 +85,26 @@ protected: */ virtual int probe(); + /** + * Perform an I2C write to the device. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int write(const uint8_t *send, unsigned send_len); + + /** + * Perform an I2C read from the device. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int read(uint8_t *recv, unsigned recv_len); + /** * Perform an I2C transaction to the device. * diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h new file mode 100644 index 0000000000..420011be5a --- /dev/null +++ b/apps/drivers/drv_range_finder.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 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 Rangefinder driver interface. + */ + +#ifndef _DRV_RANGEFINDER_H +#define _DRV_RANGEFINDER_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" + +/** + * accel report structure. Reads from the device must be in multiples of this + * structure. + */ +struct range_finder_report { + uint64_t timestamp; + float distance; /** in meters */ + uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ +}; + +/* + * ObjDev tag for raw accelerometer data. + */ +ORB_DECLARE(sensor_range_finder); + +/* + * ioctl() definitions + * + * Accelerometer drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _RANGEFINDERIOCBASE (0x7900) +#define __RANGEFINDERIOC(_n) (_IOC(_RANGEFINDERIOCBASE, _n)) + +/** set the minimum effective distance of the device */ +#define RANGEFINDERIOCSETMINIUMDISTANCE __RANGEFINDERIOC(1) + +/** set the maximum effective distance of the device */ +#define RANGEFINDERIOCSETMAXIUMDISTANCE __RANGEFINDERIOC(2) + + +#endif /* _DRV_RANGEFINDER_H */ diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile new file mode 100644 index 0000000000..b5b911be19 --- /dev/null +++ b/apps/drivers/mb12xx/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 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. +# +############################################################################ + +# +# Makefile to build the Maxbotix Sonar driver. +# + +APPNAME = mb12xx +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp new file mode 100644 index 0000000000..d7a3357c80 --- /dev/null +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -0,0 +1,842 @@ +/**************************************************************************** + * + * Copyright (C) 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 mb12xx.cpp + * @author Greg Hulands + * + * Driver for the Maxbotix sonar range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +/* Configuration Constants */ +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ + +/* MB12xx Registers addresses */ + +#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class MB12XX : public device::I2C +{ +public: + MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); + ~MB12XX(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + range_finder_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be bought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * and MB12XX_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); + +MB12XX::MB12XX(int bus, int address) : + I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + _min_distance(MB12XX_MIN_DISTANCE), + _max_distance(MB12XX_MAX_DISTANCE), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), + _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MB12XX::~MB12XX() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MB12XX::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct range_finder_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the range finder topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + + if (_range_finder_topic < 0) + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +MB12XX::probe() +{ + // TODO: take a range reading and see if it is between the min and max + + return OK; +} + +void +MB12XX::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +MB12XX::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +MB12XX::get_minimum_distance() +{ + return _min_distance; +} + +float +MB12XX::get_maximum_distance() +{ + return _max_distance; +} + +int +MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct range_finder_report *buf = new struct range_finder_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: + { + set_minimum_distance((float)arg); + return 0; + } + break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: + { + set_maximum_distance((float)arg); + return 0; + } + break; + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +MB12XX::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(MB12XX_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MB12XX::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd[] = {MB12XX_TAKE_RANGE_REG}; + ret = I2C::write(&cmd[0], 1); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +MB12XX::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = I2C::read(&val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].distance = si_units; + _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +MB12XX::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +MB12XX::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +MB12XX::cycle_trampoline(void *arg) +{ + MB12XX *dev = (MB12XX *)arg; + + dev->cycle(); +} + +void +MB12XX::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + USEC2TICK(MB12XX_CONVERSION_INTERVAL)); +} + +void +MB12XX::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace mb12xx +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MB12XX *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MB12XX(MB12XX_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +mb12xx_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + mb12xx::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + mb12xx::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + mb12xx::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mb12xx::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + mb12xx::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 3f3476f62a..1363751401 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -53,6 +53,9 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include ORB_DEFINE(sensor_baro, struct baro_report); +#include +ORB_DEFINE(sensor_range_finder, struct range_finder_report); + #include ORB_DEFINE(output_pwm, struct pwm_output_values); diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h index c3e039d66b..c415e832e0 100644 --- a/apps/uORB/topics/subsystem_info.h +++ b/apps/uORB/topics/subsystem_info.h @@ -71,7 +71,8 @@ enum SUBSYSTEM_TYPE SUBSYSTEM_TYPE_YAWPOSITION = 4096, SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, - SUBSYSTEM_TYPE_MOTORCONTROL = 65536 + SUBSYSTEM_TYPE_MOTORCONTROL = 65536, + SUBSYSTEM_TYPE_RANGEFINDER = 131072 }; /** diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 2bf3283c81..f1f70e2283 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -125,6 +125,7 @@ CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps +CONFIGURED_APPS += drivers/mb12xx # Testing stuff CONFIGURED_APPS += px4/sensors_bringup From 349af372d0823d3e7f8cbc0c247d2af81e7c44a9 Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 10:03:40 -0800 Subject: [PATCH 143/167] Changes from pull request feedback --- apps/drivers/device/i2c.cpp | 24 ------------------------ apps/drivers/device/i2c.h | 20 -------------------- apps/drivers/drv_range_finder.h | 4 ++-- apps/drivers/mb12xx/Makefile | 2 +- apps/drivers/mb12xx/mb12xx.cpp | 12 ++++++------ 5 files changed, 9 insertions(+), 53 deletions(-) diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index c6102a6c11..a416801eb7 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -114,30 +114,6 @@ I2C::probe() return OK; } -int -I2C::write(const uint8_t *send, unsigned send_len) -{ - int ret = OK; - - I2C_SETFREQUENCY(_dev, _frequency); - I2C_SETADDRESS(_dev, _address, 7); - ret = I2C_WRITE(_dev, send, send_len); - - return ret; -} - -int -I2C::read(uint8_t *recv, unsigned recv_len) -{ - int ret = OK; - - I2C_SETFREQUENCY(_dev, _frequency); - I2C_SETADDRESS(_dev, _address, 7); - ret = I2C_READ(_dev, recv, recv_len); - - return ret; -} - int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 874b22301b..472bf26abd 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -84,26 +84,6 @@ protected: * Check for the presence of the device on the bus. */ virtual int probe(); - - /** - * Perform an I2C write to the device. - * - * @param send Pointer to bytes to send. - * @param send_len Number of bytes to send. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int write(const uint8_t *send, unsigned send_len); - - /** - * Perform an I2C read from the device. - * - * @param send Pointer to bytes to send. - * @param send_len Number of bytes to send. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int read(uint8_t *recv, unsigned recv_len); /** * Perform an I2C transaction to the device. diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h index 420011be5a..0fe8564acf 100644 --- a/apps/drivers/drv_range_finder.h +++ b/apps/drivers/drv_range_finder.h @@ -57,14 +57,14 @@ struct range_finder_report { }; /* - * ObjDev tag for raw accelerometer data. + * ObjDev tag for raw range finder data. */ ORB_DECLARE(sensor_range_finder); /* * ioctl() definitions * - * Accelerometer drivers also implement the generic sensor driver + * Rangefinder drivers also implement the generic sensor driver * interfaces from drv_sensor.h */ diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile index b5b911be19..0d24057877 100644 --- a/apps/drivers/mb12xx/Makefile +++ b/apps/drivers/mb12xx/Makefile @@ -37,6 +37,6 @@ APPNAME = mb12xx PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +STACKSIZE = 2048 include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp index d7a3357c80..0e4712f5a9 100644 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -157,7 +157,7 @@ private: /** * Set the min and max distance thresholds if you want the end points of the sensors - * range to be bought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE * and MB12XX_MAX_DISTANCE */ void set_minimum_distance(float min); @@ -388,13 +388,13 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) case RANGEFINDERIOCSETMINIUMDISTANCE: { - set_minimum_distance((float)arg); + set_minimum_distance(*(float *)arg); return 0; } break; case RANGEFINDERIOCSETMAXIUMDISTANCE: { - set_maximum_distance((float)arg); + set_maximum_distance(*(float *)arg); return 0; } break; @@ -471,8 +471,8 @@ MB12XX::measure() /* * Send the command to begin a measurement. */ - uint8_t cmd[] = {MB12XX_TAKE_RANGE_REG}; - ret = I2C::write(&cmd[0], 1); + uint8_t cmd = MB12XX_TAKE_RANGE_REG; + ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { @@ -495,7 +495,7 @@ MB12XX::collect() perf_begin(_sample_perf); - ret = I2C::read(&val[0], 2); + ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { From d1e41f2c48a4ccf21634abe858454889778f7a32 Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 10:14:11 -0800 Subject: [PATCH 144/167] Missed the accel reference here --- apps/drivers/drv_range_finder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h index 0fe8564acf..03a82ec5d0 100644 --- a/apps/drivers/drv_range_finder.h +++ b/apps/drivers/drv_range_finder.h @@ -47,7 +47,7 @@ #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" /** - * accel report structure. Reads from the device must be in multiples of this + * range finder report structure. Reads from the device must be in multiples of this * structure. */ struct range_finder_report { From 160ac722bedaad0e8cb824b14f902785976e1d2b Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 10:16:04 -0800 Subject: [PATCH 145/167] Fix white space --- apps/drivers/device/i2c.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 472bf26abd..66c34dd7c4 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -84,7 +84,7 @@ protected: * Check for the presence of the device on the bus. */ virtual int probe(); - + /** * Perform an I2C transaction to the device. * From 42694a5736f0b4493318f0d1c03cab70752ddec7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 2 Mar 2013 22:17:43 -0800 Subject: [PATCH 146/167] Make some improvements to the ARMv7M fault decode logic. --- Debug/ARMv7M | 204 +++++++++++++++++++++++++++------------------------ 1 file changed, 108 insertions(+), 96 deletions(-) diff --git a/Debug/ARMv7M b/Debug/ARMv7M index 3d96da682d..803d964534 100644 --- a/Debug/ARMv7M +++ b/Debug/ARMv7M @@ -19,120 +19,132 @@ end define vecstate - set $icsr = *(uint32_t *)0xe000ed04 + set $icsr = *(unsigned *)0xe000ed04 set $vect = $icsr & 0x1ff set $pend = ($icsr & 0x1ff000) >> 12 - set $shcsr = *(uint32_t *)0xe000ed24 - set $cfsr = *(uint32_t *)0xe000ed28 + set $shcsr = *(unsigned *)0xe000ed24 + set $cfsr = *(unsigned *)0xe000ed28 set $mmfsr = $cfsr & 0xff set $bfsr = ($cfsr >> 8) & 0xff set $ufsr = ($cfsr >> 16) & 0xffff - set $hfsr = *(uint32_t *)0xe000ed2c - set $bfar = *(uint32_t *)0xe000ed38 - set $mmfar = *(uint32_t *)0xe000ed34 + set $hfsr = *(unsigned *)0xe000ed2c + set $bfar = *(unsigned *)0xe000ed38 + set $mmfar = *(unsigned *)0xe000ed34 - # XXX Currently, rather than look at $vect, we just decode the - # fault status registers directly. + if $vect < 15 - if $hfsr != 0 - printf "HardFault:" - if $hfsr & (1<<1) - printf " due to vector table read fault\n" + if $hfsr != 0 + printf "HardFault:" + if $hfsr & (1<<1) + printf " due to vector table read fault\n" + end + if $hfsr & (1<<30) + printf " forced due to escalated or disabled configurable fault (see below)\n" + end + if $hfsr & (1<<31) + printf " due to an unexpected debug event\n" + end end - if $hfsr & (1<<30) - printf " forced ue to escalated or disabled configurable fault (see below)\n" + if $mmfsr != 0 + printf "MemManage:" + if $mmfsr & (1<<5) + printf " during lazy FP state save" + end + if $mmfsr & (1<<4) + printf " during exception entry" + end + if $mmfsr & (1<<3) + printf " during exception return" + end + if $mmfsr & (1<<0) + printf " during data access" + end + if $mmfsr & (1<<0) + printf " during instruction prefetch" + end + if $mmfsr & (1<<7) + printf " accessing 0x%08x", $mmfar + end + printf "\n" end - if $hfsr & (1<<31) - printf " due to an unexpected debug event\n" + if $bfsr != 0 + printf "BusFault:" + if $bfsr & (1<<2) + printf " (imprecise)" + end + if $bfsr & (1<<1) + printf " (precise)" + end + if $bfsr & (1<<5) + printf " during lazy FP state save" + end + if $bfsr & (1<<4) + printf " during exception entry" + end + if $bfsr & (1<<3) + printf " during exception return" + end + if $bfsr & (1<<0) + printf " during instruction prefetch" + end + if $bfsr & (1<<7) + printf " accessing 0x%08x", $bfar + end + printf "\n" + end + if $ufsr != 0 + printf "UsageFault" + if $ufsr & (1<<9) + printf " due to divide-by-zero" + end + if $ufsr & (1<<8) + printf " due to unaligned memory access" + end + if $ufsr & (1<<3) + printf " due to access to disabled/absent coprocessor" + end + if $ufsr & (1<<2) + printf " due to a bad EXC_RETURN value" + end + if $ufsr & (1<<1) + printf " due to bad T or IT bits in EPSR" + end + if $ufsr & (1<<0) + printf " due to executing an undefined instruction" + end + printf "\n" + end + else + if $vect >= 15 + printf "Handling vector %u\n", $vect end end - if $mmfsr != 0 - printf "MemManage:" - if $mmfsr & (1<<5) - printf " during lazy FP state save" - end - if $mmfsr & (1<<4) - printf " during exception entry" - end - if $mmfsr & (1<<3) - printf " during exception return" - end - if $mmfsr & (1<<0) - printf " during data access" - end - if $mmfsr & (1<<0) - printf " during instruction prefetch" - end - if $mmfsr & (1<<7) - printf " accessing 0x%08x", $mmfar - end - printf "\n" - end - if $bfsr != 0 - printf "BusFault:" - if $bfsr & (1<<2) - printf " (imprecise)" - end - if $bfsr & (1<<1) - printf " (precise)" - end - if $bfsr & (1<<5) - printf " during lazy FP state save" - end - if $bfsr & (1<<4) - printf " during exception entry" - end - if $bfsr & (1<<3) - printf " during exception return" - end - if $bfsr & (1<<0) - printf " during instruction prefetch" - end - if $bfsr & (1<<7) - printf " accessing 0x%08x", $bfar - end - printf "\n" - end - if $ufsr != 0 - printf "UsageFault" - if $ufsr & (1<<9) - printf " due to divide-by-zero" - end - if $ufsr & (1<<8) - printf " due to unaligned memory access" - end - if $ufsr & (1<<3) - printf " due to access to disabled/absent coprocessor" - end - if $ufsr & (1<<2) - printf " due to a bad EXC_RETURN value" - end - if $ufsr & (1<<1) - printf " due to bad T or IT bits in EPSR" - end - if $ufsr & (1<<0) - printf " due to executing an undefined instruction" - end - printf "\n" - end - if ((uint32_t)$lr & 0xf0000000) == 0xf0000000 + if ((unsigned)$lr & 0xf0000000) == 0xf0000000 if ($lr & 1) - set $frame_ptr = (uint32_t *)$msp + printf "exception frame is on MSP\n" + #set $frame_ptr = (unsigned *)$msp + set $frame_ptr = (unsigned *)$sp else - set $frame_ptr = (uint32_t *)$psp + printf "exception frame is on PSP, backtrace may not be possible\n" + #set $frame_ptr = (unsigned *)$psp + set $frame_ptr = (unsigned *)$sp end - printf " r0: %08x r1: %08x r2: %08x r3: %08x\n, $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] + if $lr & 0x10 + set $fault_sp = $frame_ptr + (8 * 4) + else + set $fault_sp = $frame_ptr + (26 * 4) + end + + + printf " r0: %08x r1: %08x r2: %08x r3: %08x\n", $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] printf " r4: %08x r5: %08x r6: %08x r7: %08x\n", $r4, $r5, $r6, $r7 printf " r8: %08x r9: %08x r10: %08x r11: %08x\n", $r8, $r9, $r10, $r11 - printf " r12: $08x lr: %08x pc: %08xx PSR: %08x\n", $frame_ptr[4], $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] + printf " r12: %08x\n", $frame_ptr[4] + printf " sp: %08x lr: %08x pc: %08x PSR: %08x\n", $fault_sp, $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] # Swap to the context of the faulting code and try to print a backtrace set $saved_sp = $sp - if $lr & 0x10 - set $sp = $frame_ptr + (8 * 4) - else - set $sp = $frame_ptr + (26 * 4) - end + set $sp = $fault_sp set $saved_lr = $lr set $lr = $frame_ptr[5] set $saved_pc = $pc @@ -142,7 +154,7 @@ define vecstate set $lr = $saved_lr set $pc = $saved_pc else - printf "(not currently in exception state)\n" + printf "(not currently in exception handler)\n" end end From b526bab1748f8a74202caf42966a5a719bba28ad Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 4 Mar 2013 21:46:55 -0800 Subject: [PATCH 147/167] Remove extra spaces from mixers before processing them. This gives us some more working space on IO for mixer processing. --- apps/systemcmds/mixer/mixer.c | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index e2f7b5bd58..55c4f08362 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -117,7 +117,23 @@ load(const char *devname, const char *fname) if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) continue; - /* XXX an optimisation here would be to strip extra whitespace */ + /* compact whitespace in the buffer */ + char *t, *f; + for (f = buf; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) From ae98836db8948edbcf59333627b25f69df4127d4 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 6 Mar 2013 20:37:01 +0100 Subject: [PATCH 148/167] Correct RC config sanity checking and report back when RC config errors occur. --- apps/px4io/mixer.cpp | 8 +++--- apps/px4io/protocol.h | 1 + apps/px4io/px4io.c | 3 +++ apps/px4io/registers.c | 57 +++++++++++++++++++++++++++++------------- apps/px4io/safety.c | 7 +++++- 5 files changed, 55 insertions(+), 21 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed74cb3d30..0fba2cbe55 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -170,9 +170,11 @@ mixer_tick(void) * 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))); + 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)); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 0ee6d2c4b4..14d221b5b5 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -103,6 +103,7 @@ #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 */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 5892646612..a0e0002a64 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -153,6 +153,9 @@ user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); + /* initialise the registry space */ + registers_init(); + /* initialise the control inputs */ controls_init(); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 5ec2f7258b..dac09021db 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -179,6 +179,12 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; +void +registers_init(void) +{ + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; +} + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -390,27 +396,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* 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) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) - break; + 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) - break; - if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) - break; + 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 */ - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + 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 */ diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 540636e19d..5495d5ae12 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -176,12 +176,17 @@ heartbeat_blink(void *arg) 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; } From 5c12b6a91113e924f2264e1b0d04d6f865eb3c64 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 6 Mar 2013 22:52:19 +0100 Subject: [PATCH 149/167] Request result of rc config upload from IO --- apps/drivers/px4io/px4io.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2611c4e9c7..fee49b1aa0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -83,6 +83,7 @@ #include #include "uploader.h" +#include class PX4IO : public device::I2C @@ -771,9 +772,17 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); if (ret != OK) { - log("RC config update failed"); + log("rc config upload failed"); break; } + + /* check the IO initialisation flag */ + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK; + if (ret != OK) { + log("config for RC%d rejected by IO", i + 1); + break; + } + offset += PX4IO_P_RC_CONFIG_STRIDE; } From 8d1f80a9e8ef988949eed006995384800ac91e70 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 7 Mar 2013 01:03:38 +0100 Subject: [PATCH 150/167] Fix how we check for rc config init status --- apps/drivers/px4io/px4io.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index fee49b1aa0..8fb53295f4 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -777,8 +777,7 @@ PX4IO::io_set_rc_config() } /* check the IO initialisation flag */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK; - if (ret != OK) { + if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { log("config for RC%d rejected by IO", i + 1); break; } @@ -1195,7 +1194,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), @@ -1206,7 +1205,8 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC")); + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s\n", alarms, From 8f5dac3740c87636f1f000b7e67df6f8ad58822a Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 7 Mar 2013 01:47:02 +0100 Subject: [PATCH 151/167] Let's just init the status flag every time we send a config update --- apps/px4io/px4io.c | 3 --- apps/px4io/registers.c | 7 +------ 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index a0e0002a64..5892646612 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -153,9 +153,6 @@ user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* initialise the registry space */ - registers_init(); - /* initialise the control inputs */ controls_init(); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index dac09021db..d97fd8d86e 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -179,12 +179,6 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; -void -registers_init(void) -{ - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; -} - void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -389,6 +383,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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; From 4797c192be73fee2f99769a4063044c97632899e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 09:49:12 +0100 Subject: [PATCH 152/167] Fixed RC calibration scaling / assignment --- apps/px4io/controls.c | 45 +++++++++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index dabdde0af9..908334c478 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -147,32 +147,43 @@ controls_tick() { uint16_t raw = r_raw_rc_values[i]; - /* implement the deadzone */ - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } + int16_t scaled; - /* constrain to min/max values */ + /* + * 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]; - int16_t scaled = raw; + /* + * 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 center is min 0..20000, if center is max -20000..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 = 20.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])); - /* adjust to zero-relative around center (nominal -500..500) */ - scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 20.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])); - /* scale to fixed-point representation (-10000..10000) */ - scaled *= 20; + } 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; From c993ba5bbc9e9a2781d26a5837b5711298de45ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 10:27:55 +0100 Subject: [PATCH 153/167] Fixed minor scaling issue, throttle range still half --- apps/px4io/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 908334c478..b3b6398579 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -173,10 +173,10 @@ controls_tick() { * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 20.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])); + scaled = 20000.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 = 20.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])); + scaled = 20000.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 */ From ff5ca82c7546d5e6db69144b57fb2878c4585ddf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 11:45:23 +0100 Subject: [PATCH 154/167] Fixed throttle scaling issue, harmonized FMU and IO RC scaling code --- apps/px4io/controls.c | 7 ++++--- apps/sensors/sensors.cpp | 42 ++++++++++++++++++++++++++-------------- 2 files changed, 32 insertions(+), 17 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index b3b6398579..d678fd351e 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -164,7 +164,8 @@ controls_tick() { * * 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 center is min 0..20000, if center is max -20000..0). + * 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 @@ -173,10 +174,10 @@ controls_tick() { * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 20000.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])); + 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 = 20000.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])); + 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 */ diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 9b95c09397..8d04e6ad68 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1125,31 +1125,45 @@ Sensors::ppm_poll() /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { - /* scale around the mid point differently for lower and upper range */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (rc_input.values[i] < _parameters.min[i]) + rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) + rc_input.values[i] = _parameters.max[i]; + + /* + * 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), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..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 (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ _rc.chan[i].scaled = 0.0f; } - /* reverse channel if required */ - if (i == (int)_rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } - - } else { - _rc.chan[i].scaled *= _parameters.rev[i]; - } + _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) _rc.chan[i].scaled = 0.0f; } From a49382485094245b52ded5c3799a9bdd6b0fa832 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 18:06:20 +0100 Subject: [PATCH 155/167] Fixed wrong comment --- apps/px4io/controls.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index d678fd351e..2d99116f80 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -268,10 +268,11 @@ controls_tick() { bool override = false; /* - * Check mapped channel 5; if the value is 'high' then the pilot has + * 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. * - * XXX This should be configurable. */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) override = true; From ebac51cad8e144b64938e6726e26bdc23aaf45e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 19:47:43 +0100 Subject: [PATCH 156/167] Working on restart resilience, hunting down multi-load mixer issue (still present) --- apps/drivers/px4io/px4io.cpp | 38 ++++++++++++++++++++-------- apps/px4io/mixer.cpp | 16 +++++++++--- apps/px4io/registers.c | 7 +++++ apps/systemlib/mixer/mixer.cpp | 1 + apps/systemlib/mixer/mixer_group.cpp | 1 + 5 files changed, 49 insertions(+), 14 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 8fb53295f4..882ca9fed8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1556,7 +1556,7 @@ px4io_main(int argc, char *argv[]) errx(1, "already loaded"); /* create the driver - it will set g_dev */ - (void)new PX4IO; + (void)new PX4IO(); if (g_dev == nullptr) errx(1, "driver alloc failed"); @@ -1567,7 +1567,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if (argc > 2 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { if (argc > 2 + 1) { #warning implement this } else { @@ -1579,16 +1579,31 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); } + exit(0); + } if (!strcmp(argv[1], "status")) { @@ -1613,8 +1628,9 @@ px4io_main(int argc, char *argv[]) exit(1); } uint8_t level = atoi(argv[2]); - // we can cheat and call the driver directly, as it - // doesn't reference filp in ioctl() + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 0fba2cbe55..54584e6851 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -108,9 +108,11 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { + 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 */ + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ source = MIX_NONE; } else { @@ -239,6 +241,11 @@ 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; @@ -252,9 +259,12 @@ mixer_handle_text(const void *buffer, size_t length) 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; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index d97fd8d86e..511a47f8df 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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]; diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90c..df0dfe838d 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff2256..1dbc512cdb 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } } From e8e52afcc426491f5959e6f879f26c9211a88d4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 20:51:33 +0100 Subject: [PATCH 157/167] Added minimum set of IO MAVLink text messages, report critical errors such as in-air restarts --- apps/drivers/px4io/px4io.cpp | 19 +++++++++++++++++-- apps/mavlink/mavlink_log.h | 12 ++++++++++++ apps/px4io/px4io.c | 8 ++++++-- 3 files changed, 35 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 882ca9fed8..f9ffa6bcdb 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -82,6 +82,7 @@ #include #include +#include #include "uploader.h" #include @@ -112,6 +113,8 @@ private: volatile int _task; ///< worker task volatile bool _task_should_exit; + int _mavlink_fd; + perf_counter_t _perf_update; /* cached IO state */ @@ -286,6 +289,7 @@ PX4IO::PX4IO() : _update_interval(0), _task(-1), _task_should_exit(false), + _mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), @@ -302,6 +306,9 @@ PX4IO::PX4IO() : /* we need this potentially before it could be set in task_main */ g_dev = this; + /* open MAVLink text channel */ + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _debug_enabled = true; } @@ -355,6 +362,7 @@ PX4IO::init() (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -381,6 +389,8 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO * remains untouched (so manual override is still available). @@ -470,6 +480,7 @@ PX4IO::init() ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); return ret; } @@ -491,6 +502,8 @@ PX4IO::init() return -errno; } + mavlink_log_info(_mavlink_fd, "[IO] init ok"); + return OK; } @@ -1165,9 +1178,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { debug("mixer upload OK"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; } else { debug("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } /* load must have failed for some reason */ @@ -1484,7 +1499,7 @@ test(void) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); - if (ret != sizeof(servos)) + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { @@ -1567,7 +1582,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (argc > 2 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { if (argc > 2 + 1) { #warning implement this } else { diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 62e6f7ca07..233a76cb31 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -63,7 +63,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#else #define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#endif /** * Send a mavlink critical message. @@ -71,7 +75,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#else #define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#endif /** * Send a mavlink info message. @@ -79,7 +87,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#else #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#endif struct mavlink_logmessage { char text[51]; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 5892646612..0c48385231 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -215,12 +215,16 @@ user_start(int argc, char *argv[]) /* post debug state at ~1Hz */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", + + struct mallinfo minfo = mallinfo(); + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets); + (unsigned)i2c_loop_resets, + (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } } From cc628fbc27794fee52e3a6f33d091758ca1cb51a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 11:03:06 +0100 Subject: [PATCH 158/167] Add missing mixer ok check in override mode, clear FMU lost alarm when setting FMU_OK flag, print AP RX timeout in production mode as well --- apps/px4io/mixer.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed74cb3d30..65de864ec3 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -92,7 +92,7 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - debug("AP RX timeout"); + lowsyslog("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; @@ -101,6 +101,7 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; } source = MIX_FAILSAFE; @@ -123,7 +124,8 @@ mixer_tick(void) } 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_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; From 11cb9df05b3a0fb3312de59db2cf44238b10517c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 11:20:06 +0100 Subject: [PATCH 159/167] After the mb12xx driver was merged way too early, make the best out of it and fix up the init phase to the driver bails out if there is no sensor connected --- apps/drivers/mb12xx/mb12xx.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp index 0e4712f5a9..406c726fb6 100644 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -259,9 +259,7 @@ out: int MB12XX::probe() { - // TODO: take a range reading and see if it is between the min and max - - return OK; + return measure(); } void From 4b26d7aef4e1b1d714b55cfb7abb48adcfd8c975 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 9 Mar 2013 12:27:29 +0100 Subject: [PATCH 160/167] adding missing include --- apps/px4io/mixer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 092a35a443..ec69cdd64e 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include From 74bcf29c698ee9b6f8a7859d59c28f4a69a54e02 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 13:20:05 +0100 Subject: [PATCH 161/167] Refactored debug level into proper register, px4io status now correctly reads it. Added more of the missing alarms clear logic, alarms reporting now consistent. Adding missing sign change on mode switch, fixes override issue when attempting to switch to auto mode. Pending outdoor tests --- apps/px4io/controls.c | 4 +++- apps/px4io/px4io.c | 7 +++---- apps/px4io/registers.c | 8 +++++--- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 2d99116f80..b507078a17 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -213,7 +213,9 @@ controls_tick() { 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; } /* @@ -274,7 +276,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) + 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) { diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 0c48385231..9de37e1188 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -64,8 +64,7 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -/* global debug level for isr_debug() */ -volatile uint8_t debug_level = 0; +/* store i2c reset count XXX this should be a register, together with other error counters */ volatile uint32_t i2c_loop_resets = 0; /* @@ -90,7 +89,7 @@ static char msg[NUM_MSG][40]; void isr_debug(uint8_t level, const char *fmt, ...) { - if (level > debug_level) { + if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { return; } va_list ap; @@ -219,7 +218,7 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", - (unsigned)debug_level, + (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 511a47f8df..645c1565d7 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -137,7 +137,8 @@ volatile uint16_t r_page_setup[] = [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_IBATT_BIAS] = 0, + [PX4IO_P_SETUP_SET_DEBUG] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) @@ -201,6 +202,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num 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; @@ -351,8 +353,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_SET_DEBUG: - debug_level = value; - isr_debug(0, "set debug %u\n", (unsigned)debug_level); + 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: From 802d0ae2faa101b2a9eaef75f4019160faf250fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 21:07:29 +0100 Subject: [PATCH 162/167] Made dtors virtual, tested on IO and FMU --- apps/drivers/blinkm/blinkm.cpp | 2 +- apps/drivers/bma180/bma180.cpp | 2 +- apps/drivers/gps/gps.cpp | 2 +- apps/drivers/hil/hil.cpp | 2 +- apps/drivers/hmc5883/hmc5883.cpp | 2 +- apps/drivers/l3gd20/l3gd20.cpp | 2 +- apps/drivers/led/led.cpp | 2 +- apps/drivers/mb12xx/mb12xx.cpp | 2 +- apps/drivers/mpu6000/mpu6000.cpp | 2 +- apps/drivers/ms5611/ms5611.cpp | 2 +- apps/drivers/px4fmu/fmu.cpp | 2 +- apps/drivers/px4io/px4io.cpp | 2 +- apps/drivers/px4io/uploader.h | 2 +- apps/mavlink_onboard/mavlink.c | 2 +- apps/systemlib/mixer/mixer.h | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index fc929284c4..54c7d4266a 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -127,7 +127,7 @@ class BlinkM : public device::I2C { public: BlinkM(int bus, int blinkm); - ~BlinkM(); + virtual ~BlinkM(); virtual int init(); diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index 32eb5333ea..4409a8a9cb 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -126,7 +126,7 @@ class BMA180 : public device::SPI { public: BMA180(int bus, spi_dev_e device); - ~BMA180(); + virtual ~BMA180(); virtual int init(); diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 1350106539..e35bdb944a 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -86,7 +86,7 @@ class GPS : public device::CDev { public: GPS(const char* uart_path); - ~GPS(); + virtual ~GPS(); virtual int init(); diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index fe9b281f67..861ed79242 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -91,7 +91,7 @@ public: MODE_NONE }; HIL(); - ~HIL(); + virtual ~HIL(); virtual int ioctl(file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 4a201b98c6..8ab5682821 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -130,7 +130,7 @@ class HMC5883 : public device::I2C { public: HMC5883(int bus); - ~HMC5883(); + virtual ~HMC5883(); virtual int init(); diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index f2f585f42f..6227df72aa 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -152,7 +152,7 @@ class L3GD20 : public device::SPI { public: L3GD20(int bus, const char* path, spi_dev_e device); - ~L3GD20(); + virtual ~L3GD20(); virtual int init(); diff --git a/apps/drivers/led/led.cpp b/apps/drivers/led/led.cpp index 12d864be27..c7c45525e1 100644 --- a/apps/drivers/led/led.cpp +++ b/apps/drivers/led/led.cpp @@ -53,7 +53,7 @@ class LED : device::CDev { public: LED(); - ~LED(); + virtual ~LED(); virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp index 406c726fb6..9d0f6bddcf 100644 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -100,7 +100,7 @@ class MB12XX : public device::I2C { public: MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); - ~MB12XX(); + virtual ~MB12XX(); virtual int init(); diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 27e200e40b..ce7062046f 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -151,7 +151,7 @@ class MPU6000 : public device::SPI { public: MPU6000(int bus, spi_dev_e device); - ~MPU6000(); + virtual ~MPU6000(); virtual int init(); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 44014d969b..08420822a5 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -104,7 +104,7 @@ class MS5611 : public device::I2C { public: MS5611(int bus); - ~MS5611(); + virtual ~MS5611(); virtual int init(); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index c29fe0ba3d..8e13f7c62c 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -82,7 +82,7 @@ public: MODE_NONE }; PX4FMU(); - ~PX4FMU(); + virtual ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f9ffa6bcdb..90320d3810 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -91,7 +91,7 @@ class PX4IO : public device::I2C { public: PX4IO(); - ~PX4IO(); + virtual ~PX4IO(); virtual int init(); diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h index 915ee92597..f983d19811 100644 --- a/apps/drivers/px4io/uploader.h +++ b/apps/drivers/px4io/uploader.h @@ -46,7 +46,7 @@ class PX4IO_Uploader { public: PX4IO_Uploader(); - ~PX4IO_Uploader(); + virtual ~PX4IO_Uploader(); int upload(const char *filenames[]); diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 33ebe8600a..5a26855600 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -498,7 +498,7 @@ int mavlink_onboard_main(int argc, char *argv[]) mavlink_task = task_spawn("mavlink_onboard", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 6000 /* XXX probably excessive */, + 2048, mavlink_thread_main, (const char**)argv); exit(0); diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 00ddf15817..71386cba77 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -160,7 +160,7 @@ public: * @param control_cb Callback invoked when reading controls. */ Mixer(ControlCallback control_cb, uintptr_t cb_handle); - ~Mixer() {}; + virtual ~Mixer() {}; /** * Perform the mixing function. From a8a74fda96826f3fafa0a5ee4e3d8397ed4ca1e7 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 9 Mar 2013 21:58:30 +0100 Subject: [PATCH 163/167] Invert aileron actuator for correct aileron response in auto --- apps/controllib/fixedwing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index d9637b4a7a..b84a54fee5 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -322,7 +322,7 @@ void BlockMultiModeBacksideAutopilot::update() _att.roll, _att.pitch, _att.yaw, _att.rollspeed, _att.pitchspeed, _att.yawspeed ); - _actuators.control[CH_AIL] = - _backsideAutopilot.getAileron(); + _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); _actuators.control[CH_ELV] = - _backsideAutopilot.getElevator(); _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); From 921ef9178d58ff83dcad44d0584b4f71fb2cae6f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Mar 2013 00:16:55 +0100 Subject: [PATCH 164/167] Hotfix: Correctly publish servo outputs --- 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 90320d3810..791964087c 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1023,7 +1023,7 @@ PX4IO::io_publish_pwm_outputs() /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) - outputs.output[i] = REG_TO_FLOAT(ctl[i]); + outputs.output[i] = ctl[i]; outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ From c720a3238014e347fb84063a3fee2f9c812b3bf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Mar 2013 01:00:16 +0100 Subject: [PATCH 165/167] Hotfix: Correct channel order in HIL --- apps/mavlink/orb_listener.c | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 9f85d5801f..58c709aecb 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -511,28 +511,12 @@ l_actuator_outputs(struct listener *l) 0); } else { - - /* - * Catch the case where no rudder is in use and throttle is not - * on output four - */ - float rudder, throttle; - - if (act_outputs.noutputs < 4) { - rudder = 0.0f; - throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; - - } else { - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; - } - mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, + (act_outputs.output[2] - 1500.0f) / 600.0f, + (act_outputs.output[3] - 900.0f) / 1200.0f, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, From 1d444f80a3b9b575681e41b7a3a9b26a4b3d606d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Mar 2013 22:01:13 +0100 Subject: [PATCH 166/167] Fixed comment --- 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 791964087c..27c885ed7a 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -122,7 +122,7 @@ private: uint16_t _alarms; /* subscribed topics */ - int _t_actuators; ///< actuator output topic + int _t_actuators; ///< actuator controls topic int _t_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic From 424923271e5b8e802a3c082841eccea450326277 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Mar 2013 21:46:16 +0100 Subject: [PATCH 167/167] Hotfix: Throttle scaling in HIL --- apps/mavlink/orb_listener.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 58c709aecb..7c34fb4741 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -513,14 +513,14 @@ l_actuator_outputs(struct listener *l) } else { mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 600.0f, - (act_outputs.output[1] - 1500.0f) / 600.0f, - (act_outputs.output[2] - 1500.0f) / 600.0f, - (act_outputs.output[3] - 900.0f) / 1200.0f, - (act_outputs.output[4] - 1500.0f) / 600.0f, - (act_outputs.output[5] - 1500.0f) / 600.0f, - (act_outputs.output[6] - 1500.0f) / 600.0f, - (act_outputs.output[7] - 1500.0f) / 600.0f, + (act_outputs.output[0] - 1500.0f) / 500.0f, + (act_outputs.output[1] - 1500.0f) / 500.0f, + (act_outputs.output[2] - 1500.0f) / 500.0f, + (act_outputs.output[3] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1500.0f) / 500.0f, + (act_outputs.output[5] - 1500.0f) / 500.0f, + (act_outputs.output[6] - 1500.0f) / 500.0f, + (act_outputs.output[7] - 1500.0f) / 500.0f, mavlink_mode, 0); }