diff options
Diffstat (limited to 'apps/px4io/sbus.c')
-rw-r--r-- | apps/px4io/sbus.c | 26 |
1 files changed, 21 insertions, 5 deletions
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 497818c94..3d8d7c18b 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; } |