diff options
-rw-r--r-- | apps/drivers/boards/px4io/px4io_init.c | 5 | ||||
-rw-r--r-- | apps/px4io/comms.c | 29 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 2 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 8 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 3 | ||||
-rwxr-xr-x | nuttx/configs/px4io/io/defconfig | 2 |
6 files changed, 28 insertions, 21 deletions
diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c index dc2e11c30..33a6707ce 100644 --- a/apps/drivers/boards/px4io/px4io_init.c +++ b/apps/drivers/boards/px4io/px4io_init.c @@ -80,11 +80,6 @@ __EXPORT void stm32_boardinitialize(void) { - /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER - hrt_init(); -#endif - /* configure GPIOs */ stm32_configgpio(GPIO_ACC1_PWR_EN); stm32_configgpio(GPIO_ACC2_PWR_EN); diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index e7c968dcd..8fdf7d1c8 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -106,27 +106,29 @@ comms_check(void) * Check for bytes and feed them to the RX engine. * Limit the number of bytes we actually process on any one iteration. */ - struct pollfd fds[1]; - fds[0].fd = fmu_fd; - fds[0].revents = POLLIN; - if (poll(fds, 1, 0) > 0) { - char buf[8]; - ssize_t count = read(fmu_fd, buf, sizeof(buf)); - for (int i = 0; i < count; i++) - hx_stream_rx(stream, buf[i]); - } + 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]); } +int frame_rx; +int frame_bad; + static void comms_handle_frame(void *arg, const void *buffer, size_t length) { - struct px4io_command *cmd; + struct px4io_command *cmd = (struct px4io_command *)buffer; /* make sure it's what we are expecting */ - if (length != sizeof(struct px4io_command)) + if ((length != sizeof(struct px4io_command)) || + (cmd->f2i_magic != F2I_MAGIC)) { + frame_bad++; return; + } + frame_rx++; - cmd = (struct px4io_command *)buffer; + irqstate_t flags = irqsave(); /* fetch new PWM output values */ for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) @@ -143,4 +145,7 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) /* XXX do relay changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) system_state.relays[i] = cmd->relay_state[i]; + +out: + irqrestore(flags); } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index cbc03e2f8..94d10ef57 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -62,7 +62,7 @@ static unsigned mixer_input_drops; * Count of periodic calls in which we have no FMU input. */ static unsigned fmu_input_drops; -#define FMU_INPUT_DROP_LIMIT 10 +#define FMU_INPUT_DROP_LIMIT 20 /* * HRT periodic call used to check for control input data. diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 33c28fc6c..90057c790 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -71,6 +71,9 @@ int user_start(int argc, char *argv[]) bool heartbeat = false; bool failsafe = false; + /* configure the high-resolution time/callout interface */ + hrt_init(); + /* configure the PWM outputs */ up_pwm_servo_init(0xff); @@ -131,12 +134,13 @@ int user_start(int argc, char *argv[]) /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { timers[TIMER_STATUS_PRINT] = 1000; - lib_lowprintf("%c %s | %s | %s | C=%d \r", + lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d \r", cursor[cycle++ & 3], (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 + system_state.rc_channels, + frame_rx, frame_bad ); } } diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 4889036b6..bbbe91865 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -95,6 +95,9 @@ struct sys_state_s extern struct sys_state_s system_state; +extern int frame_rx; +extern int frame_bad; + /* * Software countdown timers. * diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 81153239d..64b521e21 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -164,7 +164,7 @@ CONFIG_USART2_TXBUFSIZE=32 CONFIG_USART3_TXBUFSIZE=32 CONFIG_USART1_RXBUFSIZE=32 -CONFIG_USART2_RXBUFSIZE=32 +CONFIG_USART2_RXBUFSIZE=128 CONFIG_USART3_RXBUFSIZE=32 CONFIG_USART1_BAUD=57600 |