dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #122 from PX4/sbus_rcloss_fix 

Fixed connection loss / failsafe detection, added decoding of two switches
This commit is contained in:
px4dev 2013-01-06 01:53:14 -08:00
parent 94fa60fa04 329ac2f4ba
commit 532c694ec3
1 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
apps/px4io/sbus.c
View File

@ -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 */