From 6e328b4d7ab31faef5796956cffb985a9859549d Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 3 Dec 2012 23:19:12 -0800 Subject: [PATCH 1/4] Add a 'monitor' verb to the px4io command so we can watch inputs to IO (it could get smarter). --- apps/drivers/px4io/px4io.cpp | 42 +++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba73..9f3dba047f 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 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'"); } From 1485a4ec1aa8328cc50d99a1195b20df2b11045e Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 3 Dec 2012 23:20:28 -0800 Subject: [PATCH 2/4] Fix breakage to the DSM parser introduced with the input prioritisation logic. Back out to a "any input wins" strategy; connecting multiple receivers to I/O at the same time is currently not supported (read: strange things will happen). --- apps/px4io/comms.c | 12 +++------- apps/px4io/controls.c | 56 +++++++++++++++++++++++++++++++++++++++++-- apps/px4io/dsm.c | 36 ++++++++++------------------ apps/px4io/mixer.c | 44 +++------------------------------- apps/px4io/px4io.c | 1 + apps/px4io/px4io.h | 8 ++----- apps/px4io/sbus.c | 15 ++++++------ 7 files changed, 84 insertions(+), 88 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf74..480e3f5cc4 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. @@ -132,13 +132,7 @@ comms_main(void) /* populate the report */ for (int 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 d4eace3df5..6cf3c80ac2 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -55,19 +55,23 @@ #include #include #include +#include #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,62 @@ 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. + */ if (fds[0].revents & POLLIN) dsm_input(); if (fds[1].revents & POLLIN) sbus_input(); + 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 744dac3d64..5841f29a36 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -275,10 +275,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 +296,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 +310,22 @@ 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; + /* XXX check actual values */ + system_state.rc_channel_data[channel] = 988 + value; } - /* DSM input is valid */ - system_state.dsm_input_ok = true; + /* 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; - /* check if no S.BUS data is available */ - if (!system_state.sbus_input_ok) { - - 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 483e9fe4d4..f02e98ae45 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -50,8 +50,6 @@ #include -#include - #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. @@ -88,12 +82,6 @@ mixer_tick(void) int i; 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. */ @@ -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 77524797ff..a3ac9e3e78 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -44,6 +44,7 @@ #include #include #include +#include #include diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 483b9bcc81..63a55d840e 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -69,12 +69,8 @@ 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) diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index c3949f2b04..25fe0cf382 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -195,17 +195,16 @@ 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; + 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 +227,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; } From 7c3b28d503123121403b4ad68c934bb91b05d878 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 4 Dec 2012 09:52:16 -0800 Subject: [PATCH 3/4] Lock out the PPM decoder if the DSM or S.bus decoders have seen a frame recently. --- apps/px4io/controls.c | 22 ++++++++++++++++++---- apps/px4io/dsm.c | 12 +++++++++--- apps/px4io/px4io.h | 4 ++-- apps/px4io/sbus.c | 16 +++++++++++++--- 4 files changed, 42 insertions(+), 12 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 6cf3c80ac2..3b37829188 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -86,15 +86,29 @@ controls_main(void) * 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(); - ppm_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(); /* * If we haven't seen any new control data in 200ms, assume we - * have lost input and tell FMU + * have lost input and tell FMU. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 5841f29a36..04aca709bf 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 diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 63a55d840e..0032e6d800 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -165,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 25fe0cf382..a8f628a84e 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; } /* @@ -203,6 +210,9 @@ sbus_decode(hrt_abstime frame_time) return; } + /* 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; From fd771f67f2a2392d5ba2b7dd74100338859af6d7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 4 Dec 2012 22:00:24 -0800 Subject: [PATCH 4/4] Adjust the control mapping from DSM receivers to correspond to the standard PPM control mapping for channels 0-4. --- apps/px4io/comms.c | 2 +- apps/px4io/dsm.c | 24 +++++++++++++++++++++--- apps/px4io/px4io.h | 2 +- 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 480e3f5cc4..83a006d43b 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -130,7 +130,7 @@ 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]; report.channel_count = system_state.rc_channels; report.armed = system_state.armed; diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 04aca709bf..2611f3a034 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -322,10 +322,28 @@ 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; + value += 998; - /* stuff the decoded channel into the PPM input buffer */ - /* XXX check actual values */ - system_state.rc_channel_data[channel] = 988 + value; + /* + * 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; } /* and note that we have received data from the R/C controller */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 0032e6d800..45b7cf847a 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -75,7 +75,7 @@ struct sys_state_s /* * 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;