forked from Archive/PX4-Autopilot
Reverted unwanted S.Bus changes
This commit is contained in:
Lorenz Meier
parent
ab85d201ee
commit
83039e76d3
|
|
@ -98,17 +98,11 @@ controls_main(void)
|
|||
*/
|
||||
unsigned rc_channels = system_state.rc_channels;
|
||||
|
||||
/*
|
||||
* Track if any input got an update in this round
|
||||
*/
|
||||
bool rc_updated;
|
||||
|
||||
if (fds[0].revents & POLLIN)
|
||||
locked |= dsm_input();
|
||||
|
||||
if (fds[1].revents & POLLIN)
|
||||
locked |= sbus_input(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data,
|
||||
&system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated);
|
||||
locked |= sbus_input();
|
||||
|
||||
/*
|
||||
* If we don't have lock from one of the serial receivers,
|
||||
|
|
@ -138,16 +132,17 @@ controls_main(void)
|
|||
*/
|
||||
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
|
||||
|
||||
/* set the number of channels to zero - no inputs */
|
||||
system_state.rc_channels = 0;
|
||||
rc_updated = true;
|
||||
}
|
||||
|
||||
if (system_state.rc_channels > 0) {
|
||||
/*
|
||||
* If there was a RC update OR the RC signal status (lost / present) has
|
||||
* If the RC signal status (lost / present) has
|
||||
* just changed, request an update immediately.
|
||||
*/
|
||||
system_state.fmu_report_due |= rc_updated;
|
||||
system_state.fmu_report_due = true;
|
||||
}
|
||||
|
||||
/* set the number of channels to zero - no inputs */
|
||||
system_state.rc_channels = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* PWM output updates are performed in addition on each comm update.
|
||||
|
|
|
|||
|
|
@ -49,10 +49,13 @@
|
|||
|
||||
#define DEBUG
|
||||
#include "px4io.h"
|
||||
#include "protocol.h"
|
||||
#include "debug.h"
|
||||
|
||||
#define SBUS_FRAME_SIZE 25
|
||||
#define SBUS_INPUT_CHANNELS 16
|
||||
#define SBUS_INPUT_CHANNELS 18
|
||||
|
||||
static int sbus_fd = -1;
|
||||
|
||||
static hrt_abstime last_rx_time;
|
||||
static hrt_abstime last_frame_time;
|
||||
|
|
@ -63,14 +66,11 @@ static unsigned partial_frame_count;
|
|||
|
||||
unsigned sbus_frame_drops;
|
||||
|
||||
static int sbus_decode(hrt_abstime frame_time, unsigned max_channels,
|
||||
uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time);
|
||||
static void sbus_decode(hrt_abstime frame_time);
|
||||
|
||||
int
|
||||
sbus_init(const char *device)
|
||||
{
|
||||
static int sbus_fd = -1;
|
||||
|
||||
if (sbus_fd < 0)
|
||||
sbus_fd = open(device, O_RDONLY);
|
||||
|
||||
|
|
@ -87,18 +87,16 @@ 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;
|
||||
}
|
||||
|
||||
bool
|
||||
sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count,
|
||||
uint64_t *receive_time, bool *updated)
|
||||
sbus_input(void)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
|
@ -119,7 +117,6 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
|
|||
* if we didn't drop bytes...
|
||||
*/
|
||||
now = hrt_absolute_time();
|
||||
|
||||
if ((now - last_rx_time) > 3000) {
|
||||
if (partial_frame_count > 0) {
|
||||
sbus_frame_drops++;
|
||||
|
|
@ -131,12 +128,11 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
|
|||
* Fetch bytes, but no more than we would need to complete
|
||||
* the current frame.
|
||||
*/
|
||||
ret = read(fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
|
||||
ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
|
||||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
goto out;
|
||||
|
||||
last_rx_time = now;
|
||||
|
||||
/*
|
||||
|
|
@ -152,9 +148,9 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
|
|||
|
||||
/*
|
||||
* Great, it looks like we might have a frame. Go ahead and
|
||||
* decode it, report if the receiver got something.
|
||||
* decode it.
|
||||
*/
|
||||
*updated = (sbus_decode(now, max_channels, channel_data, channel_count, receive_time) == OK);
|
||||
sbus_decode(now);
|
||||
partial_frame_count = 0;
|
||||
|
||||
out:
|
||||
|
|
@ -182,31 +178,31 @@ struct sbus_bit_pick {
|
|||
uint8_t lshift;
|
||||
};
|
||||
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
|
||||
/* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
|
||||
/* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
|
||||
/* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
|
||||
/* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
|
||||
/* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
|
||||
/* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
|
||||
/* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
|
||||
/* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
|
||||
/* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
|
||||
/* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
|
||||
/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
|
||||
/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
|
||||
/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
|
||||
/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
|
||||
/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
|
||||
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
|
||||
/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
|
||||
/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
|
||||
/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
|
||||
/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
|
||||
/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
|
||||
/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} },
|
||||
/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
|
||||
/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} },
|
||||
/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
|
||||
/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
|
||||
/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
|
||||
/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
|
||||
/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
|
||||
/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} },
|
||||
/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
|
||||
/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} }
|
||||
};
|
||||
|
||||
static int
|
||||
sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time)
|
||||
static void
|
||||
sbus_decode(hrt_abstime frame_time)
|
||||
{
|
||||
/* check frame boundary markers to avoid out-of-sync cases */
|
||||
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
|
||||
sbus_frame_drops++;
|
||||
return 1;
|
||||
return;
|
||||
}
|
||||
|
||||
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
|
||||
|
|
@ -214,18 +210,15 @@ sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_dat
|
|||
(frame[23] & (1 << 3))) { /* failsafe */
|
||||
|
||||
/* actively announce signal loss */
|
||||
*channel_count = 0;
|
||||
|
||||
system_state.rc_channels = 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* decode failsafe and RC status */
|
||||
|
||||
/* we have received something we think is a frame */
|
||||
last_frame_time = frame_time;
|
||||
|
||||
unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ?
|
||||
SBUS_INPUT_CHANNELS : max_channels;
|
||||
unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
|
||||
SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
|
||||
|
||||
/* use the decoder matrix to extract channel data */
|
||||
for (unsigned channel = 0; channel < chancount; channel++) {
|
||||
|
|
@ -243,7 +236,6 @@ sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_dat
|
|||
value |= piece;
|
||||
}
|
||||
}
|
||||
|
||||
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
|
||||
system_state.rc_channel_data[channel] = (value / 2) + 998;
|
||||
}
|
||||
|
|
@ -257,10 +249,11 @@ sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_dat
|
|||
}
|
||||
|
||||
/* note the number of channels decoded */
|
||||
*channel_count = chancount;
|
||||
system_state.rc_channels = chancount;
|
||||
|
||||
/* and note that we have received data from the R/C controller */
|
||||
*receive_time = frame_time;
|
||||
system_state.rc_channels_timestamp = frame_time;
|
||||
|
||||
return 0;
|
||||
/* trigger an immediate report to the FMU */
|
||||
system_state.fmu_report_due = true;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue