diff options
| author | px4dev <px4@purgatory.org> | 2013-01-06 02:02:14 -0800 |
|---|---|---|
| committer | px4dev <px4@purgatory.org> | 2013-01-06 02:02:14 -0800 |
| commit | 58309fd6a863805e16602f4bec82eb35e0a14262 (patch) | |
| tree | 450695e021158ec4c3d59bdbea90a33c6b8d5a59 | |
| parent | 6ae9f03544d46059ebfa7da3566a70837cb1510b (diff) | |
| parent | 532c694ec30317f1bc2a745f0d24e7a940573041 (diff) | |
| download | px4-firmware-58309fd6a863805e16602f4bec82eb35e0a14262.tar.gz px4-firmware-58309fd6a863805e16602f4bec82eb35e0a14262.tar.bz2 px4-firmware-58309fd6a863805e16602f4bec82eb35e0a14262.zip | |
Merge branch 'master' of https://github.com/PX4/Firmware
| -rw-r--r-- | apps/px4io/sbus.c | 26 |
1 files changed, 16 insertions, 10 deletions
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 9679f657b..c89388be1 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -53,7 +53,7 @@ #include "debug.h" #define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 16 +#define SBUS_INPUT_CHANNELS 18 static int sbus_fd = -1; @@ -87,10 +87,9 @@ sbus_init(const char *device) partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - debug("Sbus: ready"); - + debug("S.Bus: ready"); } else { - debug("Sbus: open failed"); + debug("S.Bus: open failed"); } return sbus_fd; @@ -208,9 +207,13 @@ sbus_decode(hrt_abstime frame_time) return; } - /* if the failsafe bit is set, we consider the frame invalid */ - if (frame[23] & (1 << 4)) { - return; + /* if the failsafe or connection lost bit is set, we consider the frame invalid */ + if ((frame[23] & (1 << 2)) && /* signal lost */ + (frame[23] & (1 << 3))) { /* failsafe */ + + /* actively announce signal loss */ + system_state.rc_channels = 0; + return 1; } /* we have received something we think is a frame */ @@ -240,9 +243,12 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channel_data[channel] = (value / 2) + 998; } - if (PX4IO_INPUT_CHANNELS >= 18) { - chancount = 18; - /* XXX decode the two switch channels */ + /* decode switch channels if data fields are wide enough */ + if (chancount > 17) { + /* channel 17 (index 16) */ + system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ |