From 9fa794a8faa2d30023d9943beae55a05ed4e48a0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 30 Nov 2012 00:02:47 -0800 Subject: 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/px4io/comms.c | 153 +++++++++++++------------------------------------- apps/px4io/controls.c | 88 +++++++++++++++++++++++++++++ apps/px4io/dsm.c | 56 +++++++++++++----- apps/px4io/mixer.c | 27 +-------- apps/px4io/protocol.h | 5 +- apps/px4io/px4io.c | 103 +++++++-------------------------- apps/px4io/px4io.h | 15 ++--- apps/px4io/safety.c | 36 ++++++++++++ apps/px4io/sbus.c | 29 ++++++++-- 9 files changed, 261 insertions(+), 251 deletions(-) create mode 100644 apps/px4io/controls.c (limited to 'apps/px4io') 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 #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) +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++; } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c new file mode 100644 index 000000000..d4eace3df --- /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 0e9fd25f1..da7d1a361 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; + + /* 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: enabled and waiting\n"); + 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 572344d00..61e716474 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 8d051d385..c704b1201 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 197bad7c6..ba16ce247 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 */ - - /* - * Main loop servicing communication with FMU - */ - while(true) { - - /* check for communication from FMU, send updates */ - comms_check(); - - /* blink the heartbeat LED */ - if (timers[TIMER_BLINK_BLUE] == 0) { - timers[TIMER_BLINK_BLUE] = 250; - LED_BLUE(heartbeat = !heartbeat); - } - - /* 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 - } - - /* 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]--; -} + /* start the flight control signal handler */ + task_create("FCon", + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)controls_main, + 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); + + /* 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 0a3a4448c..2ecab966d 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 24fc9951a..d5bd103c1 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 e363a0a78..a91e37b5c 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) { } -- cgit v1.2.3