From 811790a14f567b1a58a4434c9fae0cf764e8fd2d Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 7 Jan 2013 21:33:04 -0800 Subject: [PATCH 001/105] 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^g&p6#lBa=Vxbjhy6hT1#QThT@-p|c4rnvK$aDd$m(WX690F5W@>gP%ywgU z4=e_v5^thL6O9MsMK4~AF?!^n2T#Tjy%_c8LA{|D4jRY2`eAwo3>^Fp{p!_w^}VX< zSFdXOJ~ybX9?0bFUKFRxd&<*=nL^pN3&Irt+IF#|{g*QU7=TKx<%b-Na7i>AUves@ z@=V#b%Y~w?`HF?YOsQns(^KUs$+et$>G1I*j1idx!=#|r_%tC}iINbL*n-=UnjHQj z@k8h8KOIb_e}X-HX<(c*Yx(gjox<>?hYwx-^;-aj7-O-wXu1W>7=yu*CIbwX-a+n0 z;x^KZ)D-}xm1ueq3@eo*TUyM+NT=GQS=I*fOk2N`#?lu>XW~a8QZ)b;`-PO(*sO{- zu4JN72V0Db<3KqSG!C;*xu!-BSwN{eGyq%LK{S)DTfxhgo+z^;dCI+zliI4|j{)tsL;Pw1MbNvzb%*p(bX0Ud~4S3#bN6~t| z5#pBonqIBdu5)ygy+%RWUNem9&Z^9{gvdDaGT0XLtUY0Fv-9N%)3wcQ_I~rszU>AB zV=doXvBih-tL=r&6_3}2!h^tX$NFlkZg6YUYlQi0r?3XW-oxAt+S4rp9DVGBJ!hAk z8E49#Dio)Rh2o4o8SoZ&L%!2~X0|e)=WA77uk-ru($dWCYT$acRXgCPn_<)U?BWAc zrTh0hCxg(UWLc>D?P`l7T=j#fH>_H+h~8-VZls)OERN32?X@RqQj5Od3hjm;*t3;+ z`>>jVy%6{{9)^Ap?)*PyJ?j8)xm#_5QD!CP{2=1%yXU8o%%tWAj8SWoxtVQYcd)yR z9dtSd0dzW@4uf$nH#j8nU4s}7<SbvJ?i$ytyg5YWqKc+mW&Q3#)@ES4s04udAAY(TO#q*E_XWR2F)``ql~J2%+fmu!rWV(2l1 zf+77hwC!9U-^Cc;P>k>GKE8tn@R=ry+1}{mdw)>=C;D$2VtgN{G5Xs7(*W+)lTtQP zKkoAzv49u);*uZD#RN`i(*+C2Yb7?nuVZb(=N2#-^V9kFRo^;3itP*CpZdRz#B!eS zvjuz~0hrzhWGer0;15AmQgy!o*>x-c;!dT2i+g9vq9s&At~SWLek$So$ghVv|l9Yc}SA> zlH`3#ighLAi<#sh)qhOMc_rxyO?I)a1mwCBz)96lb=gRD;}OWN=kuo0DLae}@+^`4 EU(p(syZ`_I 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/105] 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/105] 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/105] 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^g&p6#lBa=Vxbjhy6hT1#QThT@-p|c4rnvK$aDd$m(WX690F5W@>gP%ywgU z4=e_v5^thL6O9MsMK4~AF?!^n2T#Tjy%_c8LA{|D4jRY2`eAwo3>^Fp{p!_w^}VX< zSFdXOJ~ybX9?0bFUKFRxd&<*=nL^pN3&Irt+IF#|{g*QU7=TKx<%b-Na7i>AUves@ z@=V#b%Y~w?`HF?YOsQns(^KUs$+et$>G1I*j1idx!=#|r_%tC}iINbL*n-=UnjHQj z@k8h8KOIb_e}X-HX<(c*Yx(gjox<>?hYwx-^;-aj7-O-wXu1W>7=yu*CIbwX-a+n0 z;x^KZ)D-}xm1ueq3@eo*TUyM+NT=GQS=I*fOk2N`#?lu>XW~a8QZ)b;`-PO(*sO{- zu4JN72V0Db<3KqSG!C;*xu!-BSwN{eGyq%LK{S)DTfxhgo+z^;dCI+zliI4|j{)tsL;Pw1MbNvzb%*p(bX0Ud~4S3#bN6~t| z5#pBonqIBdu5)ygy+%RWUNem9&Z^9{gvdDaGT0XLtUY0Fv-9N%)3wcQ_I~rszU>AB zV=doXvBih-tL=r&6_3}2!h^tX$NFlkZg6YUYlQi0r?3XW-oxAt+S4rp9DVGBJ!hAk z8E49#Dio)Rh2o4o8SoZ&L%!2~X0|e)=WA77uk-ru($dWCYT$acRXgCPn_<)U?BWAc zrTh0hCxg(UWLc>D?P`l7T=j#fH>_H+h~8-VZls)OERN32?X@RqQj5Od3hjm;*t3;+ z`>>jVy%6{{9)^Ap?)*PyJ?j8)xm#_5QD!CP{2=1%yXU8o%%tWAj8SWoxtVQYcd)yR z9dtSd0dzW@4uf$nH#j8nU4s}7<SbvJ?i$ytyg5YWqKc+mW&Q3#)@ES4s04udAAY(TO#q*E_XWR2F)``ql~J2%+fmu!rWV(2l1 zf+77hwC!9U-^Cc;P>k>GKE8tn@R=ry+1}{mdw)>=C;D$2VtgN{G5Xs7(*W+)lTtQP zKkoAzv49u);*uZD#RN`i(*+C2Yb7?nuVZb(=N2#-^V9kFRo^;3itP*CpZdRz#B!eS zvjuz~0hrzhWGer0;15AmQgy!o*>x-c;!dT2i+g9vq9s&At~SWLek$So$ghVv|l9Yc}SA> zlH`3#ighLAi<#sh)qhOMc_rxyO?I)a1mwCBz)96lb=gRD;}OWN=kuo0DLae}@+^`4 EU(p(syZ`_I From 7c7112a157d863665fe1b8fae2a94fbbb17dc199 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 01:11:35 -0800 Subject: [PATCH 005/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] =?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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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/105] 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 */