diff options
author | px4dev <px4@purgatory.org> | 2012-12-23 11:31:31 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-12-23 11:31:31 -0800 |
commit | 6b3f36020ced751a6e92110ac881796e8cc3b468 (patch) | |
tree | b64db0af45ab56dce1b83be72e17c5855ec5d318 | |
parent | d5aa7631187d441084b637cd9543267eaf3a07db (diff) | |
parent | 73763353d094e151dc95bc827a1bb4da84939ed0 (diff) | |
download | px4-firmware-6b3f36020ced751a6e92110ac881796e8cc3b468.tar.gz px4-firmware-6b3f36020ced751a6e92110ac881796e8cc3b468.tar.bz2 px4-firmware-6b3f36020ced751a6e92110ac881796e8cc3b468.zip |
Merge pull request #94 from PX4/DSM-decoder-fix
Untangle the DSM decoder from the input source priority logic, clean up input handling some more.
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 42 | ||||
-rw-r--r-- | apps/px4io/comms.c | 14 | ||||
-rw-r--r-- | apps/px4io/controls.c | 74 | ||||
-rw-r--r-- | apps/px4io/dsm.c | 70 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 44 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 1 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 14 | ||||
-rw-r--r-- | apps/px4io/sbus.c | 31 |
8 files changed, 187 insertions, 103 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba7..9f3dba047 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -88,6 +88,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + bool dump_one; + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; @@ -175,6 +177,7 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), + dump_one(false), _serial_fd(-1), _io_stream(nullptr), _task(-1), @@ -478,6 +481,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) _send_needed = true; + /* if monitoring, dump the received info */ + if (dump_one) { + dump_one = false; + + printf("IO: %s armed ", rep->armed ? "" : "not"); + for (unsigned i = 0; i < rep->channel_count; i++) + printf("%d: %d ", i, rep->rc_channel[i]); + printf("\n"); + } + out: unlock(); } @@ -665,6 +678,30 @@ test(void) exit(0); } +void +monitor(void) +{ + unsigned cancels = 4; + printf("Hit <enter> three times to exit monitor mode\n"); + + for (;;) { + pollfd fds[1]; + + fds[0].fd = 0; + fds[0].events = POLLIN; + poll(fds, 1, 500); + + if (fds[0].revents == POLLIN) { + int c; + read(0, &c, 1); + if (cancels-- == 0) + exit(0); + } + + if (g_dev != nullptr) + g_dev->dump_one = true; + } +} } int @@ -740,8 +777,11 @@ px4io_main(int argc, char *argv[]) !strcmp(argv[1], "rx_sbus") || !strcmp(argv[1], "rx_ppm")) errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + if (!strcmp(argv[1], "test")) test(); + if (!strcmp(argv[1], "monitor")) + monitor(); - errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'"); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf7..83a006d43 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 d4eace3df..3b3782918 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -55,19 +55,23 @@ #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" +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; @@ -75,14 +79,76 @@ 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(); - /* XXX do ppm processing, bypass mode, etc. here */ + /* + * 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) { + + /* set the number of channels to zero - no inputs */ + system_state.rc_channels = 0; + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } + + /* XXX do bypass mode, etc. here */ /* 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 483e9fe4d..f02e98ae4 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) { @@ -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. */ @@ -142,8 +132,7 @@ mixer_tick(void) /* * Decide whether the servos should be armed right now. */ - - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu; + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -166,30 +155,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 483b9bcc8..45b7cf847 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; @@ -169,9 +165,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; } |