From 83039e76d30c86e72ecf9836d00a64df5a4b9acd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jan 2013 01:32:39 +0100 Subject: Reverted unwanted S.Bus changes --- apps/px4io/controls.c | 23 +++++-------- apps/px4io/sbus.c | 91 ++++++++++++++++++++++++--------------------------- 2 files changed, 51 insertions(+), 63 deletions(-) (limited to 'apps/px4io') diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 564687b58..297fea017 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -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,17 +132,18 @@ controls_main(void) */ 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 */ 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. * the updates here are required to ensure operation if FMU is not started diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index f7a74cd12..f84e5df8a 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -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; @@ -111,7 +109,7 @@ sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *chan * frame transmission time is ~2ms. * * 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. * * 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... */ 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; /* @@ -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 (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + goto out; /* * 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: /* - * 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; } @@ -181,51 +177,48 @@ struct sbus_bit_pick { uint8_t mask; 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} } +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} } }; -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 */ if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ + (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; } -- cgit v1.2.3