diff options
author | px4dev <px4@purgatory.org> | 2012-11-30 00:02:47 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-11-30 00:02:47 -0800 |
commit | 9fa794a8faa2d30023d9943beae55a05ed4e48a0 (patch) | |
tree | cc05d6eafae584fb6c5cdfc731ece7f8be5f8f82 /apps/px4io/comms.c | |
parent | e153476950a3fbda230c6bddd9ad35018cfda559 (diff) | |
download | px4-firmware-9fa794a8faa2d30023d9943beae55a05ed4e48a0.tar.gz px4-firmware-9fa794a8faa2d30023d9943beae55a05ed4e48a0.tar.bz2 px4-firmware-9fa794a8faa2d30023d9943beae55a05ed4e48a0.zip |
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.
Diffstat (limited to 'apps/px4io/comms.c')
-rw-r--r-- | apps/px4io/comms.c | 153 |
1 files changed, 39 insertions, 114 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index c44b677d1..a93ef9cb8 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 <nuttx/config.h> #include <stdio.h> #include <stdbool.h> @@ -44,7 +45,6 @@ #include <debug.h> #include <stdlib.h> #include <errno.h> -#include <termios.h> #include <string.h> #include <poll.h> @@ -54,101 +54,50 @@ #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> +#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) +void +comms_main(void) { - 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); + struct pollfd fds; + fds.fd = fmu_fd; + fds.events = POLLIN; + debug("FMU: ready"); - dsm_init(serial_mode); - break; + for (;;) { + /* wait for serial data, but no more than 100ms */ + poll(&fds, 1, 100); - 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) -{ - /* - * Check for serial data - */ - int ret = poll(pollfds, pollcount, 0); - - if (ret > 0) { /* * 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; - - case RX_MODE_FUTABA_SBUS: - sbus_input(rx_fd); - break; - - default: - break; - } + 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)); } } - - /* - * 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++; } |