diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-16 15:31:44 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-16 15:31:44 +0100 |
commit | e56911bf2d637682f64fe93add114f8fef2682fd (patch) | |
tree | 0551e3d678cda4105574c961f0f95745bbadb634 /apps/px4io | |
parent | f81d00594c156c51ab976d3b6d101915377d7afa (diff) | |
download | px4-firmware-e56911bf2d637682f64fe93add114f8fef2682fd.tar.gz px4-firmware-e56911bf2d637682f64fe93add114f8fef2682fd.tar.bz2 px4-firmware-e56911bf2d637682f64fe93add114f8fef2682fd.zip |
Fixed signal loss detection on S.Bus parsing, stripped PX4IO code parts from S.Bus parser to allow FMU / IO parser code sharing. Added S.Bus channels 17 and 18 if channel data struct has enough space. Tested with receiver and PX4FMU.
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/comms.c | 8 | ||||
-rw-r--r-- | apps/px4io/controls.c | 32 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 4 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 3 | ||||
-rw-r--r-- | apps/px4io/sbus.c | 56 |
5 files changed, 72 insertions, 31 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 9a786234e..bfdad019d 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -52,6 +52,7 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> @@ -201,7 +202,12 @@ comms_handle_command(const void *buffer, size_t length) system_state.servo_rate = new_servo_rate; } - /* update servo values immediately */ + /* + * update servo values immediately. + * the updates are done in addition also + * in the mainloop, since this function will only + * update with a connected FMU. + */ mixer_tick(); /* XXX do relay changes here */ diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 4bd6fe1a0..48370d9d0 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -92,10 +92,22 @@ controls_main(void) */ bool locked = false; + /* + * Store RC channel count to detect switch to RC loss sooner + * than just by timeout + */ + unsigned rc_channels = system_state.rc_channels; + + /* + * Track if any input got an update in this round + */ + bool rc_updated; + if (fds[0].revents & POLLIN) locked |= dsm_input(); if (fds[1].revents & POLLIN) - locked |= sbus_input(); + locked |= sbus_input(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data, + &system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated); /* * If we don't have lock from one of the serial receivers, @@ -127,13 +139,21 @@ controls_main(void) /* set the number of channels to zero - no inputs */ system_state.rc_channels = 0; - system_state.rc_lost = true; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + rc_updated = true; } - /* PWM output updates are performed directly on each comms update */ + /* + * If there was a RC update OR the RC signal status (lost / present) has + * just changed, request an update immediately. + */ + system_state.fmu_report_due |= rc_updated; + + /* + * PWM output updates are performed in addition on each comm update. + * the updates here are required to ensure operation if FMU is not started + * or stopped responding. + */ + mixer_tick(); } } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 318183ef5..135d97bb3 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -48,7 +48,10 @@ #include <unistd.h> #include <fcntl.h> +#include <debug.h> + #include <drivers/drv_pwm_output.h> +#include <drivers/drv_hrt.h> #include "px4io.h" @@ -95,6 +98,7 @@ mixer_tick(void) /* too many frames without FMU input, time to go to failsafe */ system_state.mixer_manual_override = true; system_state.mixer_fmu_available = false; + lib_lowprintf("\nRX timeout\n"); } } else if (system_state.rc_channels > 0 && system_state.manual_override_ok) { diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index c8699c6c6..9cfd8f716 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -193,7 +193,8 @@ extern void controls_main(void); extern int dsm_init(const char *device); extern bool dsm_input(void); extern int sbus_init(const char *device); -extern bool sbus_input(void); +extern bool sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated); /* * Assertion codes diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index a8f628a84..0de0593e7 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -49,14 +49,11 @@ #define DEBUG #include "px4io.h" -#include "protocol.h" #include "debug.h" #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 -static int sbus_fd = -1; - static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; @@ -66,11 +63,14 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +static int sbus_decode(hrt_abstime frame_time, unsigned max_channels, + uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time); int sbus_init(const char *device) { + static int sbus_fd = -1; + if (sbus_fd < 0) sbus_fd = open(device, O_RDONLY); @@ -96,7 +96,8 @@ sbus_init(const char *device) } bool -sbus_input(void) +sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated) { ssize_t ret; hrt_abstime now; @@ -128,7 +129,7 @@ sbus_input(void) * 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); + ret = read(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) @@ -148,9 +149,9 @@ sbus_input(void) /* * Great, it looks like we might have a frame. Go ahead and - * decode it. + * decode it, report if the receiver got something. */ - sbus_decode(now); + *updated = (sbus_decode(now, max_channels, channel_data, channel_count, receive_time) == OK); partial_frame_count = 0; out: @@ -196,25 +197,32 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +static int +sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - return; + return 1; } - /* if the failsafe bit is set, we consider the frame invalid */ - if (frame[23] & (1 << 4)) { - return; + /* if the failsafe or connection lost bit is set, we consider the frame invalid */ + if ((frame[23] & (1 << 2)) && /* signal lost */ + (frame[23] & (1 << 3))) { /* failsafe */ + + /* actively announce signal loss */ + *channel_count = 0; + + return 1; } + /* decode failsafe and RC status */ + /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_channels; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -236,17 +244,19 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channel_data[channel] = (value / 2) + 998; } - if (PX4IO_INPUT_CHANNELS >= 18) { - chancount = 18; - /* XXX decode the two switch channels */ + /* decode switch channels if data fields are wide enough */ + if (chancount > 17) { + /* channel 17 (index 16) */ + system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ - system_state.rc_channels = chancount; + *channel_count = chancount; /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + *receive_time = frame_time; - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return 0; } |