From 811790a14f567b1a58a4434c9fae0cf764e8fd2d Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 7 Jan 2013 21:33:04 -0800 Subject: 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 +++++++++++++++++++++++++++++++++++++ 5 files changed, 344 insertions(+) create mode 100644 apps/px4io/i2c.c create mode 100644 apps/systemcmds/i2c/Makefile create mode 100644 apps/systemcmds/i2c/i2c.c (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c new file mode 100644 index 000000000..3b92fe3ef --- /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 bea9d59bc..4e3555b13 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 e388f65e3..afbaa78dc 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 000000000..046e74757 --- /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 000000000..1124e560d --- /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; +} -- cgit v1.2.3 From 7c2445f74d9616d2f55166b02eea39003d48280c Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 00:42:18 -0800 Subject: 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 (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 3b92fe3ef..c3c897811 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 640665f6c..000000000 Binary files a/nuttx/fs/stGrJvFk and /dev/null differ -- cgit v1.2.3 From f12fa7ee060b33194723df983b643b51410ea4f6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 01:11:52 -0800 Subject: Don't do retries, since it just complicates things. --- apps/systemcmds/i2c/i2c.c | 73 ++++++++++++++++++++++------------------------- 1 file changed, 34 insertions(+), 39 deletions(-) (limited to 'apps') diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 1124e560d..57c61e824 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); - - 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 + + // 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); + + // reset the I2C bus to unwedge on error + if (ret != OK) up_i2creset(i2c); - } while (tries++ < 5); return ret; } -- cgit v1.2.3 From 0dab53ae2674d7a907e861e912806f0ae8e35a35 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 01:12:25 -0800 Subject: Implement I2C slave DMA. Not working yet. --- apps/px4io/i2c.c | 132 +++++++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 118 insertions(+), 14 deletions(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index c3c897811..215326835 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 to ack our selection and get direction */ + uint16_t sr2 = rSR2; + + debug("addr"); + + if (sr2 & I2C_SR2_TRA) { + /* we are the transmitter */ + debug("tx"); - /* clear ADDR */ + 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) { -- cgit v1.2.3 From 2fb820fabd7c7b675c4e0da026c95546b62424e6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 23:24:17 -0800 Subject: I2C slave RX DMA works. --- apps/px4io/i2c.c | 84 +++++++++++++++++++++++--------------------------------- 1 file changed, 34 insertions(+), 50 deletions(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 215326835..e55e992fd 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(); } -- cgit v1.2.3 From 3cea0959b72fe160e6a05e8efef1d325d12d4544 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 9 Jan 2013 21:39:54 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index e55e992fd..3e4ac3488 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 57c61e824..422d9f915 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 -- cgit v1.2.3 From 4f285f7c80904bf7685ab3d5d8bf515b5c0ad7ab Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 00:08:13 -0800 Subject: 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 | 61 +++++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 43 insertions(+), 18 deletions(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 3e4ac3488..61daa9ada 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); + stm32_dmastop(rx_dma); - for (unsigned i = 0; i < rx_len; i++) + /* XXX handle reception */ + for (unsigned i = 0; i < rx_len; i++) { tx_buf[i] = rx_buf[i] + 1; + rx_buf[i] = 0; + } - /* XXX handle reception */ + /* 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(); } -- cgit v1.2.3 From 97136375e393d71000a8f5e7c4c5a1b1bcb0f464 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 01:56:46 -0800 Subject: Turn off the I2C register dump at startup. --- apps/px4io/i2c.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 61daa9ada..037c64522 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 -- cgit v1.2.3 From 469d13fdfe3f7aa1258209ef2bc6645a5f0c231c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 01:57:50 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c index 14e8dc13a..cb288aca8 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 0868c3cd3..f93890345 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 a70923cbf..194701a3f 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 791a7cfee..e0986b72c 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 # -- cgit v1.2.3 From 329f595bca22ca7f253833d0189c1bf61c23c631 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 02:11:53 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c index cb288aca8..14e8dc13a 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 } -- cgit v1.2.3 From 5e35491a386006c8f27b7761b3845a73bf85f3fa Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 02:12:49 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 4e3555b13..e51c1c73c 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 e0986b72c..d2a4f3848 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 # -- cgit v1.2.3 From 1cecba2a86520b042600b7194e8f7166560d1fdd Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 09:40:55 -0800 Subject: Turn off i2c slave debug output for real. --- apps/px4io/i2c.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 037c64522..4e95dd583 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -44,7 +44,7 @@ #include #include -#define DEBUG +//#define DEBUG #include "px4io.h" /* -- cgit v1.2.3 From b0fb86a693db595c7adcd61c4179f0ddb8807c4e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 10 Jan 2013 23:43:03 -0800 Subject: Sketch out the protocol as it will be on top of I2C --- apps/px4io/protocol.h | 106 +++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 104 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 90236b40c..218ab9030 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 -- cgit v1.2.3 From 8ebe21b27b279b5d941d4829e5ebee28b84b146c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 13 Jan 2013 02:20:01 -0800 Subject: 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 (limited to 'apps') diff --git a/Firmware.sublime-project b/Firmware.sublime-project index a316ae2fa..72bacee9f 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 218ab9030..aabc476dd 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 afbaa78dc..fb0659602 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 000000000..1397dd2a4 --- /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; +} -- cgit v1.2.3 From 4e38615595abd9d27d0cb000caafb98cc3670abe Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 13 Jan 2013 18:57:27 -0800 Subject: Major workover of the PX4IO firmware for I2C operation. --- apps/px4io/Makefile | 12 ++- apps/px4io/controls.c | 268 +++++++++++++++++++++++++++++++++++-------------- apps/px4io/dsm.c | 44 +++----- apps/px4io/i2c.c | 1 - apps/px4io/mixer.cpp | 128 ++++++++++++----------- apps/px4io/protocol.h | 39 +++++-- apps/px4io/px4io.c | 32 +++--- apps/px4io/px4io.h | 77 ++++++++------ apps/px4io/registers.c | 185 +++++++++++++++++++++++++--------- apps/px4io/safety.c | 29 ++---- apps/px4io/sbus.c | 42 +++----- 11 files changed, 549 insertions(+), 308 deletions(-) (limited to 'apps') diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index d97f963ab..0eb3ffcf7 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 169d5bb81..072d296da 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 + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have S.bus signal. */ - unsigned rc_channels = system_state.rc_channels; + 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; - if (fds[0].revents & POLLIN) - locked |= dsm_input(); + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - if (fds[1].revents & POLLIN) - locked |= sbus_input(); + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; /* - * 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. + * If we received a new frame from any of the RC sources, process it. */ - if (!locked) - ppm_input(); + 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; - /* 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; + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + ASSERT(mapped < MAX_CONTROL_CHANNELS); - } else { - /* override not engaged, use FMU */ - system_state.mixer_manual_override = false; + 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) { + rc_input_lost = true; + } - 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; - } + /* + * Handle losing RC input + */ + if (rc_input_lost) { + + /* 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); + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - /* set the number of channels to zero - no inputs */ - system_state.rc_channels = 0; + /* Mark the arrays as empty */ + r_raw_rc_count = 0; + r_rc_valid = 0; } /* - * 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. + * 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. */ - mixer_tick(); + if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + + bool override = false; + + /* + * Check mapped channel 5; if the value is 'high' then the pilot has + * requested override. + */ + 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; + } + } + } } -static void -ppm_input(void) +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 new PPM input. + * Look for recent PPM input. */ - if (ppm_last_valid_decode != 0) { - - /* avoid racing with PPM updates */ - irqstate_t state = irqsave(); + if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { /* PPM data exists, copy it */ - system_state.rc_channels = ppm_decoded_channels; + result = true; + *num_values = ppm_decoded_channels; + if (*num_values > MAX_CONTROL_CHANNELS) + *num_values = MAX_CONTROL_CHANNELS; - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - system_state.rc_channel_data[i] = ppm_buffer[i]; - } + for (unsigned i = 0; i < *num_values; i++) + values[i] = ppm_buffer[i]; - /* copy the timestamp and clear it */ - system_state.rc_channels_timestamp = ppm_last_valid_decode; + /* clear validity */ ppm_last_valid_decode = 0; + } - irqrestore(state); + irqrestore(state); - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; - } + return result; } diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 560ef47d9..f0e8e0f32 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 4e95dd583..d08da2ff1 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 ed7d684b6..7ccf915b0 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 if we have any control data at all. + * Run the mixers. */ - 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; - } + 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; - /* - * 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 = mixed; i < IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; + + /* + * 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. - * 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 aabc476dd..4cb8a54ef 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 e51c1c73c..13f46939a 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 fb0659602..2e2c50a3a 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 1397dd2a4..59684f1ee 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_RAW_ADC_INPUT: + r_page_adc[0] = adc_measure(ADC_VBATT); + r_page_adc[1] = adc_measure(ADC_IN5); - case PX4IO_PAGE_SERVOS: - *values = system_state.servos; - *num_values = IO_SERVO_COUNT; + SELECT_PAGE(r_page_adc); 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; + /* + * Pages that are just a straight read of the register state. + */ +#define COPY_PAGE(_page_name, _page) case _page_name: SELECT_PAGE(_page); break; - case PX4IO_PAGE_RC_INPUT: - *values = system_state.rc_channel_data; - *num_values = system_state.rc_channels; - return -1; + /* 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_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; + /* 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 0cae29ac9..a14051f76 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 568ef8091..d199a9361 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; - - /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + *num_values = chancount; - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return true; } -- cgit v1.2.3 From 6e291ddedc8b2d7bfeae8029a37df0b581262796 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 00:18:05 -0800 Subject: 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(+) (limited to 'apps') diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 474190d83..c513ae2b6 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 4d630b8a8..66c34dd7c 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. * -- cgit v1.2.3 From 2311e03379f717472a0c7cc0d990e08407d0cb5e Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 00:19:01 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2c2c236ca..301e6cc8f 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 (ret > 0) { - /* if we have new data from IO, go handle it */ - if (fds[0].revents & POLLIN) - io_recv(); + /* if we have new control data from the ORB, handle it */ + if (fds[0].revents & POLLIN) + io_set_control_state(); - /* if we have new control data from the ORB, handle it */ - if (fds[1].revents & POLLIN) { - - /* get controls */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* scale controls to PWM (temporary measure) */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _controls.control[i]); - - /* and flag for update */ - _send_needed = true; - } + /* 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 an arming state update, handle it */ - if (fds[2].revents & POLLIN) { + hrt_abstime_t now = hrt_absolute_time(); - 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; - } - } - - /* send a config packet to IO if required */ - if (_config_needed) { - _config_needed = false; - config_send(); - } + /* + * 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; - /* send a mixer update if needed */ - if (_mix_buf != nullptr) { - mixer_send(_mix_buf, _mix_buf_len); + /* + * Pull status and alarms from IO + */ + io_get_status(); - /* clear the buffer record so the ioctl handler knows we're done */ - _mix_buf = nullptr; - _mix_buf_len = 0; + switch (poll_phase) { + case 0: + /* XXX fetch raw RC values */ + break; + case 1: + /* XXX fetch servo outputs */ + break; } - /* 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]; + + /* 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); + } + } - input = controls->control[control_index]; - return 0; } +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 4cb8a54ef..682348a60 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 -- cgit v1.2.3 From 2dc47160f4ac3b1dbd2e81c31237459b50497ca3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 00:30:18 -0800 Subject: Factoring and comments. --- apps/drivers/px4io/px4io.cpp | 56 +++++++++++++++++++++++++++++++++++--------- 1 file changed, 45 insertions(+), 11 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 301e6cc8f..ad372081a 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; -- cgit v1.2.3 From 06b66ad065f096060bfdd2e1f18cdc6704c70d2c Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 01:09:42 -0800 Subject: Don't advertise things we don't have anymore. --- apps/drivers/px4io/px4io.cpp | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index ad372081a..968f433b2 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(); -- cgit v1.2.3 From 5c60ed9a9457e3ab0c51584e7e0db59bdbe4fd87 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 14 Jan 2013 01:11:29 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 7ccf915b0..f394233f4 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 682348a60..501691cd2 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 2e2c50a3a..d8cdafb4b 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 59684f1ee..0dd8fe28d 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 a14051f76..540636e19 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; } -- cgit v1.2.3 From 2686344d585c2f4c4034357fcf3bf9be7b7b92e7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:40:15 -0800 Subject: Adjust the default deadzone for RC inputs per feedback. --- apps/px4io/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 072d296da..2af62a369 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; } -- cgit v1.2.3 From f3a587dfced54bfdfe3471e6099c3ea16bc33a31 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:40:41 -0800 Subject: Wire the I2C device code into the register handler. --- apps/px4io/i2c.c | 47 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 36 insertions(+), 11 deletions(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index d08da2ff1..cf6921179 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(); -- cgit v1.2.3 From 112f5ea9697a2ada9e3852f9c2e7c10ab0e78a8a Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:41:13 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index f394233f4..178b0bb43 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 501691cd2..a2be4bfc4 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 13f46939a..c3dacb2a6 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 0dd8fe28d..2f411eebf 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 */ -- cgit v1.2.3 From 0eb5a070f165fe311dd5bdf4c40635276b000787 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:41:47 -0800 Subject: Checkpoint: more work on the px4io driver. Add raw PWM passthrough ioctl. --- apps/drivers/drv_pwm_output.h | 3 - apps/drivers/px4io/px4io.cpp | 150 ++++++++++++++++++++++++++++++------------ 2 files changed, 109 insertions(+), 44 deletions(-) (limited to 'apps') diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index c110cd5cb..fe08d3fe5 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 968f433b2..f97f4668d 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 */ - if ((arg >= 900) && (arg <= 2100)) { - _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; - _send_needed = true; + 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 { ret = -EINVAL; } break; + } + + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_servos - 1): { - 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)]; + 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: - _relays = 0; - _send_needed = true; + 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 -- cgit v1.2.3 From b4dcdae03d597b71f02850bec24c80134c10ae53 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 22:22:15 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 178b0bb43..932aab930 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 a2be4bfc4..39ee4c0b1 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 2f411eebf..0206e0db0 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; -- cgit v1.2.3 From d207d22a4f4c845ae9874846e7aa92dce9d1df20 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 22:59:57 -0800 Subject: compile fix --- apps/drivers/device/i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index c513ae2b6..a5b2fab7e 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); -- cgit v1.2.3 From 646b926ac98534004a0226dc491458728e61dabd Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 23:00:21 -0800 Subject: minor doc fix --- apps/uORB/topics/actuator_controls_effective.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index 40278c56c..088c4fc8f 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 */ -- cgit v1.2.3 From 7b367c3eb30dac38c016e78c1a64897ff10f377c Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 23:01:04 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f97f4668d..c78ce71c6 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_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_get_rc_input(rc_input_values &input_rc); + 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 932aab930..1c7a05343 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; -- cgit v1.2.3 From 4b07a9abd36f3632cff1efeb993fd02a441f61d7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 16 Jan 2013 13:02:49 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c78ce71c6..c4d7ec489 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() -- cgit v1.2.3 From d7632b179426bc3544c4bc1a7c024555f3eaafd7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 19 Jan 2013 12:38:53 -0800 Subject: Drop some commented code now the functionality is implemented. --- apps/drivers/px4io/px4io.cpp | 58 -------------------------------------------- 1 file changed, 58 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c4d7ec489..681ac650e 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) { -- cgit v1.2.3 From 3244bb83ea2656a20cc486205dd77d48f525c05c Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:56:03 -0800 Subject: Better sanity checking and error handling. --- apps/drivers/px4io/px4io.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 681ac650e..a866a37cf 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 -- cgit v1.2.3 From b34311915a04e9ec4d301930e4342f285b5c6cb4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:56:58 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index cf6921179..174ec2813 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 0206e0db0..6b7ef015f 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, -- cgit v1.2.3 From d8a013f8720e81afb637b8206fbe521ccb43ac8f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:57:16 -0800 Subject: Tinkering. --- apps/systemcmds/i2c/i2c.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 422d9f915..e1babdc12 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 -- cgit v1.2.3 From dce2afde0ffc01f8eba921212b819082b6f9297c Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 20:18:18 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a866a37cf..bdddd1b1a 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]; - - hdr[0] = page; - hdr[1] = offset; - - msgv[0].flags = 0; - msgv[0].buffer = hdr; - msgv[0].length = sizeof(hdr); + uint8_t addr[2]; + int ret; - msgv[1].flags = I2C_M_READ; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); + /* 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; + } - 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; } -- cgit v1.2.3 From 0bc836ae1d90b1805940ec8ae271639f0074a792 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 22:19:33 -0800 Subject: 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(+) (limited to 'apps') diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 927406108..4decc324e 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 bdddd1b1a..51476ddd3 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; -- cgit v1.2.3 From 82f72b96de81478fb5d627be30a2f81ef2c5135f Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:35:32 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 174ec2813..bbb72360b 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); } } -- cgit v1.2.3 From 57d028fddd096cb3336aa1fbd60d406e2d44ec48 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 25 Jan 2013 10:10:47 +1100 Subject: px4io: fixed array reference bug --- apps/px4io/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 6b7ef015f..1802433b3 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; -- cgit v1.2.3 From f8bea6d07b3ca32e697617588df2c106b8fb375c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 25 Jan 2013 10:13:00 +1100 Subject: px4io: fixed cpp error --- apps/px4io/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 1802433b3..a4d81fd6e 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); -- cgit v1.2.3 From 8972843b14a86aae3bbc4aa84e0c0a446eb7d605 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 25 Jan 2013 10:15:41 +1100 Subject: px4io: fixed mixer load --- apps/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 51476ddd3..4c1b5ddf2 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; -- cgit v1.2.3 From 1b30cd2f938c05841470e5aedcbd535105ea3f36 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:40:18 -0800 Subject: Dump a couple of unused member variables. --- apps/drivers/px4io/px4io.cpp | 3 --- 1 file changed, 3 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 4c1b5ddf2..985244873 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 -- cgit v1.2.3 From 818e898a7e084d6529da549ca3ea7c7d53fe5c46 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:54:04 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 985244873..fbbc0277e 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; } -- cgit v1.2.3 From 6c75c5909e7f72897ddb737c6459ea6bbfab3900 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:58:55 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index bbb72360b..dca5ceb1d 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 -- cgit v1.2.3 From 900b0d58ef62d1ff1406a312ac88317152f0312a Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 21:59:31 -0800 Subject: Less debug output. --- apps/drivers/px4io/px4io.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index fbbc0277e..9296aa0a5 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; -- cgit v1.2.3 From b46d05835b9ff55c1f21d37483202888eeea1656 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 22:58:33 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index dca5ceb1d..bbbbb6e46 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 1c7a05343..91a914575 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 39ee4c0b1..6dbe16229 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 d8cdafb4b..c90db5d78 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 a4d81fd6e..22ed1de70 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; -- cgit v1.2.3 From 5fe376c7b9bed861768680089bff3c62a030e2b6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 00:01:25 -0800 Subject: 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 | 92 +++++++++++++++++++++++--------------------- 1 file changed, 48 insertions(+), 44 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9296aa0a5..a0e9b18d2 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 ((now - last_poll_time) < 20000) - continue; - - /* - * Pull status and alarms from IO. - */ - io_get_status(); - /* - * Get R/C input from IO. + * If it's time for another tick of the polling status machine, + * try it now. */ - 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(); - - /* - * 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(); + if ((now - last_poll_time) >= 20000) { + + /* + * Pull status and alarms from IO. + */ + io_get_status(); + + /* + * 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(); + + /* + * 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(); + } } + + 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; -- cgit v1.2.3 From f854e2f79133c93c56a40645fd37a103a26b4623 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 00:02:45 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 2af62a369..0b2c199ee 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); -- cgit v1.2.3 From b20c050402dfe11e1f0d0002020205872b96172d Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 12:27:03 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a0e9b18d2..1678ed0de 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; -- cgit v1.2.3 From 984e68d76e6e199608aded66ed3c032a3db2fe32 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 12:27:42 -0800 Subject: 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(+) (limited to 'apps') diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index fe08d3fe5..23883323e 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 1678ed0de..61c9a793d 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); -- cgit v1.2.3 From 3a8bbe837e6c49e6fd92e47f889a2e7925173611 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:14:15 -0800 Subject: Allow readback of the direct PWM outputs (this mirrors the PWM servo outputs) --- apps/px4io/registers.c | 1 + 1 file changed, 1 insertion(+) (limited to 'apps') diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 22ed1de70..1593ed2c8 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: -- cgit v1.2.3 From 899fbcc7cf13fbcdfb371663fef7782dd9ea1456 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:14:52 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 61c9a793d..257b15685 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 { -- cgit v1.2.3 From 72fcc8aad32936692814f4e9521a462bedc6c723 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:15:27 -0800 Subject: Tidy up the write path. --- apps/drivers/px4io/px4io.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 257b15685..769363b4a 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[]); -- cgit v1.2.3 From 4ea8a64b395026d7df8739bc6c76ea121ee6b977 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:15:57 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 769363b4a..2d7495aaa 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; -- cgit v1.2.3 From 4ab490bd5017c6dd132b4422aecda597a5c8cd38 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 13:16:24 -0800 Subject: Only update the servo output values when we are armed. --- apps/px4io/mixer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 91a914575..79b32009e 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); -- cgit v1.2.3 From 11796e27f25ae9a5d3cb39d2d43f0ad53d388dcc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 14:25:22 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 1593ed2c8..1c6fbf4fb 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 + +last_page = page; +last_offset = offset; - /* if the offset is beyond the end of the page, we have no data */ - if (*num_values <= 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; } -- cgit v1.2.3 From 6ba4cd04fecd8600dbea7b94ccf0706eac40a089 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 14:26:02 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index bbbbb6e46..c16d5700c 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(); -- cgit v1.2.3 From 2a18d6466c36c50244851d225a5319207b08b0bf Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 14:36:31 -0800 Subject: Add a bus saturation test for px4io. --- apps/drivers/px4io/px4io.cpp | 63 ++++++++++++++++++++++++++++++-------------- 1 file changed, 43 insertions(+), 20 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2d7495aaa..2a860e5c1 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"); - 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); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) + err(1, "failed to get servo count"); - close(fd); + if (ioctl(fd, PWM_SERVO_ARM, 0)) + err(1, "failed to arm servos"); - actuator_armed_s aa; + for (;;) { - aa.armed = true; - aa.lockdown = false; + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; - orb_advertise(ORB_ID(actuator_armed), &aa); + ret = write(fd, servos, sizeof(servos)); + if (ret != sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); - exit(0); + 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]); + } + } } void -- cgit v1.2.3 From e0f83af96fdab2cd5b239dec3a842c4a2a92ad85 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 15:13:30 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index b8845aaf1..b215166df 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 */ -- cgit v1.2.3 From 6bd662cfb2d90628eb03c0bec1ebac54e47a7090 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:11:31 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index c16d5700c..2b8c502c7 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(); -- cgit v1.2.3 From fd28217e5982cbb848a2ddf2d118ffa3d69a4746 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:15:23 -0800 Subject: Implement the retry counter for message-vector based transfers. --- apps/drivers/device/i2c.cpp | 30 +++++++++++++++++++++--------- 1 file changed, 21 insertions(+), 9 deletions(-) (limited to 'apps') diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index a5b2fab7e..d2cd5f19b 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; } -- cgit v1.2.3 From 52ff9b7d433fed007a62fe2de375f685aa1b6b8a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:16:19 -0800 Subject: Use multi-part transactions rather than separate transfers to avoid racing between the ioctl and thread-side interfaces. --- apps/drivers/px4io/px4io.cpp | 62 +++++++++++++++++++++++--------------------- 1 file changed, 33 insertions(+), 29 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2a860e5c1..e48c84774 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; - - /* 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; - } + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + 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; -- cgit v1.2.3 From 5ee52138c4ac5c807888e3973099fd0f9a29aa59 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 08:47:21 +1100 Subject: px4io: ensure RC_OK status flag is set on good input --- apps/px4io/controls.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 0b2c199ee..4782dc742 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 -- cgit v1.2.3 From c0a46c4b93d29a438a6207ce49589bddc026c900 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 10:58:30 +1100 Subject: px4io: fixed logical vs bitwise typo --- apps/px4io/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 4782dc742..8cd5235b7 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 { -- cgit v1.2.3 From 7864176b5a1f2ac9cde4ac29ef19c5a72f87b3d3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 16:37:35 -0800 Subject: A couple of logic fixes from Tridge. --- apps/px4io/mixer.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 79b32009e..848a88621 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 -- cgit v1.2.3 From 33c12d13ad42582745989794bf1d966d2a9e070f Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 17:07:58 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index d2cd5f19b..a416801eb 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; } -- cgit v1.2.3 From 621063ac084954bba11189c8566776aff25bfaeb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 17:10:04 -0800 Subject: Increase the number of I2C retries. --- apps/drivers/hmc5883/hmc5883.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 3734d7755..4a201b98c 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) || -- cgit v1.2.3 From 666d3a401b04c259f930d6614130de97feed0034 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 18:55:26 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index b215166df..9b1d8d5b7 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); -- cgit v1.2.3 From 981477c7856c5d7694561e0a13ebb0747518370e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 19:24:18 -0800 Subject: 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 | 108 +++++++++++++++++++++++++++++++------------------ 2 files changed, 70 insertions(+), 40 deletions(-) (limited to 'apps') diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 6dbe16229..54f2fa27c 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 1c6fbf4fb..c60491f93 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -50,33 +50,8 @@ static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); /** - * Setup registers - */ -volatile uint16_t r_page_setup[] = -{ - [PX4IO_P_SETUP_FEATURES] = 0, - [PX4IO_P_SETUP_ARMING] = 0, - [PX4IO_P_SETUP_PWM_RATES] = 0, - [PX4IO_P_SETUP_PWM_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) -#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 << PX4IO_RELAY_CHANNELS) - 1) - -/** - * Control values from the FMU. - */ -volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; - -/** + * PAGE 0 + * * Static configuration parameters. */ static const uint16_t r_page_config[] = { @@ -92,6 +67,8 @@ static const uint16_t r_page_config[] = { }; /** + * PAGE 1 + * * Status values. */ uint16_t r_page_status[] = { @@ -104,26 +81,33 @@ uint16_t r_page_status[] = { }; /** - * ADC input buffer. - */ -uint16_t r_page_adc[ADC_CHANNEL_COUNT]; - -/** + * 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]; /** - * Servo PWM values + * PAGE 4 + * + * Raw RC input */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; +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[] = { @@ -132,25 +116,71 @@ uint16_t r_page_rc_input[] = { }; /** - * Raw RC input + * PAGE 6 + * + * Raw ADC input. */ -uint16_t r_page_raw_rc_input[] = +uint16_t r_page_adc[ADC_CHANNEL_COUNT]; + +/** + * PAGE 100 + * + * Setup registers + */ +volatile uint16_t r_page_setup[] = { - [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_SETUP_FEATURES] = 0, + [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, + [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) +#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 << PX4IO_RELAY_CHANNELS) - 1) + +/** + * PAGE 101 + * + * Control values from the FMU. + */ +volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; + +/* + * PAGE 102 does not have a buffer. + */ + /** + * PAGE 103 + * * R/C channel input configuration. */ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +/* valid options 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) { -- cgit v1.2.3 From 6d0363faff7f5a59264198f04bae6f5b61c54510 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 19:43:23 -0800 Subject: Disarm IO at driver startup time. --- apps/drivers/px4io/px4io.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index e48c84774..f948fec2c 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) { -- cgit v1.2.3 From 636e0cc56ad251318614f05d49b53f677456d2c1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 20:23:19 -0800 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 9b1d8d5b7..231006ae3 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); -- cgit v1.2.3 From 43ead720a711c8e29733eb4559f6ec69e17b69cb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 26 Jan 2013 21:17:10 -0800 Subject: 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 (limited to 'apps') diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c deleted file mode 100644 index e51c2771a..000000000 --- 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; - } - } -} - -- cgit v1.2.3 From a196e73842259152595d524b150a611076ca91d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 18:11:59 +0100 Subject: Fixed arm ok flag typo --- apps/drivers/px4io/px4io.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f948fec2c..2a0d72c31 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'"); } -- cgit v1.2.3 From 167ec25c4f04ceed699a20fd6490db6cdf74e223 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 18:28:41 +0100 Subject: Fixed altitude jump issue, hunted down and fix by Andrew Tridgell. --- apps/drivers/ms5611/ms5611.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 231006ae3..30166828a 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) -- cgit v1.2.3 From a645a388bcb8c598f59d11c4b4a68265f64cb022 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 9 Feb 2013 00:53:51 -0800 Subject: Fix a sign error --- apps/px4io/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index c60491f93..3824c64b0 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; -- cgit v1.2.3 From 3c8da27d72e81da923106dcc667a5e644de90b86 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 9 Feb 2013 00:57:23 -0800 Subject: Fix a misleading comment. --- apps/px4io/i2c.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 2b8c502c7..93dfd21dd 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; -- cgit v1.2.3 From aa16a63a10bb43d8614f01915cdf33996fe0d0e4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 11 Feb 2013 20:40:06 -0800 Subject: Fix assignment of relay GPIOs. --- apps/drivers/boards/px4io/px4io_internal.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h index f77d237a7..44bb98513 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 ********************************************************************/ -- cgit v1.2.3 From 857fe5d405312508439a91493802e58e547d7940 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 09:20:11 +0100 Subject: Fixes to RC config transmission from Simon Wilks --- apps/px4io/registers.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 3824c64b0..815563daa 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: -- cgit v1.2.3 From 01ada7f74f38f84fd19b74313c21f3dd13ca420d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 09:31:43 +0100 Subject: Fixed mixer transmission between FMU and IO --- apps/px4io/i2c.c | 2 +- apps/px4io/protocol.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 93dfd21dd..235f8affa 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 54f2fa27c..6278e05d4 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) -- cgit v1.2.3 From 163257f3bd4ebe13f6f3eda537793602035baf28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 09:33:52 +0100 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2a0d72c31..c65855b83 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; } -- cgit v1.2.3 From d4ca6a29a19c96d359aa1458a4e6f3d9c86b01ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 22:19:53 +0100 Subject: Ensured that the mixer output obeys the FMU and IO armed state --- apps/px4io/mixer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 848a88621..740d8aeb0 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 */ -- cgit v1.2.3 From 4595cc65b8bd30ba084783660a541c57c3921dbb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Feb 2013 07:43:51 +0100 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c65855b83..51ff3fb83 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 8cd5235b7..b4a18bae6 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 6278e05d4..29a287e45 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 815563daa..6229a6cc1 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; -- cgit v1.2.3 From bfecfbf5eef422e0ee069a17ff2482b352e29386 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 08:46:41 +1100 Subject: px4io: added isr_debug() this is useful for debugging px4io internals --- apps/px4io/px4io.c | 62 +++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index c3dacb2a6..8e0e52f04 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 +} -- cgit v1.2.3 From 6eb69b07a8fd44fff1bfcf77bf4622f4dd044eef Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 11:06:13 +1100 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 235f8affa..4485daa5b 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 8e0e52f04..b7e6ccb21 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 c90db5d78..92d55a47c 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); -- cgit v1.2.3 From 598622a00f71fac330c7bc919382466cfa059601 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 Feb 2013 18:16:29 +0100 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 6229a6cc1..f14cd8309 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; -- cgit v1.2.3 From a33f314a2500e207d7aa566e2ea4fe8ace7e6300 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 14:47:43 +1100 Subject: More output --- apps/px4io/px4io.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index b7e6ccb21..e50022a0c 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); } } -- cgit v1.2.3 From 04bea8678e98d58145d18c44bd3069433f528019 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Jan 2013 08:16:39 +1100 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 23883323e..64bb540b4 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 51ff3fb83..8bcf4bacb 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 29a287e45..f64b82ad1 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 92d55a47c..9c5989df7 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 f14cd8309..014608416 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; } -- cgit v1.2.3 From 038037d67656b92947a976cf647dfd4f748ec3ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 16:06:33 +0100 Subject: 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(-) (limited to 'apps') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b1bc0f9b..f917b7275 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 8bcf4bacb..069028c14 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. + */ + + uint16_t reg; - /* publish RC config to IO */ - ret = io_set_rc_config(); - if (ret != OK) { - log("failed to update RC input config"); + /* 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 f64b82ad1..4f1b067bd 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 014608416..be3bebada 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; - 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)) + /* + * Update arming state - disarm if no longer OK. + * This builds on the requirement that the FMU driver + * asks about the FMU arming state on initialization, + * so that an in-air reset of FMU can not lead to a + * lockup of the IO arming state. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + } + + r_setup_arming = value; + break; case PX4IO_P_SETUP_PWM_RATES: diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5..893e34537 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) */ }; /** -- cgit v1.2.3 From f689f0abb0832c3d68e462e291a7a4d6dd43e216 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 16:38:19 +0100 Subject: Fixed excessive debug buffer size --- apps/px4io/px4io.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index e50022a0c..96f49492f 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, -- cgit v1.2.3 From 56bf9855a8f8140a8a5edeeb08f4246249b27085 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 17:47:26 +0100 Subject: 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(-) (limited to 'apps') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 069028c14..b2e0c6ee4 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 4f1b067bd..a957a9e79 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 96f49492f..c102bf479 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 be3bebada..40bf72482 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; -- cgit v1.2.3 From 9f15f38e5705d73e1dfdf381c8d3b458a8a1557b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Feb 2013 14:31:26 +1100 Subject: 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(-) (limited to 'apps') diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index fec5eed23..122f00754 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 6944776a9..cd5977258 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 40bf72482..23ad4aa88 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 */ -- cgit v1.2.3