diff options
author | px4dev <px4@purgatory.org> | 2012-12-01 00:50:39 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-12-01 00:50:39 -0800 |
commit | ea8872f5456dbba566fb4167ec66b4afa183b3e1 (patch) | |
tree | 40def937aa4253700cb78c07b0e70b0963537da1 /apps/px4io/sbus.c | |
parent | efd3b9dea689ef4244a3a2fd9f4217a544b254ee (diff) | |
parent | ef4a54666d760a18b18800163a24faf5883c1e61 (diff) | |
download | px4-firmware-ea8872f5456dbba566fb4167ec66b4afa183b3e1.tar.gz px4-firmware-ea8872f5456dbba566fb4167ec66b4afa183b3e1.tar.bz2 px4-firmware-ea8872f5456dbba566fb4167ec66b4afa183b3e1.zip |
Merge branch 'sbus' of https://github.com/PX4/Firmware into #61-px4io-spektrum-decoder
Diffstat (limited to 'apps/px4io/sbus.c')
-rw-r--r-- | apps/px4io/sbus.c | 30 |
1 files changed, 21 insertions, 9 deletions
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 39d2c4939..c3949f2b0 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 #define SBUS_INPUT_CHANNELS 16 @@ -192,21 +193,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; + } - /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < SBUS_INPUT_CHANNELS; channel++) { - - if (channel >= PX4IO_INPUT_CHANNELS) - break; + unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; + /* use the decoder matrix to extract channel data */ + for (unsigned channel = 0; channel < chancount; channel++) { unsigned value = 0; for (unsigned pick = 0; pick < 3; pick++) { @@ -222,8 +224,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; } |