diff options
Diffstat (limited to 'apps/px4io/controls.c')
-rw-r--r-- | apps/px4io/controls.c | 76 |
1 files changed, 67 insertions, 9 deletions
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; + } +} |