From 1f2ad5029a5f3e68423c462c4c8077c65059b775 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 23 Nov 2012 23:59:39 -0800 Subject: [PATCH 01/21] Improved termios support for the STM32 UART driver. Also add a little more termios processing to the generic serial layer. Implement FIONREAD. --- nuttx/arch/arm/src/stm32/stm32_serial.c | 200 +++++++++++----- nuttx/drivers/serial/serial.c | 291 ++++++++++++++++++------ nuttx/include/nuttx/fs/ioctl.h | 4 + nuttx/include/nuttx/serial/serial.h | 11 +- nuttx/include/termios.h | 30 +-- 5 files changed, 387 insertions(+), 149 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c index ff18d208bc..0868c3cd3d 100644 --- a/nuttx/arch/arm/src/stm32/stm32_serial.c +++ b/nuttx/arch/arm/src/stm32/stm32_serial.c @@ -185,11 +185,15 @@ struct up_dev_s uint8_t parity; /* 0=none, 1=odd, 2=even */ uint8_t bits; /* Number of bits (7 or 8) */ bool stopbits2; /* True: Configure with 2 stop bits instead of 1 */ + bool iflow; /* input flow control (RTS) enabled */ + bool oflow; /* output flow control (CTS) enabled */ uint32_t baud; /* Configured baud */ #else const uint8_t parity; /* 0=none, 1=odd, 2=even */ const uint8_t bits; /* Number of bits (7 or 8) */ const bool stopbits2; /* True: Configure with 2 stop bits instead of 1 */ + const bool iflow; /* input flow control (RTS) enabled */ + const bool oflow; /* output flow control (CTS) enabled */ const uint32_t baud; /* Configured baud */ #endif @@ -221,7 +225,7 @@ struct up_dev_s * Private Function Prototypes ****************************************************************************/ -static void up_setspeed(struct uart_dev_s *dev); +static void up_set_format(struct uart_dev_s *dev); static int up_setup(struct uart_dev_s *dev); static void up_shutdown(struct uart_dev_s *dev); static int up_attach(struct uart_dev_s *dev); @@ -393,6 +397,8 @@ static struct up_dev_s g_usart1priv = .parity = CONFIG_USART1_PARITY, .bits = CONFIG_USART1_BITS, .stopbits2 = CONFIG_USART1_2STOP, + .iflow = false, + .oflow = false, .baud = CONFIG_USART1_BAUD, .apbclock = STM32_PCLK2_FREQUENCY, .usartbase = STM32_USART1_BASE, @@ -444,6 +450,8 @@ static struct up_dev_s g_usart2priv = .parity = CONFIG_USART2_PARITY, .bits = CONFIG_USART2_BITS, .stopbits2 = CONFIG_USART2_2STOP, + .iflow = false, + .oflow = false, .baud = CONFIG_USART2_BAUD, .apbclock = STM32_PCLK1_FREQUENCY, .usartbase = STM32_USART2_BASE, @@ -495,6 +503,8 @@ static struct up_dev_s g_usart3priv = .parity = CONFIG_USART3_PARITY, .bits = CONFIG_USART3_BITS, .stopbits2 = CONFIG_USART3_2STOP, + .iflow = false, + .oflow = false, .baud = CONFIG_USART3_BAUD, .apbclock = STM32_PCLK1_FREQUENCY, .usartbase = STM32_USART3_BASE, @@ -546,17 +556,15 @@ static struct up_dev_s g_uart4priv = .parity = CONFIG_UART4_PARITY, .bits = CONFIG_UART4_BITS, .stopbits2 = CONFIG_UART4_2STOP, + .iflow = false, + .oflow = false, .baud = CONFIG_UART4_BAUD, .apbclock = STM32_PCLK1_FREQUENCY, .usartbase = STM32_UART4_BASE, .tx_gpio = GPIO_UART4_TX, .rx_gpio = GPIO_UART4_RX, -#ifdef GPIO_UART4_CTS - .cts_gpio = GPIO_UART4_CTS, -#endif -#ifdef GPIO_UART4_RTS - .rts_gpio = GPIO_UART4_RTS, -#endif + .cts_gpio = 0, /* flow control not supported on this port */ + .rts_gpio = 0, /* flow control not supported on this port */ #ifdef CONFIG_UART4_RXDMA .rxdma_channel = DMAMAP_UART4_RX, .rxfifo = g_uart4rxfifo, @@ -597,17 +605,15 @@ static struct up_dev_s g_uart5priv = .parity = CONFIG_UART5_PARITY, .bits = CONFIG_UART5_BITS, .stopbits2 = CONFIG_UART5_2STOP, + .iflow = false, + .oflow = false, .baud = CONFIG_UART5_BAUD, .apbclock = STM32_PCLK1_FREQUENCY, .usartbase = STM32_UART5_BASE, .tx_gpio = GPIO_UART5_TX, .rx_gpio = GPIO_UART5_RX, -#ifdef GPIO_UART5_CTS - .cts_gpio = GPIO_UART5_CTS, -#endif -#ifdef GPIO_UART5_RTS - .rts_gpio = GPIO_UART5_RTS, -#endif + .cts_gpio = 0, /* flow control not supported on this port */ + .rts_gpio = 0, /* flow control not supported on this port */ #ifdef CONFIG_UART5_RXDMA .rxdma_channel = DMAMAP_UART5_RX, .rxfifo = g_uart5rxfifo, @@ -648,6 +654,8 @@ static struct up_dev_s g_usart6priv = .parity = CONFIG_USART6_PARITY, .bits = CONFIG_USART6_BITS, .stopbits2 = CONFIG_USART6_2STOP, + .iflow = false, + .oflow = false, .baud = CONFIG_USART6_BAUD, .apbclock = STM32_PCLK2_FREQUENCY, .usartbase = STM32_USART6_BASE, @@ -812,21 +820,22 @@ static int up_dma_nextrx(struct up_dev_s *priv) #endif /**************************************************************************** - * Name: up_setspeed + * Name: up_set_format * * Description: - * Set the serial line speed. + * Set the serial line format and speed. * ****************************************************************************/ #ifndef CONFIG_SUPPRESS_UART_CONFIG -static void up_setspeed(struct uart_dev_s *dev) +static void up_set_format(struct uart_dev_s *dev) { struct up_dev_s *priv = (struct up_dev_s*)dev->priv; uint32_t usartdiv32; uint32_t mantissa; uint32_t fraction; uint32_t brr; + uint32_t regval; /* Configure the USART Baud Rate. The baud rate for the receiver and * transmitter (Rx and Tx) are both set to the same value as programmed @@ -844,20 +853,64 @@ static void up_setspeed(struct uart_dev_s *dev) * usartdiv32 = 32 * usartdiv = fCK / (baud/2) */ - usartdiv32 = priv->apbclock / (priv->baud >> 1); + usartdiv32 = priv->apbclock / (priv->baud >> 1); - /* The mantissa part is then */ + /* The mantissa part is then */ - mantissa = usartdiv32 >> 5; - brr = mantissa << USART_BRR_MANT_SHIFT; + mantissa = usartdiv32 >> 5; + brr = mantissa << USART_BRR_MANT_SHIFT; - /* The fractional remainder (with rounding) */ + /* The fractional remainder (with rounding) */ + + fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + brr |= fraction << USART_BRR_FRAC_SHIFT; + up_serialout(priv, STM32_USART_BRR_OFFSET, brr); + + /* Configure parity mode */ + + regval = up_serialin(priv, STM32_USART_CR1_OFFSET); + regval &= ~(USART_CR1_PCE|USART_CR1_PS); + + if (priv->parity == 1) /* Odd parity */ + { + regval |= (USART_CR1_PCE|USART_CR1_PS); + } + else if (priv->parity == 2) /* Even parity */ + { + regval |= USART_CR1_PCE; + } + + up_serialout(priv, STM32_USART_CR1_OFFSET, regval); + + /* Configure STOP bits */ + + regval = up_serialin(priv, STM32_USART_CR2_OFFSET); + regval &= ~(USART_CR2_STOP_MASK); + + if (priv->stopbits2) + { + regval |= USART_CR2_STOP2; + } + up_serialout(priv, STM32_USART_CR2_OFFSET, regval); + + /* Configure hardware flow control */ + + regval = up_serialin(priv, STM32_USART_CR3_OFFSET); + regval &= ~(USART_CR3_CTSE|USART_CR3_RTSE); + + if (priv->iflow && (priv->rts_gpio != 0)) + { + regval |= USART_CR3_RTSE; + } + if (priv->oflow && (priv->cts_gpio != 0)) + { + regval |= USART_CR3_CTSE; + } + + up_serialout(priv, STM32_USART_CR3_OFFSET, regval); - fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; - brr |= fraction << USART_BRR_FRAC_SHIFT; - up_serialout(priv, STM32_USART_BRR_OFFSET, brr); } -#endif +#endif /* CONFIG_SUPPRESS_UART_CONFIG */ /**************************************************************************** * Name: up_setup @@ -894,43 +947,28 @@ static int up_setup(struct uart_dev_s *dev) } /* Configure CR2 */ - /* Clear STOP, CLKEN, CPOL, CPHA, LBCL, and interrupt enable bits */ + /* Clear CLKEN, CPOL, CPHA, LBCL, and interrupt enable bits */ regval = up_serialin(priv, STM32_USART_CR2_OFFSET); - regval &= ~(USART_CR2_STOP_MASK|USART_CR2_CLKEN|USART_CR2_CPOL| + regval &= ~(USART_CR2_CLKEN|USART_CR2_CPOL| USART_CR2_CPHA|USART_CR2_LBCL|USART_CR2_LBDIE); - /* Configure STOP bits */ - - if (priv->stopbits2) - { - regval |= USART_CR2_STOP2; - } up_serialout(priv, STM32_USART_CR2_OFFSET, regval); /* Configure CR1 */ - /* Clear M, PCE, PS, TE, REm and all interrupt enable bits */ + /* Clear M, TE, REm and all interrupt enable bits */ regval = up_serialin(priv, STM32_USART_CR1_OFFSET); - regval &= ~(USART_CR1_M|USART_CR1_PCE|USART_CR1_PS|USART_CR1_TE| + regval &= ~(USART_CR1_M|USART_CR1_TE| USART_CR1_RE|USART_CR1_ALLINTS); - /* Configure word length and parity mode */ + /* Configure word length */ if (priv->bits == 9) /* Default: 1 start, 8 data, n stop */ { regval |= USART_CR1_M; /* 1 start, 9 data, n stop */ } - if (priv->parity == 1) /* Odd parity */ - { - regval |= (USART_CR1_PCE|USART_CR1_PS); - } - else if (priv->parity == 2) /* Even parity */ - { - regval |= USART_CR1_PCE; - } - up_serialout(priv, STM32_USART_CR1_OFFSET, regval); /* Configure CR3 */ @@ -943,16 +981,17 @@ static int up_setup(struct uart_dev_s *dev) up_serialout(priv, STM32_USART_CR3_OFFSET, regval); - /* Configure the USART Baud Rate. */ + /* Configure the USART line format and speed. */ - up_setspeed(dev); + up_set_format(dev); /* Enable Rx, Tx, and the USART */ regval = up_serialin(priv, STM32_USART_CR1_OFFSET); regval |= (USART_CR1_UE|USART_CR1_TE|USART_CR1_RE); up_serialout(priv, STM32_USART_CR1_OFFSET, regval); -#endif + +#endif /* CONFIG_SUPPRESS_UART_CONFIG */ /* Set up the cached interrupt enables value */ @@ -1279,12 +1318,21 @@ static int up_ioctl(struct file *filep, int cmd, unsigned long arg) break; } - /* TODO: Other termios fields are not yet returned. - * Note that only cfsetospeed is not necessary because we have - * knowledge that only one speed is supported. + cfsetispeed(termiosp, priv->baud); + + /* Note that since we only support 8/9 bit modes and + * there is no way to report 9-bit mode, we always claim 8. */ - cfsetispeed(termiosp, priv->baud); + termiosp->c_cflag = + ((priv->parity != 0) ? PARENB : 0) | + ((priv->parity == 1) ? PARODD : 0) | + ((priv->stopbits2) ? CSTOPB : 0) | + ((priv->oflow) ? CCTS_OFLOW : 0) | + ((priv->iflow) ? CRTS_IFLOW : 0) | + CS8; + + /* TODO: CCTS_IFLOW, CCTS_OFLOW */ } break; @@ -1298,16 +1346,48 @@ static int up_ioctl(struct file *filep, int cmd, unsigned long arg) break; } - /* TODO: Handle other termios settings. - * Note that only cfgetispeed is used besued we have knowledge + /* Perform some sanity checks before accepting any changes */ + + if (((termiosp->c_cflag & CSIZE) != CS8) || + ((termiosp->c_cflag & CCTS_OFLOW) && (priv->cts_gpio == 0)) || + ((termiosp->c_cflag & CRTS_IFLOW) && (priv->rts_gpio == 0))) + { + ret = -EINVAL; + break; + } + + if (termiosp->c_cflag & PARENB) + { + priv->parity = (termiosp->c_cflag & PARODD) ? 1 : 2; + } + else + { + priv->parity = 0; + } + + priv->stopbits2 = (termiosp->c_cflag & CSTOPB) != 0; + priv->oflow = (termiosp->c_cflag & CCTS_OFLOW) != 0; + priv->iflow = (termiosp->c_cflag & CRTS_IFLOW) != 0; + + /* Note that since there is no way to request 9-bit mode + * and no way to support 5/6/7-bit modes, we ignore them + * all here. + */ + + /* Note that only cfgetispeed is used because we have knowledge * that only one speed is supported. */ priv->baud = cfgetispeed(termiosp); - up_setspeed(dev); + + /* effect the changes immediately - note that we do not implement + * TCSADRAIN / TCSAFLUSH + */ + + up_set_format(dev); } break; -#endif +#endif /* CONFIG_SERIAL_TERMIOS */ #ifdef CONFIG_USART_BREAKS case TIOCSBRK: /* BSD compatibility: Turn break on, unconditionally */ @@ -1945,10 +2025,10 @@ void stm32_serial_dma_poll(void) int up_putc(int ch) { #if CONSOLE_UART > 0 -// struct up_dev_s *priv = uart_devs[CONSOLE_UART - 1]; -// uint16_t ie; + struct up_dev_s *priv = uart_devs[CONSOLE_UART - 1]; + uint16_t ie; -// up_disableusartint(priv, &ie); + up_disableusartint(priv, &ie); /* Check for LF */ @@ -1960,7 +2040,7 @@ int up_putc(int ch) } up_lowputc(ch); -// up_restoreusartint(priv, ie); + up_restoreusartint(priv, ie); #endif return ch; } diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index 40011199b1..c650da5db3 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -252,7 +252,7 @@ static inline ssize_t uart_irqwrite(FAR uart_dev_t *dev, FAR const char *buffer, { int ch = *buffer++; - /* If this is the console, then we should replace LF with CR-LF */ + /* assume that this is console text output and always do \n -> \r\n conversion */ if (ch == '\n') { @@ -277,6 +277,7 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t FAR uart_dev_t *dev = inode->i_private; ssize_t nread = buflen; int ret; + char ch; /* We may receive console writes through this path from interrupt handlers and * from debug output in the IDLE task! In these cases, we will need to do things @@ -308,8 +309,7 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t if (ret < 0) { /* A signal received while waiting for access to the xmit.head will - * abort the transfer. After the transfer has started, we are committed - * and signals will be ignored. + * abort the transfer. */ return ret; @@ -323,53 +323,64 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t uart_disabletxint(dev); for (; buflen; buflen--) { - int ch = *buffer++; + ch = *buffer++; - /* If the ONLCR flag is set, we should translate \n to \r\n */ + /* Do output post-processing */ - ret = OK; - if ((ch == '\n') && (dev->termios_s.c_oflag & ONLCR)) +#ifdef CONFIG_SERIAL_TERMIOS + + if (dev->tc_oflag & OPOST) + { + + /* Mapping CR to NL? */ + + if ((ch == '\r') && (dev->tc_oflag & OCRNL)) + { + ch = '\n'; + } + + /* Are we interested in newline processing? */ + + if ((ch == '\n') && (dev->tc_oflag & (ONLCR | ONLRET))) + { + ret = uart_putxmitchar(dev, '\r'); + + if (ret != OK) + { + break; + } + } + + /* Specifically not handled: + * + * OXTABS - primarily a full-screen terminal optimisation + * ONOEOT - Unix interoperability hack + * OLCUC - Not specified by Posix + * ONOCR - low-speed interactive optimisation + */ + + } + +#else /* !CONFIG_SERIAL_TERMIOS */ + + /* If this is the console, convert \n -> \r\n */ + + if (dev->isconsole && ch == '\n') { ret = uart_putxmitchar(dev, '\r'); } +#endif + /* Put the character into the transmit buffer */ - if (ret == OK) - { - ret = uart_putxmitchar(dev, ch); - } - - /* Were we awakened by a signal? That should be the only condition that - * uart_putxmitchar() should return an error. - */ - - if (ret < 0) - { - /* POSIX requires that we return -1 and errno set if no data was - * transferred. Otherwise, we return the number of bytes in the - * interrupted transfer. - */ - - if (buflen < nread) - { - /* Some data was transferred. Return the number of bytes that were - * successfully transferred. - */ - - nread -= buflen; - } - else - { - /* No data was transferred. Return -EINTR. The VFS layer will - * set the errno value appropriately). - */ - - nread = -EINTR; - } + ret = uart_putxmitchar(dev, ch); + if (ret != OK) + { break; } + } if (dev->xmit.head != dev->xmit.tail) @@ -378,6 +389,36 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t } uart_givesem(&dev->xmit.sem); + + /* Were we interrupted by a signal? That should be the only condition that + * uart_putxmitchar() should return an error. + */ + + if (ret < 0) + { + /* POSIX requires that we return -1 and errno set if no data was + * transferred. Otherwise, we return the number of bytes in the + * interrupted transfer. + */ + + if (buflen < nread) + { + /* Some data was transferred. Return the number of bytes that were + * successfully transferred. + */ + + nread -= buflen; + } + else + { + /* No data was transferred. Return -EINTR. The VFS layer will + * set the errno value appropriately). + */ + + nread = -EINTR; + } + } + return nread; } @@ -393,6 +434,7 @@ static ssize_t uart_read(FAR struct file *filep, FAR char *buffer, size_t buflen ssize_t recvd = 0; int16_t tail; int ret; + char ch; /* Only one user can access dev->recv.tail at a time */ @@ -430,8 +472,7 @@ static ssize_t uart_read(FAR struct file *filep, FAR char *buffer, size_t buflen { /* Take the next character from the tail of the buffer */ - *buffer++ = dev->recv.buffer[tail]; - recvd++; + ch = dev->recv.buffer[tail]; /* Increment the tail index. Most operations are done using the * local variable 'tail' so that the final dev->recv.tail update @@ -444,6 +485,49 @@ static ssize_t uart_read(FAR struct file *filep, FAR char *buffer, size_t buflen } dev->recv.tail = tail; + +#ifdef CONFIG_SERIAL_TERMIOS + + /* Do input processing if any is enabled */ + + if (dev->tc_iflag & (INLCR | IGNCR | ICRNL)) + { + + /* \n -> \r or \r -> \n translation? */ + + if ((ch == '\n') && (dev->tc_iflag & INLCR)) + { + ch = '\r'; + } + else if ((ch == '\r') && (dev->tc_iflag & ICRNL)) + { + ch = '\n'; + } + + /* discarding \r ? */ + if ((ch == '\r') & (dev->tc_iflag & IGNCR)) + { + continue; + } + + } + + /* Specifically not handled: + * + * All of the local modes; echo, line editing, etc. + * Anything to do with break or parity errors. + * ISTRIP - we should be 8-bit clean. + * IUCLC - Not Posix + * IXON/OXOFF - no xon/xoff flow control. + */ + +#endif + + /* store the received character */ + + *buffer++ = ch; + recvd++; + } #ifdef CONFIG_DEV_SERIAL_FULLBLOCKS @@ -573,43 +657,77 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) /* Handle TTY-level IOCTLs here */ /* Let low-level driver handle the call first */ + int ret = dev->ops->ioctl(filep, cmd, arg); + /* Append any higher level TTY flags */ - switch (cmd) + + if (ret == OK) { - case TCGETS: - { - struct termios *termiosp = (struct termios*)arg; + switch (cmd) + { - if (!termiosp) - { - ret = -EINVAL; - break; - } + case FIONREAD: + { + int count; + irqstate_t state = irqsave(); - /* Fetch the out flags */ - termiosp->c_oflag = dev->termios_s.c_oflag; - /* Fetch the in flags */ - termiosp->c_iflag = dev->termios_s.c_iflag; - } - break; + /* determine the number of bytes available in the buffer */ - case TCSETS: - { - struct termios *termiosp = (struct termios*)arg; + if (dev->recv.tail <= dev->recv.head) + { + count = dev->recv.head - dev->recv.tail; + } + else + { + count = dev->recv.size - (dev->recv.tail - dev->recv.head); + } - if (!termiosp) - { - ret = -EINVAL; - break; - } + irqrestore(state); - /* Set the out flags */ - dev->termios_s.c_oflag = termiosp->c_oflag; - /* Set the in flags */ - dev->termios_s.c_iflag = termiosp->c_iflag; - } - break; + *(int *)arg = count; + } + +#ifdef CONFIG_SERIAL_TERMIOS + case TCGETS: + { + struct termios *termiosp = (struct termios*)arg; + + if (!termiosp) + { + ret = -EINVAL; + break; + } + + /* and update with flags from this layer */ + + termiosp->c_iflag = dev->tc_iflag; + termiosp->c_oflag = dev->tc_oflag; + termiosp->c_lflag = dev->tc_lflag; + } + + break; + + case TCSETS: + { + struct termios *termiosp = (struct termios*)arg; + + if (!termiosp) + { + ret = -EINVAL; + break; + } + + /* update the flags we keep at this layer */ + + dev->tc_iflag = termiosp->c_iflag; + dev->tc_oflag = termiosp->c_oflag; + dev->tc_lflag = termiosp->c_lflag; + } + + break; +#endif + } } return ret; } @@ -900,6 +1018,25 @@ static int uart_open(FAR struct file *filep) dev->recv.head = 0; dev->recv.tail = 0; + /* initialise termios state */ + +#ifdef CONFIG_SERIAL_TERMIOS + + dev->tc_iflag = 0; + if (dev->isconsole == true) + { + + /* enable \n -> \r\n translation for the console */ + + dev->tc_oflag = OPOST | ONLCR; + } + else + { + dev->tc_oflag = 0; + } + +#endif + /* Enable the RX interrupt */ uart_enablerxint(dev); @@ -937,14 +1074,22 @@ int uart_register(FAR const char *path, FAR uart_dev_t *dev) #ifndef CONFIG_DISABLE_POLL sem_init(&dev->pollsem, 0, 1); #endif + /* Setup termios flags */ - memset(&dev->termios_s, 0, sizeof(dev->termios_s)); + +#ifdef CONFIG_SERIAL_TERMIOS + if (dev->isconsole == true) { - /* Device is console, set up termios flags */ - dev->termios_s.c_oflag |= ONLCR; + + /* enable \n -> \r\n translation for the console as early as possible */ + + dev->tc_oflag = OPOST | ONLCR; + dev->tc_iflag = 0; } +#endif + dbg("Registering %s\n", path); return register_driver(path, &g_serialops, 0666, dev); } diff --git a/nuttx/include/nuttx/fs/ioctl.h b/nuttx/include/nuttx/fs/ioctl.h index 19f29b1fb6..08f62e1648 100644 --- a/nuttx/include/nuttx/fs/ioctl.h +++ b/nuttx/include/nuttx/fs/ioctl.h @@ -106,6 +106,10 @@ * OUT: None */ +#define FIONREAD _FIOC(0x0004) /* IN: Location to return value (int *) + * OUT: Bytes readable from this fd + */ + /* NuttX file system ioctl definitions **************************************/ #define _DIOCVALID(c) (_IOC_TYPE(c)==_DIOCBASE) diff --git a/nuttx/include/nuttx/serial/serial.h b/nuttx/include/nuttx/serial/serial.h index 67a7f9d992..49dba37953 100644 --- a/nuttx/include/nuttx/serial/serial.h +++ b/nuttx/include/nuttx/serial/serial.h @@ -46,7 +46,9 @@ #include #include #include -#include +#ifdef CONFIG_SERIAL_TERMIOS +# include +#endif #include /************************************************************************************ @@ -216,7 +218,12 @@ struct uart_dev_s #endif /* Terminal control flags */ - struct termios termios_s; + +#ifdef CONFIG_SERIAL_TERMIOS + tcflag_t tc_iflag; /* Input modes */ + tcflag_t tc_oflag; /* Output modes */ + tcflag_t tc_lflag; /* Local modes */ +#endif }; typedef struct uart_dev_s uart_dev_t; diff --git a/nuttx/include/termios.h b/nuttx/include/termios.h index e381814e3c..976da7970e 100644 --- a/nuttx/include/termios.h +++ b/nuttx/include/termios.h @@ -58,7 +58,7 @@ #define INLCR (1 << 5) /* Bit 5: Map NL to CR on input */ #define INPCK (1 << 6) /* Bit 6: Enable input parity check */ #define ISTRIP (1 << 7) /* Bit 7: Strip character */ -#define IUCLC (1 << 8) /* Bit 8: Map upper-case to lower-case on input (LEGACY) */ + /* Bit 8: unused */ #define IXANY (1 << 9) /* Bit 9: Enable any character to restart output */ #define IXOFF (1 << 10) /* Bit 10: Enable start/stop input control */ #define IXON (1 << 11) /* Bit 11: Enable start/stop output control */ @@ -67,7 +67,7 @@ /* Terminal output modes (c_oflag in the termios structure) */ #define OPOST (1 << 0) /* Bit 0: Post-process output */ -#define OLCUC (1 << 1) /* Bit 1: Map lower-case to upper-case on output (LEGACY) */ + /* Bit 1: unused */ #define ONLCR (1 << 2) /* Bit 2: Map NL to CR-NL on output */ #define OCRNL (1 << 3) /* Bit 3: Map CR to NL on output */ #define ONOCR (1 << 4) /* Bit 4: No CR output at column 0 */ @@ -98,17 +98,20 @@ /* Control Modes (c_cflag in the termios structure) */ -#define CSIZE (3 << 0) /* Bits 0-1: Character size: */ -# define CS5 (0 << 0) /* 5 bits */ -# define CS6 (1 << 0) /* 6 bits */ -# define CS7 (2 << 0) /* 7 bits */ -# define CS8 (3 << 0) /* 8 bits */ -#define CSTOPB (1 << 2) /* Bit 2: Send two stop bits, else one */ -#define CREAD (1 << 3) /* Bit 3: Enable receiver */ -#define PARENB (1 << 4) /* Bit 4: Parity enable */ -#define PARODD (1 << 5) /* Bit 5: Odd parity, else even */ -#define HUPCL (1 << 6) /* Bit 6: Hang up on last close */ -#define CLOCAL (1 << 7) /* Bit 7: Ignore modem status lines */ +#define CSIZE (3 << 0) /* Bits 0-1: Character size: */ +# define CS5 (0 << 0) /* 5 bits */ +# define CS6 (1 << 0) /* 6 bits */ +# define CS7 (2 << 0) /* 7 bits */ +# define CS8 (3 << 0) /* 8 bits */ +#define CSTOPB (1 << 2) /* Bit 2: Send two stop bits, else one */ +#define CREAD (1 << 3) /* Bit 3: Enable receiver */ +#define PARENB (1 << 4) /* Bit 4: Parity enable */ +#define PARODD (1 << 5) /* Bit 5: Odd parity, else even */ +#define HUPCL (1 << 6) /* Bit 6: Hang up on last close */ +#define CLOCAL (1 << 7) /* Bit 7: Ignore modem status lines */ +#define CCTS_OFLOW (1 << 8) /* Bit 8: CTS flow control of output */ +#define CRTSCTS CCTS_OFLOW +#define CRTS_IFLOW (1 << 9) /* Bit 9: RTS flow control of input */ /* Local Modes (c_lflag in the termios structure) */ @@ -121,7 +124,6 @@ #define ISIG (1 << 6) /* Bit 6: Enable signals */ #define NOFLSH (1 << 7) /* Bit 7: Disable flush after interrupt or quit */ #define TOSTOP (1 << 8) /* Bit 8: Send SIGTTOU for background output */ -#define XCASE (1 << 9) /* Bit 9: Canonical upper/lower presentation (LEGACY) */ /* The following are subscript names for the termios c_cc array */ From 92e1d5eb78d9d04a89b0413718c8bab6e9af7f63 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 28 Nov 2012 20:12:36 -0800 Subject: [PATCH 02/21] Possible fix for #78 - increase the wait timeout for discard when flashing PX4IO. It's not clear this solves the issue, but I can't reproduce it with this added. --- apps/drivers/px4io/uploader.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp index 5669aeb015..8b354ff600 100644 --- a/apps/drivers/px4io/uploader.cpp +++ b/apps/drivers/px4io/uploader.cpp @@ -189,8 +189,10 @@ PX4IO_Uploader::drain() int ret; do { - ret = recv(c, 10); - //log("discard 0x%02x", c); + ret = recv(c, 250); + if (ret == OK) { + //log("discard 0x%02x", c); + } } while (ret == OK); } From 8c0c979655bcdcb7277e7ebbd79852e09a8c87e0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 29 Nov 2012 00:33:20 -0800 Subject: [PATCH 03/21] Better sizing for PX4IO serial buffers. --- nuttx/configs/px4io/io/defconfig | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index e90e7ce62b..d354ebfe57 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -161,13 +161,13 @@ CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n -CONFIG_USART1_TXBUFSIZE=32 -CONFIG_USART2_TXBUFSIZE=32 -CONFIG_USART3_TXBUFSIZE=32 +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=128 -CONFIG_USART3_RXBUFSIZE=32 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 CONFIG_USART1_BAUD=115200 CONFIG_USART2_BAUD=115200 From 03a82e0a039d8ce3bea5cc97c181d649b97d6edf Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 29 Nov 2012 00:33:44 -0800 Subject: [PATCH 04/21] Fix includes for debug output. --- apps/px4io/px4io.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index f50e29252a..0a3a4448c6 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -56,10 +56,11 @@ * Debug logging */ -#if 1 -# define debug(fmt, ...) lib_lowprintf(fmt "\n", ##args) +#ifdef DEBUG +# include +# define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args) #else -# define debug(fmt, ...) do {} while(0) +# define debug(fmt, args...) do {} while(0) #endif /* From 3321ca08886fe1030c82094254586c31fcb07b32 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 29 Nov 2012 00:34:44 -0800 Subject: [PATCH 05/21] Don't print the status line so much; we seem to drop a lot of receive characters this way. --- apps/px4io/px4io.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index dec2cdde67..197bad7c6d 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -131,6 +131,7 @@ int user_start(int argc, char *argv[]) LED_AMBER(failsafe); /* print some simple status */ +#if 0 if (timers[TIMER_STATUS_PRINT] == 0) { timers[TIMER_STATUS_PRINT] = 1000; lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r", @@ -143,6 +144,7 @@ int user_start(int argc, char *argv[]) frame_rx, frame_bad ); } +#endif } /* Should never reach here */ From d0efd1a419497ec2fadb7b516cd2f9cc5a09ca5d Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 29 Nov 2012 00:35:21 -0800 Subject: [PATCH 06/21] Fix the DSM (spektrum) protocol decoder, and add some format auto-detection to it. --- apps/drivers/px4io/px4io.cpp | 32 ++++--- apps/px4io/comms.c | 7 +- apps/px4io/dsm.c | 156 ++++++++++++++++++++++++++++++----- apps/px4io/protocol.h | 5 +- 4 files changed, 158 insertions(+), 42 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f2c87473c4..11f2f6f688 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -637,10 +637,12 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) void PX4IO::set_rx_mode(unsigned mode) { - if (mode != _rx_mode) { - _rx_mode = mode; - _config_needed = true; - } + /* + * Always (re)set the rx mode; makes testing + * easier after PX4IO has been restarted. + */ + _rx_mode = mode; + _config_needed = true; } extern "C" __EXPORT int px4io_main(int argc, char *argv[]); @@ -744,25 +746,29 @@ px4io_main(int argc, char *argv[]) return ret; } - if (!strcmp(argv[1], "rx_dsm_10bit")) { + if (!strcmp(argv[1], "rx_dsm") || + !strcmp(argv[1], "rx_dsm_10bit") || + !strcmp(argv[1], "rx_dsm_11bit")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_DSM_10BIT); - } - if (!strcmp(argv[1], "rx_dsm_11bit")) { - if (g_dev == nullptr) - errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_DSM_11BIT); + g_dev->set_rx_mode(RX_MODE_DSM); + exit(0); } if (!strcmp(argv[1], "rx_sbus")) { if (g_dev == nullptr) errx(1, "not started"); g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); + exit(0); + } + if (!strcmp(argv[1], "rx_ppm")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_PPM_ONLY); + exit(0); } if (!strcmp(argv[1], "test")) test(); - - errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'"); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index f1dac433fb..c44b677d17 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -112,9 +112,7 @@ serial_rx_init(unsigned serial_mode) tcgetattr(rx_fd, &t); switch (serial_mode) { - case RX_MODE_DSM_10BIT: - case RX_MODE_DSM_11BIT: - + case RX_MODE_DSM: /* 115200, no parity, one stop bit */ cfsetspeed(&t, 115200); t.c_cflag &= ~(CSTOPB | PARENB); @@ -163,8 +161,7 @@ comms_check(void) */ if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) { switch (system_state.serial_rx_mode) { - case RX_MODE_DSM_10BIT: - case RX_MODE_DSM_11BIT: + case RX_MODE_DSM: dsm_input(rx_fd); break; diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 79b6301c7d..4384ab5239 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -49,12 +49,15 @@ #include +#define DEBUG + #include "px4io.h" #include "protocol.h" #define DSM_FRAME_SIZE 16 #define DSM_FRAME_CHANNELS 7 +static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; static uint8_t frame[DSM_FRAME_SIZE]; @@ -63,21 +66,21 @@ static unsigned partial_frame_count; static bool insync; static unsigned channel_shift; -static void dsm_decode(void); +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); void dsm_init(unsigned mode) { insync = false; partial_frame_count = 0; + last_rx_time = hrt_absolute_time(); - if (mode == RX_MODE_DSM_10BIT) { - channel_shift = 10; - } else { - channel_shift = 11; - } + /* reset the format detector */ + dsm_guess_format(true); - last_frame_time = hrt_absolute_time(); + debug("DSM: enabled and waiting\n"); } void @@ -97,10 +100,17 @@ dsm_input(int fd) * We expect to only be called when bytes arrive for processing, * and if an interval of more than 5ms passes between calls, * the first byte we read will be the first byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... */ now = hrt_absolute_time(); - if ((now - last_frame_time) > 5000) + if ((now - last_rx_time) > 5000) { + if (partial_frame_count > 0) + debug("DSM: reset @ %d", partial_frame_count); partial_frame_count = 0; + } /* * Fetch bytes, but no more than we would need to complete @@ -111,6 +121,7 @@ dsm_input(int fd) /* if the read failed for any reason, just give up here */ if (ret < 1) return; + last_rx_time = now; /* * Add bytes to the current frame @@ -123,20 +134,125 @@ dsm_input(int fd) */ if (partial_frame_count < DSM_FRAME_SIZE) return; - last_frame_time = now; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - dsm_decode(); + dsm_decode(now); partial_frame_count = 0; } -static void -dsm_decode(void) +static bool +dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { - uint16_t data_mask = (1 << channel_shift) - 1; + + if (raw == 0xffff) + return false; + + *channel = (raw >> shift) & 0xf; + + uint16_t data_mask = (1 << shift) - 1; + *value = raw & data_mask; + + //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); + + return true; +} + +static void +dsm_guess_format(bool reset) +{ + static uint32_t cs10; + static uint32_t cs11; + static unsigned samples; + + /* reset the 10/11 bit sniffed channel masks */ + if (reset) { + cs10 = 0; + cs11 = 0; + samples = 0; + channel_shift = 0; + return; + } + + /* scan the channels in the current frame in both 10- and 11-bit mode */ + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + + uint8_t *dp = &frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; + + /* if the channel decodes, remember the assigned number */ + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) + cs10 |= (1 << channel); + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) + cs11 |= (1 << channel); + + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */ + } + + /* wait until we have seen plenty of frames - 2 should normally be enough */ + if (samples++ < 5) + return; + + /* + * Iterate the set of sensible sniffed channel sets and see whether + * decoding in 10 or 11-bit mode has yielded anything we recognise. + */ + static uint32_t masks[] = { + 0x3f, /* 6 channels (DX6) */ + 0x7f, /* 7 channels (DX7) */ + 0xff, /* 8 channels (DX8) */ + 0x3ff, /* 10 channels (DX10) */ + 0x3fff /* 18 channels (DX10) */ + }; + unsigned votes10 = 0; + unsigned votes11 = 0; + + for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) { + + if (cs10 == masks[i]) + votes10++; + if (cs11 == masks[i]) + votes11++; + } + if ((votes11 == 1) && (votes10 == 0)) { + channel_shift = 11; + debug("DSM: detected 11-bit format"); + return; + } + if ((votes10 == 1) && (votes11 == 0)) { + channel_shift = 10; + debug("DSM: detected 10-bit format"); + return; + } + + /* call ourselves to reset our state ... we have to try again */ + debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + dsm_guess_format(true); +} + +static void +dsm_decode(hrt_abstime frame_time) +{ + +/* + debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], + frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); +*/ + /* + * If we have lost signal for at least a second, reset the + * format guessing heuristic. + */ + if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) + dsm_guess_format(true); + last_frame_time = frame_time; + if (channel_shift == 0) { + dsm_guess_format(false); + return; + } /* * The encoding of the first byte is uncertain, so we're going @@ -159,26 +275,24 @@ dsm_decode(void) uint8_t *dp = &frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; - /* ignore pad channels */ - if (raw == 0xffff) + if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) continue; - unsigned channel = (raw >> channel_shift) & 0xf; - /* ignore channels out of range */ if (channel >= PX4IO_INPUT_CHANNELS) continue; + /* update the decoded channel count */ if (channel > ppm_decoded_channels) ppm_decoded_channels = channel; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - unsigned data = raw & data_mask; if (channel_shift == 11) - data /= 2; - ppm_buffer[channel] = 988 + data; - + value /= 2; + ppm_buffer[channel] = 988 + value; } + ppm_last_valid_decode = hrt_absolute_time(); } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 660eed12b6..8d051d3857 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -64,9 +64,8 @@ struct px4io_config { uint8_t serial_rx_mode; #define RX_MODE_PPM_ONLY 0 -#define RX_MODE_DSM_10BIT 1 -#define RX_MODE_DSM_11BIT 2 -#define RX_MODE_FUTABA_SBUS 3 +#define RX_MODE_DSM 1 +#define RX_MODE_FUTABA_SBUS 2 }; /* report from IO to FMU */ From 9c8101d4f189837fdf9b54e6b529ff3962f56c1c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 29 Nov 2012 10:18:21 -0800 Subject: [PATCH 07/21] Add some more information to comments. --- apps/px4io/dsm.c | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 4384ab5239..0e9fd25f1c 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -199,6 +199,13 @@ dsm_guess_format(bool reset) /* * Iterate the set of sensible sniffed channel sets and see whether * decoding in 10 or 11-bit mode has yielded anything we recognise. + * + * XXX Note that due to what seem to be bugs in the DSM2 high-resolution + * stream, we may want to sniff for longer in some cases when we think we + * are talking to a DSM2 receiver in high-resolution mode (so that we can + * reject it, ideally). + * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion + * of this issue. */ static uint32_t masks[] = { 0x3f, /* 6 channels (DX6) */ @@ -255,14 +262,8 @@ dsm_decode(hrt_abstime frame_time) } /* - * The encoding of the first byte is uncertain, so we're going - * to ignore it for now. - * - * The second byte may tell us about the protocol, but it's not - * actually very interesting since what we really want to know - * is how the channel data is formatted, and there doesn't seem - * to be a reliable way to determine this from the protocol ID - * alone. + * The encoding of the first two bytes is uncertain, so we're + * going to ignore them for now. * * Each channel is a 16-bit unsigned value containing either a 10- * or 11-bit channel value and a 4-bit channel number, shifted From e153476950a3fbda230c6bddd9ad35018cfda559 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 00:00:02 -0800 Subject: [PATCH 08/21] Config changes for PX4IO: - enable USART3 for S.bus - reduce the scheduling tick to 1ms - disable RR scheduling - reduce stdio buffer sizes (not used) --- nuttx/configs/px4io/io/defconfig | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index d354ebfe57..1ac70f8abf 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -124,7 +124,7 @@ CONFIG_STM32_TIM7=n CONFIG_STM32_WWDG=n CONFIG_STM32_SPI2=n CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=n +CONFIG_STM32_USART3=y CONFIG_STM32_I2C1=y CONFIG_STM32_I2C2=n CONFIG_STM32_BKP=n @@ -341,26 +341,28 @@ CONFIG_DEBUG_CAN=n CONFIG_DEBUG_I2C=n CONFIG_DEBUG_INPUT=n +CONFIG_MSEC_PER_TICK=1 CONFIG_HAVE_CXX=n CONFIG_HAVE_CXXINITIALIZE=n CONFIG_MM_REGIONS=1 CONFIG_MM_SMALL=y CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=200 +CONFIG_RR_INTERVAL=0 CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=0 +CONFIG_TASK_NAME_SIZE=8 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 CONFIG_GREGORIAN_TIME=n CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why CONFIG_DEV_CONSOLE=y CONFIG_DEV_LOWCONSOLE=n CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=n CONFIG_SEM_PREALLOCHOLDERS=0 CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_DISABLE=y CONFIG_FDCLONE_STDIO=y CONFIG_SDCLONE_DISABLE=y CONFIG_SCHED_WORKQUEUE=n @@ -469,7 +471,7 @@ CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=8 CONFIG_NFILE_STREAMS=0 CONFIG_NAME_MAX=32 -CONFIG_STDIO_BUFFER_SIZE=64 +CONFIG_STDIO_BUFFER_SIZE=32 CONFIG_STDIO_LINEBUFFER=n CONFIG_NUNGET_CHARS=2 CONFIG_PREALLOC_MQ_MSGS=4 From 9fa794a8faa2d30023d9943beae55a05ed4e48a0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 00:02:47 -0800 Subject: [PATCH 09/21] Rework the PX4IO software architecture: - Use a separate thread for handing R/C inputs and outputs. - Remove all PX4IO R/C receiver configuration; it's all automatic now. - Rework the main loop, dedicate it to PX4FMU communications after startup. - Fix several issues in the px4io driver that would cause a crash if PX4IO was not responding. --- apps/drivers/px4io/px4io.cpp | 96 ++++++++-------------- apps/px4io/comms.c | 151 +++++++++-------------------------- apps/px4io/controls.c | 88 ++++++++++++++++++++ apps/px4io/dsm.c | 56 +++++++++---- apps/px4io/mixer.c | 27 +------ apps/px4io/protocol.h | 5 +- apps/px4io/px4io.c | 93 ++++----------------- apps/px4io/px4io.h | 15 ++-- apps/px4io/safety.c | 36 +++++++++ apps/px4io/sbus.c | 29 ++++++- 10 files changed, 288 insertions(+), 308 deletions(-) create mode 100644 apps/px4io/controls.c diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 11f2f6f688..80a61e3037 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -87,15 +87,13 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); - void set_rx_mode(unsigned mode); - private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; int _serial_fd; ///< serial interface to PX4IO hx_stream_t _io_stream; ///< HX protocol stream - int _task; ///< worker task + volatile int _task; ///< worker task volatile bool _task_should_exit; volatile bool _connected; ///< true once we have received a valid frame @@ -121,8 +119,6 @@ private: 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 - uint8_t _rx_mode; ///< the current RX mode on IO - /** * Trampoline to the worker task */ @@ -190,8 +186,7 @@ PX4IO::PX4IO() : _primary_pwm_device(false), _switch_armed(false), _send_needed(false), - _config_needed(false), - _rx_mode(RX_MODE_PPM_ONLY) + _config_needed(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -201,33 +196,17 @@ PX4IO::PX4IO() : PX4IO::~PX4IO() { - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; + /* tell the task we want it to go away */ + _task_should_exit = true; - /* spin waiting for the thread to stop */ - unsigned i = 10; - - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); } - - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - /* kill the HX stream */ - if (_io_stream != nullptr) - hx_stream_free(_io_stream); + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + task_delete(_task); g_dev = nullptr; } @@ -295,6 +274,16 @@ PX4IO::task_main() 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) { @@ -402,8 +391,16 @@ 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); /* tell the dtor that we are exiting */ _task = -1; @@ -514,7 +511,6 @@ PX4IO::config_send() int ret; cfg.f2i_config_magic = F2I_CONFIG_MAGIC; - cfg.serial_rx_mode = _rx_mode; ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); if (ret) @@ -634,17 +630,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } -void -PX4IO::set_rx_mode(unsigned mode) -{ - /* - * Always (re)set the rx mode; makes testing - * easier after PX4IO has been restarted. - */ - _rx_mode = mode; - _config_needed = true; -} - extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace @@ -748,25 +733,10 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || - !strcmp(argv[1], "rx_dsm_11bit")) { - if (g_dev == nullptr) - errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_DSM); - exit(0); - } - if (!strcmp(argv[1], "rx_sbus")) { - if (g_dev == nullptr) - errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); - exit(0); - } - if (!strcmp(argv[1], "rx_ppm")) { - if (g_dev == nullptr) - errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_PPM_ONLY); - exit(0); - } - + !strcmp(argv[1], "rx_dsm_11bit") || + !strcmp(argv[1], "rx_sbus") || + !strcmp(argv[1], "rx_ppm")) + errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); if (!strcmp(argv[1], "test")) test(); diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index c44b677d17..a93ef9cb8f 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -32,10 +32,11 @@ ****************************************************************************/ /** - * @file FMU communication for the PX4IO module. + * @file comms.c + * + * FMU communication for the PX4IO module. */ - #include #include #include @@ -44,7 +45,6 @@ #include #include #include -#include #include #include @@ -54,101 +54,50 @@ #include #include +#define DEBUG #include "px4io.h" #define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */ #define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */ +int frame_rx; +int frame_bad; + static int fmu_fd; static hx_stream_t stream; -static int rx_fd; static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); -static struct pollfd pollfds[2]; -static int pollcount; - void comms_init(void) { /* initialise the FMU interface */ fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK); - if (fmu_fd < 0) - lib_lowprintf("COMMS: fmu open failed %d\n", errno); stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); - pollfds[0].fd = fmu_fd; - pollfds[0].events = POLLIN; - pollcount = 1; /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; - -} - -static void -serial_rx_init(unsigned serial_mode) -{ - if (serial_mode == system_state.serial_rx_mode) - return; - system_state.serial_rx_mode = serial_mode; - - if (serial_mode == RX_MODE_PPM_ONLY) { - if (rx_fd != -1) { - pollcount = 1; - close(rx_fd); - rx_fd = -1; - } - return; - } - - /* open the serial port used for DSM and S.bus communication */ - rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); - pollfds[1].fd = rx_fd; - pollfds[1].events = POLLIN; - pollcount = 2; - - struct termios t; - tcgetattr(rx_fd, &t); - - switch (serial_mode) { - case RX_MODE_DSM: - /* 115200, no parity, one stop bit */ - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - - dsm_init(serial_mode); - break; - - case RX_MODE_FUTABA_SBUS: - /* 100000, even parity, two stop bits */ - cfsetspeed(&t, 100000); - t.c_cflag |= (CSTOPB | PARENB); - - sbus_init(serial_mode); - break; - - default: - break; - } - - tcsetattr(rx_fd, TCSANOW, &t); } void -comms_check(void) +comms_main(void) { - /* - * Check for serial data - */ - int ret = poll(pollfds, pollcount, 0); - if (ret > 0) { + struct pollfd fds; + fds.fd = fmu_fd; + fds.events = POLLIN; + debug("FMU: ready"); + + for (;;) { + /* wait for serial data, but no more than 100ms */ + poll(&fds, 1, 100); + /* * Pull bytes from FMU and feed them to the HX engine. * Limit the number of bytes we actually process on any one iteration. */ - if (pollfds[0].revents & POLLIN) { + if (fds.revents & POLLIN) { char buf[32]; ssize_t count = read(fmu_fd, buf, sizeof(buf)); for (int i = 0; i < count; i++) @@ -156,54 +105,32 @@ comms_check(void) } /* - * Pull bytes from the serial RX port and feed them to the decoder - * if we care about serial RX data. + * Decide if it's time to send an update to the FMU. */ - if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) { - switch (system_state.serial_rx_mode) { - case RX_MODE_DSM: - dsm_input(rx_fd); - break; + static hrt_abstime last_report_time; + hrt_abstime now, delta; - case RX_MODE_FUTABA_SBUS: - sbus_input(rx_fd); - break; + /* 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))) { - default: - break; - } + system_state.fmu_report_due = false; + last_report_time = now; + + /* populate the report */ + for (int 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; + + /* and send it */ + hx_stream_send(stream, &report, sizeof(report)); } } - - /* - * 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 (int 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; - - /* and send it */ - hx_stream_send(stream, &report, sizeof(report)); - } } -int frame_rx; -int frame_bad; - static void comms_handle_config(const void *buffer, size_t length) { @@ -215,8 +142,6 @@ comms_handle_config(const void *buffer, size_t length) } frame_rx++; - - serial_rx_init(cfg->serial_rx_mode); } static void @@ -274,9 +199,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) comms_handle_config(buffer, length); break; default: + frame_bad++; break; } } - frame_bad++; } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c new file mode 100644 index 0000000000..d4eace3df5 --- /dev/null +++ b/apps/px4io/controls.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file controls.c + * + * R/C inputs and servo outputs. + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#define DEBUG +#include "px4io.h" + +void +controls_main(void) +{ + struct pollfd fds[2]; + + fds[0].fd = dsm_init("/dev/ttyS0"); + fds[0].events = POLLIN; + + + fds[1].fd = sbus_init("/dev/ttyS2"); + fds[1].events = POLLIN; + + for (;;) { + /* run this loop at ~100Hz */ + poll(fds, 2, 10); + + if (fds[0].revents & POLLIN) + dsm_input(); + if (fds[1].revents & POLLIN) + sbus_input(); + + /* XXX do ppm processing, bypass mode, etc. here */ + + /* do PWM output updates */ + mixer_tick(); + } +} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 0e9fd25f1c..da7d1a3618 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -44,6 +44,7 @@ #include #include #include +#include #include @@ -57,6 +58,8 @@ #define DSM_FRAME_SIZE 16 #define DSM_FRAME_CHANNELS 7 +static int dsm_fd = -1; + static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; @@ -66,27 +69,46 @@ static unsigned partial_frame_count; static bool insync; static unsigned channel_shift; +unsigned dsm_frame_drops; + static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); static void dsm_guess_format(bool reset); static void dsm_decode(hrt_abstime now); -void -dsm_init(unsigned mode) +int +dsm_init(const char *device) { - insync = false; - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); + if (dsm_fd < 0) + dsm_fd = open(device, O_RDONLY); - /* reset the format detector */ - dsm_guess_format(true); + if (dsm_fd >= 0) { + struct termios t; - debug("DSM: enabled and waiting\n"); + /* 115200bps, no parity, one stop bit */ + tcgetattr(dsm_fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(dsm_fd, TCSANOW, &t); + + /* initialise the decoder */ + insync = false; + partial_frame_count = 0; + last_rx_time = hrt_absolute_time(); + + /* reset the format detector */ + dsm_guess_format(true); + + debug("DSM: ready"); + } else { + debug("DSM: open failed"); + } + + return dsm_fd; } void -dsm_input(int fd) +dsm_input(void) { - uint8_t buf[DSM_FRAME_SIZE]; ssize_t ret; hrt_abstime now; @@ -107,16 +129,17 @@ dsm_input(int fd) */ now = hrt_absolute_time(); if ((now - last_rx_time) > 5000) { - if (partial_frame_count > 0) - debug("DSM: reset @ %d", partial_frame_count); - partial_frame_count = 0; + if (partial_frame_count > 0) { + dsm_frame_drops++; + partial_frame_count = 0; + } } /* * Fetch bytes, but no more than we would need to complete * the current frame. */ - ret = read(fd, buf, DSM_FRAME_SIZE - partial_frame_count); + ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ if (ret < 1) @@ -126,7 +149,6 @@ dsm_input(int fd) /* * Add bytes to the current frame */ - memcpy(&frame[partial_frame_count], buf, ret); partial_frame_count += ret; /* @@ -292,8 +314,12 @@ dsm_decode(hrt_abstime frame_time) /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) value /= 2; + + /* stuff the decoded channel into the PPM input buffer */ ppm_buffer[channel] = 988 + 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 */ ppm_last_valid_decode = hrt_absolute_time(); } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 572344d000..61e716474d 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -60,17 +60,6 @@ */ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 - -/* - * HRT periodic call used to check for control input data. - */ -static struct hrt_call mixer_input_call; - -/* - * Mixer periodic tick. - */ -static void mixer_tick(void *arg); - /* * Collect RC input data from the controller source(s). */ @@ -92,20 +81,8 @@ struct mixer { /* XXX more config here */ } mixers[IO_SERVO_COUNT]; -int -mixer_init(void) -{ - - - /* look for control data at 50Hz */ - hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); - - return 0; -} - - -static void -mixer_tick(void *arg) +void +mixer_tick(void) { uint16_t *control_values; int control_count; diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 8d051d3857..c704b12016 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -62,10 +62,7 @@ struct px4io_config { uint16_t f2i_config_magic; #define F2I_CONFIG_MAGIC 0x6366 - uint8_t serial_rx_mode; -#define RX_MODE_PPM_ONLY 0 -#define RX_MODE_DSM 1 -#define RX_MODE_FUTABA_SBUS 2 + /* XXX currently nothing here */ }; /* report from IO to FMU */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 197bad7c6d..ba16ce2472 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -55,37 +55,15 @@ __EXPORT int user_start(int argc, char *argv[]); struct sys_state_s system_state; -int gpio_fd; - -static const char cursor[] = {'|', '/', '-', '\\'}; - -static struct hrt_call timer_tick_call; -volatile int timers[TIMER_NUM_TIMERS]; -static void timer_tick(void *arg); int user_start(int argc, char *argv[]) { - int cycle = 0; - bool heartbeat = false; - bool failsafe = false; /* configure the high-resolution time/callout interface */ hrt_init(); - /* init the FMU and receiver links */ - comms_init(); - - /* configure the first 8 PWM outputs (i.e. all of them) */ - /* note, must do this after comms init to steal back PA0, which is CTS otherwise */ - up_pwm_servo_init(0xff); - /* print some startup info */ lib_lowprintf("\nPX4IO: starting\n"); - struct mallinfo minfo = mallinfo(); - lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); - - /* start the software timer service */ - hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL); /* default all the LEDs to off while we start */ LED_AMBER(false); @@ -95,66 +73,27 @@ int user_start(int argc, char *argv[]) /* turn on servo power */ POWER_SERVO(true); - /* start the mixer */ - mixer_init(); - /* start the safety switch handler */ safety_init(); - /* set up some timers for the main loop */ - timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */ - timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */ + /* start the flight control signal handler */ + task_create("FCon", + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)controls_main, + NULL); - /* - * Main loop servicing communication with FMU - */ - while(true) { - /* check for communication from FMU, send updates */ - comms_check(); + /* initialise the FMU communications interface */ + comms_init(); - /* blink the heartbeat LED */ - if (timers[TIMER_BLINK_BLUE] == 0) { - timers[TIMER_BLINK_BLUE] = 250; - LED_BLUE(heartbeat = !heartbeat); - } + /* configure the first 8 PWM outputs (i.e. all of them) */ + /* note, must do this after comms init to steal back PA0, which is CTS otherwise */ + up_pwm_servo_init(0xff); - /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_use_fmu) { - if (timers[TIMER_BLINK_AMBER] == 0) { - timers[TIMER_BLINK_AMBER] = 125; - failsafe = !failsafe; - } - } else { - failsafe = false; - } - LED_AMBER(failsafe); - - /* print some simple status */ -#if 0 - if (timers[TIMER_STATUS_PRINT] == 0) { - timers[TIMER_STATUS_PRINT] = 1000; - lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r", - cursor[cycle++ & 3], - (system_state.arm_ok ? "FMU_ARMED" : "FMU_SAFE"), - (system_state.armed ? "ARMED" : "SAFE"), - (system_state.rc_channels ? "RC OK" : "NO RC"), - (system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"), - system_state.rc_channels, - frame_rx, frame_bad - ); - } -#endif - } + struct mallinfo minfo = mallinfo(); + lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); - /* Should never reach here */ - return ERROR; -} - -static void -timer_tick(void *arg) -{ - for (unsigned i = 0; i < TIMER_NUM_TIMERS; i++) - if (timers[i] > 0) - timers[i]--; -} + /* 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 0a3a4448c6..2ecab966d0 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -147,7 +147,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; /* * Mixer */ -extern int mixer_init(void); +extern void mixer_tick(void); /* * Safety switch/LED. @@ -158,15 +158,16 @@ extern void safety_init(void); * FMU communications */ extern void comms_init(void); -extern void comms_check(void); +extern void comms_main(void) __attribute__((noreturn)); /* - * Serial receiver decoders. + * R/C receiver handling. */ -extern void dsm_init(unsigned mode); -extern void dsm_input(int fd); -extern void sbus_init(unsigned mode); -extern void sbus_input(int fd); +extern void controls_main(void); +extern int dsm_init(const char *device); +extern void dsm_input(void); +extern int sbus_init(const char *device); +extern void sbus_input(void); /* * Assertion codes diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 24fc9951a8..d5bd103c10 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -51,6 +51,8 @@ #include "px4io.h" static struct hrt_call arming_call; +static struct hrt_call heartbeat_call; +static struct hrt_call failsafe_call; /* * Count the number of times in a row that we see the arming button @@ -63,13 +65,22 @@ static unsigned counter; static bool safety_led_state; static bool safety_button_pressed; + static void safety_check_button(void *arg); +static void heartbeat_blink(void *arg); +static void failsafe_blink(void *arg); void safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); + + /* arrange for the heartbeat handler to be called at 4Hz */ + hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); + + /* arrange for the failsafe blinker to be called at 8Hz */ + hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } static void @@ -117,3 +128,28 @@ safety_check_button(void *arg) } LED_SAFETY(safety_led_state); } + + +static void +heartbeat_blink(void *arg) +{ + static bool heartbeat = false; + + /* XXX add flags here that need to be frobbed by various loops */ + + LED_BLUE(heartbeat = !heartbeat); +} + +static void +failsafe_blink(void *arg) +{ + static bool failsafe = false; + + /* blink the failsafe LED if we don't have FMU input */ + if (!system_state.mixer_use_fmu) { + failsafe = !failsafe; + } else { + failsafe = false; + } + LED_AMBER(failsafe); +} \ No newline at end of file diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index e363a0a78f..a91e37b5c8 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -41,18 +41,39 @@ #include #include +#include #include +#define DEBUG #include "px4io.h" -#include "protocol.h" -void -sbus_init(unsigned mode) +static int sbus_fd = -1; + +int +sbus_init(const char *device) { + if (sbus_fd < 0) + sbus_fd = open(device, O_RDONLY); + + if (sbus_fd >= 0) { + struct termios t; + + /* 100000bps, even parity, two stop bits */ + tcgetattr(sbus_fd, &t); + cfsetspeed(&t, 100000); + t.c_cflag |= (CSTOPB | PARENB); + tcsetattr(sbus_fd, TCSANOW, &t); + + debug("Sbus: ready"); + } else { + debug("Sbus: open failed"); + } + + return sbus_fd; } void -sbus_input(int fd) +sbus_input(void) { } From c961dd8bab38c1a8570eaf02b912353641e45df3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 01:34:33 -0800 Subject: [PATCH 10/21] Just for fun, add a (completely untested) S.bus decoder. --- apps/px4io/dsm.c | 4 +- apps/px4io/sbus.c | 145 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 146 insertions(+), 3 deletions(-) diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index da7d1a3618..2c2e099057 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -66,7 +66,6 @@ static hrt_abstime last_frame_time; static uint8_t frame[DSM_FRAME_SIZE]; static unsigned partial_frame_count; -static bool insync; static unsigned channel_shift; unsigned dsm_frame_drops; @@ -91,7 +90,6 @@ dsm_init(const char *device) tcsetattr(dsm_fd, TCSANOW, &t); /* initialise the decoder */ - insync = false; partial_frame_count = 0; last_rx_time = hrt_absolute_time(); @@ -321,5 +319,5 @@ dsm_decode(hrt_abstime frame_time) /* and note that we have received data from the R/C controller */ /* XXX failsafe will cause problems here - need a strategy for detecting it */ - ppm_last_valid_decode = hrt_absolute_time(); + ppm_last_valid_decode = frame_time; } diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index a91e37b5c8..497818c94f 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -43,13 +43,28 @@ #include #include +#include + #include #define DEBUG #include "px4io.h" +#include "protocol.h" + +#define SBUS_FRAME_SIZE 25 static int sbus_fd = -1; +static hrt_abstime last_rx_time; + +static uint8_t frame[SBUS_FRAME_SIZE]; + +static unsigned partial_frame_count; + +unsigned sbus_frame_drops; + +static void sbus_decode(hrt_abstime frame_time); + int sbus_init(const char *device) { @@ -65,6 +80,10 @@ sbus_init(const char *device) t.c_cflag |= (CSTOPB | PARENB); tcsetattr(sbus_fd, TCSANOW, &t); + /* initialise the decoder */ + partial_frame_count = 0; + last_rx_time = hrt_absolute_time(); + debug("Sbus: ready"); } else { debug("Sbus: open failed"); @@ -76,4 +95,130 @@ sbus_init(const char *device) void sbus_input(void) { + ssize_t ret; + hrt_abstime now; + + /* + * The S.bus protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 3ms passes between calls, + * the first byte we read will be the first byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ + now = hrt_absolute_time(); + if ((now - last_rx_time) > 3000) { + if (partial_frame_count > 0) { + sbus_frame_drops++; + partial_frame_count = 0; + } + } + + /* + * Fetch bytes, but no more than we would need to complete + * the current frame. + */ + ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return; + last_rx_time = now; + + /* + * Add bytes to the current frame + */ + partial_frame_count += ret; + + /* + * If we don't have a full frame, return + */ + if (partial_frame_count < SBUS_FRAME_SIZE) + return; + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + sbus_decode(now); + partial_frame_count = 0; +} + +/* + * S.bus decoder matrix. + * + * Each channel value can come from up to 3 input bytes. Each row in the + * matrix describes up to three bytes, and each entry gives: + * + * - byte offset in the data portion of the frame + * - right shift applied to the data byte + * - mask for the data byte + * - left shift applied to the result into the channel value + */ +struct sbus_bit_pick { + uint8_t byte; + uint8_t rshift; + uint8_t mask; + uint8_t lshift; +}; +static struct sbus_bit_pick sbus_decoder[][3] = { +/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, +/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, +/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} }, +/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, +/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, +/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} }, +/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, +/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} }, +/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, +/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, +/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} }, +/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, +/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, +/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} }, +/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, +/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} } +}; + +static void +sbus_decode(hrt_abstime frame_time) +{ + /* check frame boundary markers to avoid out-of-sync cases */ + if ((frame[0] != 0xf0) || (frame[24] != 0x00)) { + sbus_frame_drops++; + return; + } + + /* if the failsafe bit is set, we consider that a loss of RX signal */ + if (frame[23] & (1 << 4)) + return; + + /* use the decoder matrix to extract channel data */ + for (unsigned channel = 0; channel < 16; channel++) { + unsigned value = 0; + + for (unsigned pick = 0; pick < 3; pick++) { + struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; + + if (decode->mask != 0) { + unsigned piece = frame[1 + decode->byte]; + piece >>= decode->rshift; + piece &= decode->mask; + piece <<= decode->lshift; + + value |= piece; + } + } + /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ + ppm_buffer[channel] = (value / 2) + 998; + } + /* and note that we have received data from the R/C controller */ + ppm_last_valid_decode = frame_time; } From d16d66f990664481cb8ef6dddd038c575fe16380 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Nov 2012 10:42:27 +0100 Subject: [PATCH 11/21] Enabled UART3, added JTAG make target for IO, removed potentially problematic usleep --- Makefile | 15 +++++++++++++-- apps/drivers/px4io/px4io.cpp | 1 - nuttx/configs/px4io/io/defconfig | 2 +- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index 801256cfec..d9469bb49a 100644 --- a/Makefile +++ b/Makefile @@ -114,10 +114,21 @@ endif upload: $(FIRMWARE_BUNDLE) $(UPLOADER) @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE) - + +# +# JTAG firmware uploading with OpenOCD +# +ifeq ($(JTAGCONFIG),) +JTAGCONFIG=interface/olimex-jtag-tiny.cfg +endif + upload-jtag-px4fmu: @echo Attempting to flash PX4FMU board via JTAG - @openocd -f interface/olimex-jtag-tiny.cfg -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown + @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown + +upload-jtag-px4io: all + @echo Attempting to flash PX4IO board via JTAG + @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown # # Hacks and fixups diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 11f2f6f688..a74437d086 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -348,7 +348,6 @@ PX4IO::task_main() /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index d354ebfe57..7af9c9e4f7 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -124,7 +124,7 @@ CONFIG_STM32_TIM7=n CONFIG_STM32_WWDG=n CONFIG_STM32_SPI2=n CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=n +CONFIG_STM32_USART3=y CONFIG_STM32_I2C1=y CONFIG_STM32_I2C2=n CONFIG_STM32_BKP=n From e0df7e6a7648bb7c40411561e308a408b5ee7beb Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 01:46:14 -0800 Subject: [PATCH 12/21] save ~200 bytes of RAM by correctly positioning the S.bus decoder table in flash. --- apps/px4io/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 497818c94f..ed69358e95 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -168,7 +168,7 @@ struct sbus_bit_pick { uint8_t mask; uint8_t lshift; }; -static struct sbus_bit_pick sbus_decoder[][3] = { +static const struct sbus_bit_pick sbus_decoder[][3] = { /* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, /* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, /* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} }, From ef4a54666d760a18b18800163a24faf5883c1e61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Nov 2012 14:57:54 +0100 Subject: [PATCH 13/21] Harmonized PPM, S.BUS and DSM input (order: first preference S.Bus, then DSM, then PPM, first available and valid source is chosen), tested with FMU, valid channel inputs --- apps/px4io/dsm.c | 26 ++++++++++++++++++++++---- apps/px4io/mixer.c | 25 +++++++++++++++++-------- apps/px4io/px4io.c | 2 ++ apps/px4io/px4io.h | 5 +++++ apps/px4io/sbus.c | 26 +++++++++++++++++++++----- 5 files changed, 67 insertions(+), 17 deletions(-) diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 2c2e099057..744dac3d64 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -278,6 +278,7 @@ dsm_decode(hrt_abstime frame_time) last_frame_time = frame_time; if (channel_shift == 0) { dsm_guess_format(false); + system_state.dsm_input_ok = false; return; } @@ -292,6 +293,10 @@ dsm_decode(hrt_abstime frame_time) * seven channels are being transmitted. */ + const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS; + + uint16_t dsm_channels[dsm_chancount]; + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; @@ -314,10 +319,23 @@ dsm_decode(hrt_abstime frame_time) value /= 2; /* stuff the decoded channel into the PPM input buffer */ - ppm_buffer[channel] = 988 + value; + dsm_channels[channel] = 988 + 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 */ - ppm_last_valid_decode = frame_time; + /* DSM input is valid */ + system_state.dsm_input_ok = true; + + /* check if no S.BUS data is available */ + if (!system_state.sbus_input_ok) { + + for (unsigned i = 0; i < dsm_chancount; i++) { + system_state.rc_channel_data[i] = dsm_channels[i]; + } + + /* 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; + system_state.rc_channels = dsm_chancount; + system_state.fmu_report_due = true; + } } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 61e716474d..d8ce06e423 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -172,17 +172,26 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) static void mixer_get_rc_input(void) { - /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { - system_state.rc_channels = 0; - system_state.fmu_report_due = true; + + /* input was ok and timed out, mark as update */ + if (system_state.ppm_input_ok) { + system_state.ppm_input_ok = false; + system_state.fmu_report_due = true; + } return; } - /* otherwise, copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; - system_state.fmu_report_due = true; + /* mark PPM as valid */ + system_state.ppm_input_ok = true; + + /* check if no DSM and S.BUS data is available */ + if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) { + /* otherwise, copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + system_state.fmu_report_due = true; + } } diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index ba16ce2472..34b9c8c49d 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -58,6 +58,8 @@ struct sys_state_s system_state; int user_start(int argc, char *argv[]) { + /* reset all to zero */ + memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 2ecab966d0..5a26d355a3 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -72,11 +72,16 @@ struct sys_state_s bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ + bool ppm_input_ok; /* valid PPM input data */ + bool dsm_input_ok; /* valid Spektrum DSM data */ + bool sbus_input_ok; /* valid Futaba S.Bus data */ + /* * Data from the remote control input(s) */ int rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; + uint64_t rc_channels_timestamp; /* * Control signals from FMU. diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 497818c94f..3d8d7c18bf 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -50,6 +50,7 @@ #define DEBUG #include "px4io.h" #include "protocol.h" +#include "debug.h" #define SBUS_FRAME_SIZE 25 @@ -191,17 +192,22 @@ static void sbus_decode(hrt_abstime frame_time) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0xf0) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; + system_state.sbus_input_ok = false; return; } /* if the failsafe bit is set, we consider that a loss of RX signal */ - if (frame[23] & (1 << 4)) + if (frame[23] & (1 << 4)) { + system_state.sbus_input_ok = false; return; + } + + unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < 16; channel++) { + for (unsigned channel = 0; channel < chancount; channel++) { unsigned value = 0; for (unsigned pick = 0; pick < 3; pick++) { @@ -217,8 +223,18 @@ sbus_decode(hrt_abstime frame_time) } } /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - ppm_buffer[channel] = (value / 2) + 998; + system_state.rc_channel_data[channel] = (value / 2) + 998; } + + if (PX4IO_INPUT_CHANNELS >= 18) { + /* decode two switch channels */ + chancount = 18; + } + + system_state.rc_channels = chancount; + system_state.sbus_input_ok = true; + system_state.fmu_report_due = true; + /* and note that we have received data from the R/C controller */ - ppm_last_valid_decode = frame_time; + system_state.rc_channels_timestamp = frame_time; } From 2ac0cac11f3e3d896efee541eff36be42ff76914 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 21:50:19 -0800 Subject: [PATCH 14/21] Build fix - need --- apps/drivers/px4io/px4io.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 80a61e3037..07a39c8818 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include From 7f22811afb3078b1f86b2d462d13e0d06e3a5c88 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 21:50:55 -0800 Subject: [PATCH 15/21] Fix for c++ use (cannot mark speed const) --- nuttx/include/termios.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/include/termios.h b/nuttx/include/termios.h index e381814e3c..af1867a22a 100644 --- a/nuttx/include/termios.h +++ b/nuttx/include/termios.h @@ -230,7 +230,7 @@ struct termios * cf[set|get][o|i]speed() POSIX interfaces. */ - const speed_t c_speed; /* Input/output speed (non-POSIX)*/ + speed_t c_speed; /* Input/output speed (non-POSIX)*/ }; /**************************************************************************** From 1e6e06595af51fe8449da6bd599126868601ed01 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 21:51:36 -0800 Subject: [PATCH 16/21] Avoid processing S.bus channels that cannot be communicated to FMU --- apps/px4io/sbus.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index ed69358e95..39d2c49390 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -52,6 +52,7 @@ #include "protocol.h" #define SBUS_FRAME_SIZE 25 +#define SBUS_INPUT_CHANNELS 16 static int sbus_fd = -1; @@ -168,7 +169,7 @@ struct sbus_bit_pick { uint8_t mask; uint8_t lshift; }; -static const struct sbus_bit_pick sbus_decoder[][3] = { +static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, /* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, /* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} }, @@ -201,11 +202,15 @@ sbus_decode(hrt_abstime frame_time) return; /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < 16; channel++) { + for (unsigned channel = 0; channel < SBUS_INPUT_CHANNELS; channel++) { + + if (channel >= PX4IO_INPUT_CHANNELS) + break; + unsigned value = 0; for (unsigned pick = 0; pick < 3; pick++) { - struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; + const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; if (decode->mask != 0) { unsigned piece = frame[1 + decode->byte]; From 8c4e9de70a36119d0ff4b5c4369c4667bfae4383 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 21:52:18 -0800 Subject: [PATCH 17/21] Use the right constraint for the output mixer; we might end up wanting more virtual control channels. --- apps/px4io/mixer.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 61e716474d..fb553bc6ee 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -49,7 +49,6 @@ #include #include -#include #include @@ -125,12 +124,11 @@ mixer_tick(void) /* we have no control input */ control_count = 0; } - /* * Tickle each mixer, if we have control data. */ if (control_count > 0) { - for (i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { + for (i = 0; i < IO_SERVO_COUNT; i++) { mixer_update(i, control_values, control_count); /* From 7d9d307ab096d8d5f07a40959638da67dca9f676 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 22:15:40 -0800 Subject: [PATCH 18/21] We don't need non-blocking I/O for this context anymore; it's OK for it to block. --- apps/px4io/comms.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index a93ef9cb8f..b24fba09d9 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -73,7 +73,7 @@ void comms_init(void) { /* initialise the FMU interface */ - fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK); + fmu_fd = open("/dev/ttyS1", O_RDWR); stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); /* default state in the report to FMU */ From 3e036e26c9951acaadfed25d0e9e7bc1f1c73e89 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 22:31:07 -0800 Subject: [PATCH 19/21] Disable the flow control signals for USART2/3, since we use them for other things. --- nuttx/configs/px4io/include/board.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nuttx/configs/px4io/include/board.h b/nuttx/configs/px4io/include/board.h index e9181baf18..d941985b47 100755 --- a/nuttx/configs/px4io/include/board.h +++ b/nuttx/configs/px4io/include/board.h @@ -96,6 +96,18 @@ #define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) #define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#define GPIO_USART2_CTS 0xffffffff +#define GPIO_USART2_RTS 0xffffffff +#define GPIO_USART2_CK 0xffffffff +#define GPIO_USART3_TX 0xffffffff +#define GPIO_USART3_CK 0xffffffff +#define GPIO_USART3_CTS 0xffffffff +#define GPIO_USART3_RTS 0xffffffff + /* * High-resolution timer */ From efd3b9dea689ef4244a3a2fd9f4217a544b254ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 22:36:17 -0800 Subject: [PATCH 20/21] Clean up the FMU communications init. --- apps/px4io/comms.c | 12 +++++++++++- apps/px4io/px4io.c | 10 +++------- apps/px4io/px4io.h | 1 - 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index b24fba09d9..375336730a 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -47,6 +47,7 @@ #include #include #include +#include #include @@ -69,7 +70,7 @@ static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); -void +static void comms_init(void) { /* initialise the FMU interface */ @@ -78,11 +79,20 @@ comms_init(void) /* 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); } void comms_main(void) { + comms_init(); struct pollfd fds; fds.fd = fmu_fd; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index ba16ce2472..bba236322d 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -76,6 +76,9 @@ int user_start(int argc, char *argv[]) /* start the safety switch handler */ safety_init(); + /* configure the first 8 PWM outputs (i.e. all of them) */ + up_pwm_servo_init(0xff); + /* start the flight control signal handler */ task_create("FCon", SCHED_PRIORITY_DEFAULT, @@ -84,13 +87,6 @@ int user_start(int argc, char *argv[]) NULL); - /* initialise the FMU communications interface */ - comms_init(); - - /* configure the first 8 PWM outputs (i.e. all of them) */ - /* note, must do this after comms init to steal back PA0, which is CTS otherwise */ - up_pwm_servo_init(0xff); - struct mallinfo minfo = mallinfo(); lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 2ecab966d0..21c1482bed 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -157,7 +157,6 @@ extern void safety_init(void); /* * FMU communications */ -extern void comms_init(void); extern void comms_main(void) __attribute__((noreturn)); /* From de88732e8ecdf1f8451c8c037594f4edb1e2bdc0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Dec 2012 10:49:52 +0100 Subject: [PATCH 21/21] Prevented unhealthy RC input from propagating through the system --- apps/drivers/px4io/px4io.cpp | 18 ++++++++++-------- apps/px4io/comms.c | 8 +++++++- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 49ad80943f..296c541c95 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -461,15 +461,17 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) } _connected = true; - /* publish raw rc channel values from IO */ - _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]; - } + /* 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); + orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); + } /* remember the latched arming switch state */ _switch_armed = rep->armed; diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 375336730a..40ea38cf74 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -132,7 +132,13 @@ comms_main(void) /* populate the report */ for (int 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; + + if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) { + report.channel_count = system_state.rc_channels; + } else { + report.channel_count = 0; + } + report.armed = system_state.armed; /* and send it */