fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 09:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed connection loss / failsafe detection, added decoding of two switch channels if IO supports 18 RC channels. Loss is tested, switch channels are not.

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-31 09:21:44 +01:00
parent c08135ffef
commit 0cc723dbc3
1 changed files with 16 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4io/sbus.c
+16 -9
View File
@@ -53,7 +53,7 @@
#include "debug.h" #include "debug.h"
#define SBUS_FRAME_SIZE 25 #define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16 #define SBUS_INPUT_CHANNELS 18
static int sbus_fd = -1; static int sbus_fd = -1;
@@ -87,9 +87,9 @@ sbus_init(const char *device)
partial_frame_count = 0; partial_frame_count = 0;
last_rx_time = hrt_absolute_time(); last_rx_time = hrt_absolute_time();
debug("Sbus: ready"); debug("S.Bus: ready");
} else { } else {
debug("Sbus: open failed"); debug("S.Bus: open failed");
} }
return sbus_fd; return sbus_fd;
@@ -205,9 +205,13 @@ sbus_decode(hrt_abstime frame_time)
return; return;
} }
/* if the failsafe bit is set, we consider the frame invalid */ /* if the failsafe or connection lost bit is set, we consider the frame invalid */
if (frame[23] & (1 << 4)) { if ((frame[23] & (1 << 2)) && /* signal lost */
return; (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 */ /* we have received something we think is a frame */
@@ -236,9 +240,12 @@ sbus_decode(hrt_abstime frame_time)
system_state.rc_channel_data[channel] = (value / 2) + 998; system_state.rc_channel_data[channel] = (value / 2) + 998;
} }
if (PX4IO_INPUT_CHANNELS >= 18) { /* decode switch channels if data fields are wide enough */
chancount = 18; if (chancount > 17) {
/* XXX decode the two switch channels */ /* 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 */ /* note the number of channels decoded */