diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 2c2e099057..744dac3d64 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -278,6 +278,7 @@ dsm_decode(hrt_abstime frame_time) last_frame_time = frame_time; if (channel_shift == 0) { dsm_guess_format(false); + system_state.dsm_input_ok = false; return; } @@ -292,6 +293,10 @@ 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)]; @@ -314,10 +319,23 @@ dsm_decode(hrt_abstime frame_time) value /= 2; /* stuff the decoded channel into the PPM input buffer */ - ppm_buffer[channel] = 988 + value; + dsm_channels[channel] = 988 + value; } - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - ppm_last_valid_decode = frame_time; + /* DSM input is valid */ + system_state.dsm_input_ok = true; + + /* 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; + } } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 61e716474d..d8ce06e423 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -172,17 +172,26 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) 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) { - system_state.rc_channels = 0; - system_state.fmu_report_due = true; + + /* 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; } - /* 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; + /* 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 ba16ce2472..34b9c8c49d 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -58,6 +58,8 @@ struct sys_state_s system_state; int user_start(int argc, char *argv[]) { + /* reset all to zero */ + memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 2ecab966d0..5a26d355a3 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -72,11 +72,16 @@ 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 */ + /* * Data from the remote control input(s) */ int rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; + uint64_t rc_channels_timestamp; /* * Control signals from FMU. diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 497818c94f..3d8d7c18bf 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -50,6 +50,7 @@ #define DEBUG #include "px4io.h" #include "protocol.h" +#include "debug.h" #define SBUS_FRAME_SIZE 25 @@ -191,17 +192,22 @@ static void sbus_decode(hrt_abstime frame_time) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0xf0) || (frame[24] != 0x00)) { + 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 (frame[23] & (1 << 4)) + if (frame[23] & (1 << 4)) { + system_state.sbus_input_ok = false; return; + } + + unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < 16; channel++) { + for (unsigned channel = 0; channel < chancount; channel++) { unsigned value = 0; for (unsigned pick = 0; pick < 3; pick++) { @@ -217,8 +223,18 @@ sbus_decode(hrt_abstime frame_time) } } /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - ppm_buffer[channel] = (value / 2) + 998; + system_state.rc_channel_data[channel] = (value / 2) + 998; } + + if (PX4IO_INPUT_CHANNELS >= 18) { + /* decode two switch channels */ + chancount = 18; + } + + 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 */ - ppm_last_valid_decode = frame_time; + system_state.rc_channels_timestamp = frame_time; }