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

Reverted unwanted S.Bus changes

This commit is contained in:
Lorenz Meier 2013-01-06 01:32:39 +01:00
parent ab85d201ee
commit 83039e76d3
2 changed files with 51 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
apps/px4io/controls.c
View File

@ -98,17 +98,11 @@ controls_main(void)
*/ */
unsigned rc_channels = system_state.rc_channels; 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) if (fds[0].revents & POLLIN)
locked |= dsm_input(); locked |= dsm_input();
if (fds[1].revents & POLLIN) if (fds[1].revents & POLLIN)
locked |= sbus_input(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data, locked |= sbus_input();
&system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated);
/* /*
* If we don't have lock from one of the serial receivers, * If we don't have lock from one of the serial receivers,
@ -138,17 +132,18 @@ controls_main(void)
*/ */
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
if (system_state.rc_channels > 0) {
/*
* If the RC signal status (lost / present) has
* just changed, request an update immediately.
*/
system_state.fmu_report_due = true;
}
/* set the number of channels to zero - no inputs */ /* set the number of channels to zero - no inputs */
system_state.rc_channels = 0; system_state.rc_channels = 0;
rc_updated = true;
} }
/*
* If there was a RC update OR the RC signal status (lost / present) has
* just changed, request an update immediately.
*/
system_state.fmu_report_due |= rc_updated;
/* /*
* PWM output updates are performed in addition on each comm update. * PWM output updates are performed in addition on each comm update.
* the updates here are required to ensure operation if FMU is not started * the updates here are required to ensure operation if FMU is not started

91
apps/px4io/sbus.c
View File

@ -49,10 +49,13 @@
#define DEBUG #define DEBUG
#include "px4io.h" #include "px4io.h"
#include "protocol.h"
#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 hrt_abstime last_rx_time; static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time; static hrt_abstime last_frame_time;
@ -63,14 +66,11 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops; unsigned sbus_frame_drops;
static int sbus_decode(hrt_abstime frame_time, unsigned max_channels, static void sbus_decode(hrt_abstime frame_time);
uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time);
int int
sbus_init(const char *device) sbus_init(const char *device)
{ {
static int sbus_fd = -1;
if (sbus_fd < 0) if (sbus_fd < 0)
sbus_fd = open(device, O_RDONLY); sbus_fd = open(device, O_RDONLY);
@ -87,18 +87,16 @@ 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;
} }
bool bool
sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, sbus_input(void)
uint64_t *receive_time, bool *updated)
{ {
ssize_t ret; ssize_t ret;
hrt_abstime now; hrt_abstime now;
@ -111,7 +109,7 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
* frame transmission time is ~2ms. * frame transmission time is ~2ms.
* *
* We expect to only be called when bytes arrive for processing, * We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls, * and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame. * the first byte we read will be the first byte of a frame.
* *
* In the case where byte(s) are dropped from a frame, this also * In the case where byte(s) are dropped from a frame, this also
@ -119,7 +117,6 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
* if we didn't drop bytes... * if we didn't drop bytes...
*/ */
now = hrt_absolute_time(); now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) { if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) { if (partial_frame_count > 0) {
sbus_frame_drops++; 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 * Fetch bytes, but no more than we would need to complete
* the current frame. * 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 the read failed for any reason, just give up here */
if (ret < 1) if (ret < 1)
goto out; goto out;
last_rx_time = now; last_rx_time = now;
/* /*
@ -148,18 +144,18 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan
* If we don't have a full frame, return * If we don't have a full frame, return
*/ */
if (partial_frame_count < SBUS_FRAME_SIZE) if (partial_frame_count < SBUS_FRAME_SIZE)
goto out; goto out;
/* /*
* Great, it looks like we might have a frame. Go ahead and * 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; partial_frame_count = 0;
out: out:
/* /*
* If we have seen a frame in the last 200ms, we consider ourselves 'locked' * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/ */
return (now - last_frame_time) < 200000; return (now - last_frame_time) < 200000;
} }
@ -181,51 +177,48 @@ struct sbus_bit_pick {
uint8_t mask; uint8_t mask;
uint8_t lshift; uint8_t lshift;
}; };
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} } /* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} }
}; };
static int static void
sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time) sbus_decode(hrt_abstime frame_time)
{ {
/* check frame boundary markers to avoid out-of-sync cases */ /* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
sbus_frame_drops++; sbus_frame_drops++;
return 1; return;
} }
/* if the failsafe or connection lost 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 << 2)) && /* signal lost */ if ((frame[23] & (1 << 2)) && /* signal lost */
(frame[23] & (1 << 3))) { /* failsafe */ (frame[23] & (1 << 3))) { /* failsafe */
/* actively announce signal loss */ /* actively announce signal loss */
*channel_count = 0; system_state.rc_channels = 0;
return 1; return 1;
} }
/* decode failsafe and RC status */
/* we have received something we think is a frame */ /* we have received something we think is a frame */
last_frame_time = frame_time; last_frame_time = frame_time;
unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ? unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : max_channels; SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
/* use the decoder matrix to extract channel data */ /* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) { 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; value |= piece;
} }
} }
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998; 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 */ /* 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 */ /* 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;
} }