diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-05 19:55:33 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-05 19:55:33 +0100 |
commit | 1ebb3b4ada6bdf2109f3e6bb45800f0459b35ccc (patch) | |
tree | d0ac45b615ef77168dd802799aa0c4262ae15e07 /apps/px4io | |
parent | 126e6ac2073ffb96c3867e7cbdd4e51e8408d0ec (diff) | |
parent | fd771f67f2a2392d5ba2b7dd74100338859af6d7 (diff) | |
download | px4-firmware-1ebb3b4ada6bdf2109f3e6bb45800f0459b35ccc.tar.gz px4-firmware-1ebb3b4ada6bdf2109f3e6bb45800f0459b35ccc.tar.bz2 px4-firmware-1ebb3b4ada6bdf2109f3e6bb45800f0459b35ccc.zip |
Merged DSM fixes
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/comms.c | 14 | ||||
-rw-r--r-- | apps/px4io/controls.c | 76 | ||||
-rw-r--r-- | apps/px4io/dsm.c | 70 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 41 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 1 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 14 | ||||
-rw-r--r-- | apps/px4io/sbus.c | 31 |
7 files changed, 142 insertions, 105 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index f4eddbdd3..5033aeb98 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -100,8 +100,8 @@ comms_main(void) debug("FMU: ready"); for (;;) { - /* wait for serial data, but no more than 100ms */ - poll(&fds, 1, 100); + /* wait for serial data, but no more than 10ms */ + poll(&fds, 1, 10); /* * Pull bytes from FMU and feed them to the HX engine. @@ -130,15 +130,9 @@ comms_main(void) last_report_time = now; /* populate the report */ - for (int i = 0; i < system_state.rc_channels; i++) + for (unsigned i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; - - 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.channel_count = system_state.rc_channels; report.armed = system_state.armed; /* and send it */ diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 0e0210373..e64f8b204 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -55,6 +55,7 @@ #include <drivers/drv_hrt.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> +#include <systemlib/ppm_decode.h> #define DEBUG #include "px4io.h" @@ -63,15 +64,18 @@ #define RC_CHANNEL_HIGH_THRESH 1600 #define RC_CHANNEL_LOW_THRESH 1400 +static void ppm_input(void); + void controls_main(void) { struct pollfd fds[2]; + /* DSM input */ fds[0].fd = dsm_init("/dev/ttyS0"); fds[0].events = POLLIN; - + /* S.bus input */ fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; @@ -79,10 +83,32 @@ controls_main(void) /* run this loop at ~100Hz */ poll(fds, 2, 10); + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ + bool locked = false; + if (fds[0].revents & POLLIN) - dsm_input(); + locked |= dsm_input(); if (fds[1].revents & POLLIN) - sbus_input(); + locked |= sbus_input(); + + /* + * If we don't have lock from one of the serial receivers, + * look for PPM. It shares an input with S.bus, so there's + * a possibility it will mis-parse an S.bus frame. + * + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have an alternate + * receiver lock. + */ + if (!locked) + ppm_input(); /* force manual input override */ if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { @@ -92,16 +118,48 @@ controls_main(void) system_state.mixer_use_fmu = true; } - /* detect rc loss event */ - if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) { - system_state.rc_lost = true; - } + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input and tell FMU. + */ + if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { - /* XXX detect fmu loss event */ + /* set the number of channels to zero - no inputs */ + system_state.rc_channels = 0; + system_state.rc_lost = true; - /* XXX handle failsave events - RC loss and FMU loss - here */ + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } /* do PWM output updates */ mixer_tick(); } } + +static void +ppm_input(void) +{ + /* + * Look for new PPM input. + */ + if (ppm_last_valid_decode != 0) { + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + + /* PPM data exists, copy it */ + 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]; + + /* copy the timestamp and clear it */ + system_state.rc_channels_timestamp = ppm_last_valid_decode; + ppm_last_valid_decode = 0; + + irqrestore(state); + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } +} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 744dac3d6..2611f3a03 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -104,7 +104,7 @@ dsm_init(const char *device) return dsm_fd; } -void +bool dsm_input(void) { ssize_t ret; @@ -141,7 +141,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -153,7 +153,7 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -161,6 +161,12 @@ dsm_input(void) */ dsm_decode(now); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } static bool @@ -275,10 +281,13 @@ dsm_decode(hrt_abstime frame_time) */ if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) dsm_guess_format(true); + + /* we have received something we think is a frame */ last_frame_time = frame_time; + + /* if we don't know the frame format, update the guessing state machine */ if (channel_shift == 0) { dsm_guess_format(false); - system_state.dsm_input_ok = false; return; } @@ -293,10 +302,6 @@ 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)]; @@ -311,31 +316,40 @@ dsm_decode(hrt_abstime frame_time) continue; /* update the decoded channel count */ - if (channel > ppm_decoded_channels) - ppm_decoded_channels = channel; + if (channel >= system_state.rc_channels) + system_state.rc_channels = channel + 1; /* 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 */ - dsm_channels[channel] = 988 + value; + value += 998; + + /* + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ + switch (channel) { + case 0: + channel = 2; + break; + case 1: + channel = 0; + break; + case 2: + channel = 1; + default: + break; + } + system_state.rc_channel_data[channel] = value; } - /* DSM input is valid */ - system_state.dsm_input_ok = true; - - /* check if no S.BUS data is available */ - if (!system_state.sbus_input_ok) { + /* 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; - 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; - } + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index d9626c1e3..4fa3e1d5d 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -50,8 +50,6 @@ #include <drivers/drv_pwm_output.h> -#include <systemlib/ppm_decode.h> - #include "px4io.h" /* @@ -59,10 +57,6 @@ */ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 -/* - * Collect RC input data from the controller source(s). - */ -static void mixer_get_rc_input(void); /* * Update a mixer based on the current control signals. @@ -89,12 +83,6 @@ mixer_tick(void) bool should_arm; /* - * Start by looking for R/C control inputs. - * This updates system_state with any control inputs received. - */ - mixer_get_rc_input(); - - /* * Decide which set of inputs we're using. */ if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) { @@ -122,8 +110,10 @@ mixer_tick(void) } else { /* we have no control input */ + /* XXX builtin failsafe would activate here */ control_count = 0; } + /* * Tickle each mixer, if we have control data. */ @@ -167,30 +157,3 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) mixers[mixer].current_value = 0; } } - -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) { - - /* 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; - } - - /* 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 77524797f..a3ac9e3e7 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -44,6 +44,7 @@ #include <debug.h> #include <stdlib.h> #include <errno.h> +#include <string.h> #include <nuttx/clock.h> diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 9bfe3b1e4..e0b7cdda1 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -69,17 +69,13 @@ 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 */ + bool armed; /* IO armed */ + bool arm_ok; /* FMU says OK to arm */ /** * Data from the remote control input(s) */ - int rc_channels; + unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; @@ -184,9 +180,9 @@ extern void comms_main(void) __attribute__((noreturn)); */ extern void controls_main(void); extern int dsm_init(const char *device); -extern void dsm_input(void); +extern bool dsm_input(void); extern int sbus_init(const char *device); -extern void sbus_input(void); +extern bool sbus_input(void); /* * Assertion codes diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index c3949f2b0..a8f628a84 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -58,6 +58,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; +static hrt_abstime last_frame_time; static uint8_t frame[SBUS_FRAME_SIZE]; @@ -94,7 +95,7 @@ sbus_init(const char *device) return sbus_fd; } -void +bool sbus_input(void) { ssize_t ret; @@ -131,7 +132,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -143,7 +144,7 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -151,6 +152,12 @@ sbus_input(void) */ sbus_decode(now); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } /* @@ -195,17 +202,19 @@ sbus_decode(hrt_abstime frame_time) /* check frame boundary markers to avoid out-of-sync cases */ 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 the failsafe bit is set, we consider the frame invalid */ if (frame[23] & (1 << 4)) { - system_state.sbus_input_ok = false; return; } - unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; + /* 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; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -228,14 +237,16 @@ sbus_decode(hrt_abstime frame_time) } if (PX4IO_INPUT_CHANNELS >= 18) { - /* decode two switch channels */ chancount = 18; + /* XXX decode the two switch channels */ } + /* note the number of channels decoded */ 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 */ system_state.rc_channels_timestamp = frame_time; + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } |