forked from Archive/PX4-Autopilot
Merge pull request #122 from PX4/sbus_rcloss_fix
Fixed connection loss / failsafe detection, added decoding of two switches
This commit is contained in:
commit
532c694ec3
|
@ -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,10 +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;
|
||||||
|
@ -208,9 +207,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 */
|
||||||
|
@ -240,9 +243,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 */
|
||||||
|
|
Loading…
Reference in New Issue