From 1dcac14b1eee70609e571abc524aee40219fb545 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 20 Jul 2018 14:25:40 +1000 Subject: [PATCH] AP_RCProtocol: cleanup code style using Tools/CodeStyle/ardupilot-astyle.sh --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 2 +- libraries/AP_RCProtocol/AP_RCProtocol.h | 16 +- .../AP_RCProtocol/AP_RCProtocol_Backend.cpp | 4 +- .../AP_RCProtocol/AP_RCProtocol_Backend.h | 7 +- libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp | 353 +++++++++--------- libraries/AP_RCProtocol/AP_RCProtocol_DSM.h | 8 +- .../AP_RCProtocol/AP_RCProtocol_PPMSum.cpp | 2 +- .../AP_RCProtocol/AP_RCProtocol_PPMSum.h | 2 +- .../AP_RCProtocol/AP_RCProtocol_SBUS.cpp | 193 +++++----- libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h | 4 +- .../AP_RCProtocol/AP_RCProtocol_SBUS_NI.h | 5 +- .../AP_RCProtocol/AP_RCProtocol_SRXL.cpp | 29 +- libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h | 2 +- .../AP_RCProtocol/AP_RCProtocol_ST24.cpp | 242 ++++++------ libraries/AP_RCProtocol/AP_RCProtocol_ST24.h | 6 +- .../AP_RCProtocol/AP_RCProtocol_SUMD.cpp | 266 ++++++------- libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h | 6 +- 17 files changed, 581 insertions(+), 566 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 0a24083171..f78b783e93 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index d794f1bf3e..3112302264 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once @@ -24,7 +24,7 @@ class AP_RCProtocol_Backend; class AP_RCProtocol { public: - enum rcprotocol_t{ + enum rcprotocol_t { PPM = 0, SBUS, SBUS_NI, @@ -35,15 +35,21 @@ public: NONE //last enum always is None }; void init(); - bool valid_serial_prot() { return _valid_serial_prot; } + bool valid_serial_prot() + { + return _valid_serial_prot; + } void process_pulse(uint32_t width_s0, uint32_t width_s1); void process_byte(uint8_t byte); - enum rcprotocol_t protocol_detected() { return _detected_protocol; } + enum rcprotocol_t protocol_detected() + { + return _detected_protocol; + } uint8_t num_channels(); uint16_t read(uint8_t chan); bool new_input(); void start_bind(void); - + private: enum rcprotocol_t _detected_protocol = NONE; AP_RCProtocol_Backend *backend[NONE]; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index feb71e58e1..ba7923a745 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -11,14 +11,14 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include "AP_RCProtocol.h" #include -AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) : +AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) : frontend(_frontend), rc_input_count(0), last_rc_input_count(0), diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 44b9723c57..17f5570689 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -19,8 +19,7 @@ #include "AP_RCProtocol.h" -class AP_RCProtocol_Backend -{ +class AP_RCProtocol_Backend { friend class AP_RCProtcol; public: @@ -42,7 +41,7 @@ public: }; protected: void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe); - + private: AP_RCProtocol &frontend; unsigned int rc_input_count; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index 118b6f85a3..bb6c746835 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ /* @@ -48,7 +48,7 @@ void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1) byte_ofs = dsm_state.bit_ofs/10; bit_ofs = dsm_state.bit_ofs%10; - if(byte_ofs > 15) { + if (byte_ofs > 15) { // invalid data goto reset; } @@ -134,17 +134,18 @@ reset: bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { - if (raw == 0xffff) - return false; + if (raw == 0xffff) { + return false; + } - *channel = (raw >> shift) & 0xf; + *channel = (raw >> shift) & 0xf; - uint16_t data_mask = (1 << shift) - 1; - *value = raw & data_mask; + uint16_t data_mask = (1 << shift) - 1; + *value = raw & data_mask; - //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); + //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); - return true; + return true; } /** @@ -154,212 +155,222 @@ bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigne */ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]) { - static uint32_t cs10; - static uint32_t cs11; - static unsigned samples; + static uint32_t cs10; + static uint32_t cs11; + static unsigned samples; - /* reset the 10/11 bit sniffed channel masks */ - if (reset) { - cs10 = 0; - cs11 = 0; - samples = 0; - dsm_channel_shift = 0; - return; - } + /* reset the 10/11 bit sniffed channel masks */ + if (reset) { + cs10 = 0; + cs11 = 0; + samples = 0; + dsm_channel_shift = 0; + return; + } - /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - const uint8_t *dp = &dsm_frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; + const uint8_t *dp = &dsm_frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; - /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) - cs10 |= (1 << channel); + /* if the channel decodes, remember the assigned number */ + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) { + cs10 |= (1 << channel); + } - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) - cs11 |= (1 << channel); + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) { + cs11 |= (1 << channel); + } - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ - } + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ + } - /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples++ < 5) - return; + /* wait until we have seen plenty of frames - 5 should normally be enough */ + if (samples++ < 5) { + return; + } - /* - * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognize. - * - * XXX Note that due to what seem to be bugs in the DSM2 high-resolution - * stream, we may want to sniff for longer in some cases when we think we - * are talking to a DSM2 receiver in high-resolution mode (so that we can - * reject it, ideally). - * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion - * of this issue. - */ - static uint32_t masks[] = { - 0x3f, /* 6 channels (DX6) */ - 0x7f, /* 7 channels (DX7) */ - 0xff, /* 8 channels (DX8) */ - 0x1ff, /* 9 channels (DX9, etc.) */ - 0x3ff, /* 10 channels (DX10) */ - 0x1fff, /* 13 channels (DX10t) */ - 0x3fff /* 18 channels (DX10) */ - }; - unsigned votes10 = 0; - unsigned votes11 = 0; + /* + * Iterate the set of sensible sniffed channel sets and see whether + * decoding in 10 or 11-bit mode has yielded anything we recognize. + * + * XXX Note that due to what seem to be bugs in the DSM2 high-resolution + * stream, we may want to sniff for longer in some cases when we think we + * are talking to a DSM2 receiver in high-resolution mode (so that we can + * reject it, ideally). + * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion + * of this issue. + */ + static uint32_t masks[] = { + 0x3f, /* 6 channels (DX6) */ + 0x7f, /* 7 channels (DX7) */ + 0xff, /* 8 channels (DX8) */ + 0x1ff, /* 9 channels (DX9, etc.) */ + 0x3ff, /* 10 channels (DX10) */ + 0x1fff, /* 13 channels (DX10t) */ + 0x3fff /* 18 channels (DX10) */ + }; + unsigned votes10 = 0; + unsigned votes11 = 0; - for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) { + for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) { - if (cs10 == masks[i]) - votes10++; + if (cs10 == masks[i]) { + votes10++; + } - if (cs11 == masks[i]) - votes11++; - } + if (cs11 == masks[i]) { + votes11++; + } + } - if ((votes11 == 1) && (votes10 == 0)) { - dsm_channel_shift = 11; - debug("DSM: 11-bit format"); - return; - } + if ((votes11 == 1) && (votes10 == 0)) { + dsm_channel_shift = 11; + debug("DSM: 11-bit format"); + return; + } - if ((votes10 == 1) && (votes11 == 0)) { - dsm_channel_shift = 10; - debug("DSM: 10-bit format"); - return; - } + if ((votes10 == 1) && (votes11 == 0)) { + dsm_channel_shift = 10; + debug("DSM: 10-bit format"); + return; + } - /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); - dsm_guess_format(true, dsm_frame); + /* call ourselves to reset our state ... we have to try again */ + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + dsm_guess_format(true, dsm_frame); } /** * Decode the entire dsm frame (all contained channels) * */ -bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], +bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values) { #if 0 - debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], - dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); + debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], + dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); #endif - /* - * If we have lost signal for at least a second, reset the - * format guessing heuristic. - */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { - dsm_guess_format(true, dsm_frame); + /* + * If we have lost signal for at least a second, reset the + * format guessing heuristic. + */ + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { + dsm_guess_format(true, dsm_frame); } - /* we have received something we think is a dsm_frame */ - dsm_last_frame_time = frame_time; + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; - /* if we don't know the dsm_frame format, update the guessing state machine */ - if (dsm_channel_shift == 0) { - dsm_guess_format(false, dsm_frame); - return false; - } + /* if we don't know the dsm_frame format, update the guessing state machine */ + if (dsm_channel_shift == 0) { + dsm_guess_format(false, dsm_frame); + return false; + } - /* - * The encoding of the first two bytes is uncertain, so we're - * going to ignore them for now. - * - * Each channel is a 16-bit unsigned value containing either a 10- - * or 11-bit channel value and a 4-bit channel number, shifted - * either 10 or 11 bits. The MSB may also be set to indicate the - * second dsm_frame in variants of the protocol where more than - * seven channels are being transmitted. - */ + /* + * The encoding of the first two bytes is uncertain, so we're + * going to ignore them for now. + * + * Each channel is a 16-bit unsigned value containing either a 10- + * or 11-bit channel value and a 4-bit channel number, shifted + * either 10 or 11 bits. The MSB may also be set to indicate the + * second dsm_frame in variants of the protocol where more than + * seven channels are being transmitted. + */ - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - const uint8_t *dp = &dsm_frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; + const uint8_t *dp = &dsm_frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; - if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) - continue; + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) { + continue; + } - /* ignore channels out of range */ - if (channel >= max_values) - continue; + /* ignore channels out of range */ + if (channel >= max_values) { + continue; + } - /* update the decoded channel count */ - if (channel >= *num_values) - *num_values = channel + 1; + /* update the decoded channel count */ + if (channel >= *num_values) { + *num_values = channel + 1; + } - /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (dsm_channel_shift == 10) - value *= 2; + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ + if (dsm_channel_shift == 10) { + value *= 2; + } - /* - * Spektrum scaling is special. There are these basic considerations - * - * * Midpoint is 1520 us - * * 100% travel channels are +- 400 us - * - * We obey the original Spektrum scaling (so a default setup will scale from - * 1100 - 1900 us), but we do not obey the weird 1520 us center point - * and instead (correctly) center the center around 1500 us. This is in order - * to get something useful without requiring the user to calibrate on a digital - * link for no reason. - */ + /* + * Spektrum scaling is special. There are these basic considerations + * + * * Midpoint is 1520 us + * * 100% travel channels are +- 400 us + * + * We obey the original Spektrum scaling (so a default setup will scale from + * 1100 - 1900 us), but we do not obey the weird 1520 us center point + * and instead (correctly) center the center around 1500 us. This is in order + * to get something useful without requiring the user to calibrate on a digital + * link for no reason. + */ - /* scaled integer for decent accuracy while staying efficient */ - value = ((((int)value - 1024) * 1000) / 1700) + 1500; + /* scaled integer for decent accuracy while staying efficient */ + value = ((((int)value - 1024) * 1000) / 1700) + 1500; - /* - * Store the decoded channel into the R/C input buffer, taking into - * account the different ideas about channel assignement that we have. - * - * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, - * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. - */ - switch (channel) { - case 0: - channel = 2; - break; + /* + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ + switch (channel) { + case 0: + channel = 2; + break; - case 1: - channel = 0; - break; + case 1: + channel = 0; + break; - case 2: - channel = 1; + case 2: + channel = 1; - default: - break; - } + default: + break; + } - values[channel] = value; - } + values[channel] = value; + } - /* - * Spektrum likes to send junk in higher channel numbers to fill - * their packets. We don't know about a 13 channel model in their TX - * lines, so if we get a channel count of 13, we'll return 12 (the last - * data index that is stable). - */ - if (*num_values == 13) - *num_values = 12; + /* + * Spektrum likes to send junk in higher channel numbers to fill + * their packets. We don't know about a 13 channel model in their TX + * lines, so if we get a channel count of 13, we'll return 12 (the last + * data index that is stable). + */ + if (*num_values == 13) { + *num_values = 12; + } #if 0 - if (dsm_channel_shift == 11) { - /* Set the 11-bit data indicator */ - *num_values |= 0x8000; - } + if (dsm_channel_shift == 11) { + /* Set the 11-bit data indicator */ + *num_values |= 0x8000; + } #endif - /* - * XXX Note that we may be in failsafe here; we need to work out how to detect that. - */ - return true; + /* + * XXX Note that we may be in failsafe here; we need to work out how to detect that. + */ + return true; } @@ -410,14 +421,14 @@ void AP_RCProtocol_DSM::update(void) hal.scheduler->delay_microseconds(120); hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 0); hal.scheduler->delay_microseconds(120); - hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1); + hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1); } bind_last_ms = now; bind_state = BIND_STATE4; } break; } - + case BIND_STATE4: { uint32_t now = AP_HAL::millis(); if (now - bind_last_ms > 50) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index 0900750f18..956d5be9f0 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -25,12 +25,12 @@ public: void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void start_bind(void) override; void update(void) override; - + private: void dsm_decode(); bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]); - bool dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], + bool dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values); uint64_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */ @@ -50,5 +50,5 @@ private: BIND_STATE4, } bind_state; uint32_t bind_last_ms; - + }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp index 2e9138b91b..1915d32276 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h index 48935a41d2..ae2ff9a431 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp index 22cda1119e..3f6f476a42 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp @@ -47,7 +47,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -82,120 +82,119 @@ * - left shift applied to the result into the channel value */ struct sbus_bit_pick { - uint8_t byte; - uint8_t rshift; - uint8_t mask; - uint8_t lshift; + uint8_t byte; + uint8_t rshift; + 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} } + /* 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} } }; bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, - bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { - /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f)) { - return false; - } + /* check frame boundary markers to avoid out-of-sync cases */ + if ((frame[0] != 0x0f)) { + return false; + } - switch (frame[24]) { - case 0x00: - /* this is S.BUS 1 */ - break; - case 0x03: - /* S.BUS 2 SLOT0: RX battery and external voltage */ - break; - case 0x83: - /* S.BUS 2 SLOT1 */ - break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: - break; - default: - /* we expect one of the bits above, but there are some we don't know yet */ - break; - } + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of the bits above, but there are some we don't know yet */ + break; + } - unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : max_values; + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; - /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < chancount; channel++) { - unsigned value = 0; + /* use the decoder matrix to extract channel data */ + for (unsigned channel = 0; channel < chancount; channel++) { + unsigned value = 0; - for (unsigned pick = 0; pick < 3; pick++) { - const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; + for (unsigned pick = 0; pick < 3; pick++) { + const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; - if (decode->mask != 0) { - unsigned piece = frame[1 + decode->byte]; - piece >>= decode->rshift; - piece &= decode->mask; - piece <<= decode->lshift; + if (decode->mask != 0) { + unsigned piece = frame[1 + decode->byte]; + piece >>= decode->rshift; + piece &= decode->mask; + piece <<= decode->lshift; - value |= piece; - } - } + value |= piece; + } + } - /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; - } + /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + } - /* decode switch channels if data fields are wide enough */ - if (max_values > 17 && chancount > 15) { - chancount = 18; + /* decode switch channels if data fields are wide enough */ + if (max_values > 17 && chancount > 15) { + chancount = 18; - /* channel 17 (index 16) */ - values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; - /* channel 18 (index 17) */ - values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; - } + /* channel 17 (index 16) */ + values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; + } - /* note the number of channels decoded */ - *num_values = chancount; + /* note the number of channels decoded */ + *num_values = chancount; - /* decode and handle failsafe and frame-lost flags */ - if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ - /* report that we failed to read anything valid off the receiver */ - *sbus_failsafe = true; - *sbus_frame_drop = true; - } - else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, - * e.g. by prematurely issuing return-to-launch!!! */ + /* decode and handle failsafe and frame-lost flags */ + if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ + /* report that we failed to read anything valid off the receiver */ + *sbus_failsafe = true; + *sbus_frame_drop = true; + } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + /* set a special warning flag + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, + * e.g. by prematurely issuing return-to-launch!!! */ - *sbus_failsafe = false; - *sbus_frame_drop = true; - } else { - *sbus_failsafe = false; - *sbus_frame_drop = false; - } + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; + } - return true; + return true; } @@ -222,9 +221,9 @@ void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1) goto reset; } - if (byte_ofs > 25) { - goto reset; - } + if (byte_ofs > 25) { + goto reset; + } // pull in the high bits sbus_state.bytes[byte_ofs] |= ((1U<. - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -25,7 +25,7 @@ public: void process_pulse(uint32_t width_s0, uint32_t width_s1) override; private: bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, - bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); struct { uint16_t bytes[25]; // including start bit, parity and stop bits uint16_t bit_ofs; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h index 17dadc2ed8..d6d39a4a73 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * */ #pragma once @@ -22,7 +22,8 @@ class AP_RCProtocol_SBUS_NI : public AP_RCProtocol_SBUS { public: AP_RCProtocol_SBUS_NI(AP_RCProtocol &_frontend) : AP_RCProtocol_SBUS(_frontend), saved_width(0) {} - void process_pulse(uint32_t width_s0, uint32_t width_s1) override { + void process_pulse(uint32_t width_s0, uint32_t width_s1) override + { AP_RCProtocol_SBUS::process_pulse(saved_width, width_s0); saved_width = width_s1; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp index 018a521007..ea70c37aa6 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp @@ -41,8 +41,8 @@ uint16_t AP_RCProtocol_SRXL::srxl_crc16(uint16_t crc, uint8_t new_byte) { uint8_t loop; crc = crc ^ (uint16_t)new_byte << 8; - for(loop = 0; loop < 8; loop++) { - crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); + for (loop = 0; loop < 8; loop++) { + crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); } return crc; } @@ -163,7 +163,7 @@ int AP_RCProtocol_SRXL::srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num } /* provide channel data to FMU */ - if ( (uint16_t)*num_values > max_values) { + if ((uint16_t)*num_values > max_values) { *num_values = (uint8_t)max_values; } memcpy(values, channels, (*num_values)*2); @@ -204,7 +204,7 @@ int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_v int32_t v = b & 0x7FF; if (b & 0x8000) { continue; - } + } if (c == 12) { // special handling for channel 12 // see http://www.deviationtx.com/forum/protocol-development/2088-18-channels-for-dsm2-dsmx?start=40 @@ -230,7 +230,7 @@ int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_v max_channels = c+1; } } - + //printf("%u:%u ", (unsigned)c, (unsigned)v); } //printf("\n"); @@ -244,7 +244,7 @@ int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_v // check failsafe bit, this goes low when connection to the // transmitter is lost *failsafe_state = ((buffer[1] & 2) == 0); - + // success return 0; } @@ -254,9 +254,9 @@ void AP_RCProtocol_SRXL::process_byte(uint8_t byte) uint64_t timestamp_us = AP_HAL::micros64(); /*----------------------------------------distinguish different srxl variants at the beginning of each frame---------------------------------------------- */ /* Check if we have a new begin of a frame --> indicators: Time gap in datastream + SRXL header 0xA*/ - if ( (timestamp_us - last_data_us) >= SRXL_MIN_FRAMESPACE_US) { + if ((timestamp_us - last_data_us) >= SRXL_MIN_FRAMESPACE_US) { /* Now detect SRXL variant based on header */ - switch(byte) { + switch (byte) { case SRXL_HEADER_V1: frame_len_full = SRXL_FRAMELEN_V1; frame_header = SRXL_HEADER_V1; @@ -304,11 +304,11 @@ void AP_RCProtocol_SRXL::process_byte(uint8_t byte) buflen++; /* CRC not over last 2 frame bytes as these bytes inhabitate the crc */ if (buflen <= (frame_len_full-2)) { - crc_fmu = srxl_crc16(crc_fmu,byte); + crc_fmu = srxl_crc16(crc_fmu,byte); } - if( buflen == frame_len_full ) { + if (buflen == frame_len_full) { /* CRC check here */ - crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]); + crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]); if (crc_receiver == crc_fmu) { /* at this point buffer contains all frame data and crc is valid --> extract channel info according to SRXL variant */ uint16_t values[SRXL_MAX_CHANNELS]; @@ -332,8 +332,7 @@ void AP_RCProtocol_SRXL::process_byte(uint8_t byte) } } decode_state_next = STATE_IDLE; /* frame data buffering and decoding finished --> statemachine not in use until new header drops is */ - } - else { + } else { /* frame not completely received --> frame data buffering still ongoing */ decode_state_next = STATE_COLLECT; } @@ -343,6 +342,6 @@ void AP_RCProtocol_SRXL::process_byte(uint8_t byte) break; } /* switch (decode_state) */ - decode_state = decode_state_next; - last_data_us = timestamp_us; + decode_state = decode_state_next; + last_data_us = timestamp_us; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h index 530d062628..08ed832945 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp index 3ae2c68c48..bb11cce6dc 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp @@ -50,28 +50,28 @@ extern const AP_HAL::HAL& hal; uint8_t AP_RCProtocol_ST24::st24_crc8(uint8_t *ptr, uint8_t len) { - uint8_t i, crc ; - crc = 0; + uint8_t i, crc ; + crc = 0; - while (len--) { - for (i = 0x80; i != 0; i >>= 1) { - if ((crc & 0x80) != 0) { - crc <<= 1; - crc ^= 0x07; + while (len--) { + for (i = 0x80; i != 0; i >>= 1) { + if ((crc & 0x80) != 0) { + crc <<= 1; + crc ^= 0x07; - } else { - crc <<= 1; - } + } else { + crc <<= 1; + } - if ((*ptr & i) != 0) { - crc ^= 0x07; - } - } + if ((*ptr & i) != 0) { + crc ^= 0x07; + } + } - ptr++; - } + ptr++; + } - return (crc); + return (crc); } @@ -142,141 +142,141 @@ void AP_RCProtocol_ST24::process_byte(uint8_t byte) { - switch (_decode_state) { - case ST24_DECODE_STATE_UNSYNCED: - if (byte == ST24_STX1) { - _decode_state = ST24_DECODE_STATE_GOT_STX1; + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; - } + } - break; + break; - case ST24_DECODE_STATE_GOT_STX1: - if (byte == ST24_STX2) { - _decode_state = ST24_DECODE_STATE_GOT_STX2; + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; - } else { - _decode_state = ST24_DECODE_STATE_UNSYNCED; - } + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } - break; + break; - case ST24_DECODE_STATE_GOT_STX2: + case ST24_DECODE_STATE_GOT_STX2: - /* ensure no data overflow failure or hack is possible */ - if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { - _rxpacket.length = byte; - _rxlen = 0; - _decode_state = ST24_DECODE_STATE_GOT_LEN; + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; - } else { - _decode_state = ST24_DECODE_STATE_UNSYNCED; - } + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } - break; + break; - case ST24_DECODE_STATE_GOT_LEN: - _rxpacket.type = byte; - _rxlen++; - _decode_state = ST24_DECODE_STATE_GOT_TYPE; - break; + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; - case ST24_DECODE_STATE_GOT_TYPE: - _rxpacket.st24_data[_rxlen - 1] = byte; - _rxlen++; + case ST24_DECODE_STATE_GOT_TYPE: + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; - if (_rxlen == (_rxpacket.length - 1)) { - _decode_state = ST24_DECODE_STATE_GOT_DATA; - } + if (_rxlen == (_rxpacket.length - 1)) { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } - break; + break; - case ST24_DECODE_STATE_GOT_DATA: - _rxpacket.crc8 = byte; - _rxlen++; + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; - if (st24_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + if (st24_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { - /* decode the actual packet */ + /* decode the actual packet */ - switch (_rxpacket.type) { + switch (_rxpacket.type) { - case ST24_PACKET_TYPE_CHANNELDATA12: { - uint16_t values[12]; - uint8_t num_values; - ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; - //TBD: add support for RSSI - // *rssi = d->rssi; - //*rx_count = d->packet_count; + case ST24_PACKET_TYPE_CHANNELDATA12: { + uint16_t values[12]; + uint8_t num_values; + ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + //TBD: add support for RSSI + // *rssi = d->rssi; + //*rx_count = d->packet_count; - /* this can lead to rounding of the strides */ - num_values = (MAX_RCIN_CHANNELS < 12) ? MAX_RCIN_CHANNELS : 12; + /* this can lead to rounding of the strides */ + num_values = (MAX_RCIN_CHANNELS < 12) ? MAX_RCIN_CHANNELS : 12; - unsigned stride_count = (num_values * 3) / 2; - unsigned chan_index = 0; + unsigned stride_count = (num_values * 3) / 2; + unsigned chan_index = 0; - for (unsigned i = 0; i < stride_count; i += 3) { - values[chan_index] = ((uint16_t)d->channel[i] << 4); - values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; - chan_index++; + for (unsigned i = 0; i < stride_count; i += 3) { + values[chan_index] = ((uint16_t)d->channel[i] << 4); + values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; - values[chan_index] = ((uint16_t)d->channel[i + 2]); - values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; - chan_index++; - } - } - break; + values[chan_index] = ((uint16_t)d->channel[i + 2]); + values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; - case ST24_PACKET_TYPE_CHANNELDATA24: { - uint16_t values[24]; - uint8_t num_values; - ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + case ST24_PACKET_TYPE_CHANNELDATA24: { + uint16_t values[24]; + uint8_t num_values; + ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; - //*rssi = d->rssi; - //*rx_count = d->packet_count; + //*rssi = d->rssi; + //*rx_count = d->packet_count; - /* this can lead to rounding of the strides */ - num_values = (MAX_RCIN_CHANNELS < 24) ? MAX_RCIN_CHANNELS : 24; + /* this can lead to rounding of the strides */ + num_values = (MAX_RCIN_CHANNELS < 24) ? MAX_RCIN_CHANNELS : 24; - unsigned stride_count = (num_values * 3) / 2; - unsigned chan_index = 0; + unsigned stride_count = (num_values * 3) / 2; + unsigned chan_index = 0; - for (unsigned i = 0; i < stride_count; i += 3) { - values[chan_index] = ((uint16_t)d->channel[i] << 4); - values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; - chan_index++; + for (unsigned i = 0; i < stride_count; i += 3) { + values[chan_index] = ((uint16_t)d->channel[i] << 4); + values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; - values[chan_index] = ((uint16_t)d->channel[i + 2]); - values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; - chan_index++; - } - } - break; + values[chan_index] = ((uint16_t)d->channel[i + 2]); + values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; - case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { - // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; - /* we silently ignore this data for now, as it is unused */ - } - break; + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + /* we silently ignore this data for now, as it is unused */ + } + break; - default: - break; - } + default: + break; + } - } else { - /* decoding failed */ - } + } else { + /* decoding failed */ + } - _decode_state = ST24_DECODE_STATE_UNSYNCED; - break; - } + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; + } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h index 82aeca26f1..6676125762 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -48,7 +48,7 @@ private: ST24_PACKET_TYPE_TRANSMITTERGPSDATA }; - #pragma pack(push, 1) +#pragma pack(push, 1) typedef struct { uint8_t header1; ///< 0x55 for a valid packet uint8_t header2; ///< 0x55 for a valid packet @@ -129,7 +129,7 @@ private: uint8_t pressCompassStatus; ///< baro / compass status } TelemetryData; - #pragma pack(pop) +#pragma pack(pop) enum ST24_DECODE_STATE { ST24_DECODE_STATE_UNSYNCED = 0, diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp index ee1e291337..fef17fa24d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp @@ -66,20 +66,20 @@ extern const AP_HAL::HAL& hal; uint16_t AP_RCProtocol_SUMD::sumd_crc16(uint16_t crc, uint8_t value) { - int i; - crc ^= (uint16_t)value << 8; + int i; + crc ^= (uint16_t)value << 8; - for (i = 0; i < 8; i++) { - crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); - } + for (i = 0; i < 8; i++) { + crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); + } - return crc; + return crc; } uint8_t AP_RCProtocol_SUMD::sumd_crc8(uint8_t crc, uint8_t value) { - crc += value; - return crc; + crc += value; + return crc; } void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1) @@ -97,9 +97,9 @@ void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1) byte_ofs = sumd_state.bit_ofs/10; bit_ofs = sumd_state.bit_ofs%10; - if (byte_ofs >= SUMD_FRAME_MAXLEN) { - goto reset; - } + if (byte_ofs >= SUMD_FRAME_MAXLEN) { + goto reset; + } // pull in the high bits nbits = bits_s0; if (nbits+bit_ofs > 10) { @@ -147,170 +147,170 @@ reset: void AP_RCProtocol_SUMD::process_byte(uint8_t byte) { - switch (_decode_state) { - case SUMD_DECODE_STATE_UNSYNCED: + switch (_decode_state) { + case SUMD_DECODE_STATE_UNSYNCED: #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_UNSYNCED \n") ; #endif - if (byte == SUMD_HEADER_ID) { - _rxpacket.header = byte; - _sumd = true; - _rxlen = 0; - _crc16 = 0x0000; - _crc8 = 0x00; - _crcOK = false; - _crc16 = sumd_crc16(_crc16, byte); - _crc8 = sumd_crc8(_crc8, byte); - _decode_state = SUMD_DECODE_STATE_GOT_HEADER; + if (byte == SUMD_HEADER_ID) { + _rxpacket.header = byte; + _sumd = true; + _rxlen = 0; + _crc16 = 0x0000; + _crc8 = 0x00; + _crcOK = false; + _crc16 = sumd_crc16(_crc16, byte); + _crc8 = sumd_crc8(_crc8, byte); + _decode_state = SUMD_DECODE_STATE_GOT_HEADER; #ifdef SUMD_DEBUG - hal.console->printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; + hal.console->printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; #endif - } - break; + } + break; - case SUMD_DECODE_STATE_GOT_HEADER: - if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { - _rxpacket.status = byte; + case SUMD_DECODE_STATE_GOT_HEADER: + if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { + _rxpacket.status = byte; - if (byte == SUMD_ID_SUMH) { - _sumd = false; - } + if (byte == SUMD_ID_SUMH) { + _sumd = false; + } - if (_sumd) { - _crc16 = sumd_crc16(_crc16, byte); + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); - } else { - _crc8 = sumd_crc8(_crc8, byte); - } + } else { + _crc8 = sumd_crc8(_crc8, byte); + } - _decode_state = SUMD_DECODE_STATE_GOT_STATE; + _decode_state = SUMD_DECODE_STATE_GOT_STATE; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; #endif - } else { - _decode_state = SUMD_DECODE_STATE_UNSYNCED; - } + } else { + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + } - break; + break; - case SUMD_DECODE_STATE_GOT_STATE: - if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { - _rxpacket.length = byte; + case SUMD_DECODE_STATE_GOT_STATE: + if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { + _rxpacket.length = byte; - if (_sumd) { - _crc16 = sumd_crc16(_crc16, byte); + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); - } else { - _crc8 = sumd_crc8(_crc8, byte); - } + } else { + _crc8 = sumd_crc8(_crc8, byte); + } - _rxlen++; - _decode_state = SUMD_DECODE_STATE_GOT_LEN; + _rxlen++; + _decode_state = SUMD_DECODE_STATE_GOT_LEN; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; #endif - } else { - _decode_state = SUMD_DECODE_STATE_UNSYNCED; - } + } else { + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + } - break; + break; - case SUMD_DECODE_STATE_GOT_LEN: - _rxpacket.sumd_data[_rxlen] = byte; + case SUMD_DECODE_STATE_GOT_LEN: + _rxpacket.sumd_data[_rxlen] = byte; - if (_sumd) { - _crc16 = sumd_crc16(_crc16, byte); + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); - } else { - _crc8 = sumd_crc8(_crc8, byte); - } + } else { + _crc8 = sumd_crc8(_crc8, byte); + } - _rxlen++; + _rxlen++; - if (_rxlen <= ((_rxpacket.length * 2))) { + if (_rxlen <= ((_rxpacket.length * 2))) { #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ; #endif - } else { - _decode_state = SUMD_DECODE_STATE_GOT_DATA; + } else { + _decode_state = SUMD_DECODE_STATE_GOT_DATA; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; #endif - } + } - break; + break; - case SUMD_DECODE_STATE_GOT_DATA: - _rxpacket.crc16_high = byte; + case SUMD_DECODE_STATE_GOT_DATA: + _rxpacket.crc16_high = byte; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; #endif - if (_sumd) { - _decode_state = SUMD_DECODE_STATE_GOT_CRC; + if (_sumd) { + _decode_state = SUMD_DECODE_STATE_GOT_CRC; - } else { - _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; - } + } else { + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; + } - break; + break; - case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: - _rxpacket.crc16_low = byte; + case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: + _rxpacket.crc16_low = byte; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; #endif - _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; - break; + break; - case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: - _rxpacket.telemetry = byte; + case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: + _rxpacket.telemetry = byte; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; #endif - _decode_state = SUMD_DECODE_STATE_GOT_CRC; + _decode_state = SUMD_DECODE_STATE_GOT_CRC; - break; + break; - case SUMD_DECODE_STATE_GOT_CRC: - if (_sumd) { - _rxpacket.crc16_low = byte; + case SUMD_DECODE_STATE_GOT_CRC: + if (_sumd) { + _rxpacket.crc16_low = byte; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; #endif - if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { - _crcOK = true; - } + if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { + _crcOK = true; + } - } else { - _rxpacket.crc8 = byte; + } else { + _rxpacket.crc8 = byte; #ifdef SUMD_DEBUG hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; #endif - if (_crc8 == _rxpacket.crc8) { - _crcOK = true; - } - } + if (_crc8 == _rxpacket.crc8) { + _crcOK = true; + } + } - if (_crcOK) { + if (_crcOK) { #ifdef SUMD_DEBUG hal.console->printf(" CRC - OK \n") ; #endif @@ -318,69 +318,69 @@ void AP_RCProtocol_SUMD::process_byte(uint8_t byte) #ifdef SUMD_DEBUG hal.console->printf(" Got valid SUMD Packet\n") ; #endif - } else { + } else { #ifdef SUMD_DEBUG hal.console->printf(" Got valid SUMH Packet\n") ; #endif - } + } #ifdef SUMD_DEBUG - hal.console->printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ; + hal.console->printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ; #endif - unsigned i; + unsigned i; uint8_t num_values; uint16_t values[SUMD_MAX_CHANNELS]; - /* received Channels */ - if ((uint16_t)_rxpacket.length > SUMD_MAX_CHANNELS) { - _rxpacket.length = (uint8_t) SUMD_MAX_CHANNELS; - } + /* received Channels */ + if ((uint16_t)_rxpacket.length > SUMD_MAX_CHANNELS) { + _rxpacket.length = (uint8_t) SUMD_MAX_CHANNELS; + } - num_values = (uint16_t)_rxpacket.length; + num_values = (uint16_t)_rxpacket.length; - /* decode the actual packet */ - /* reorder first 4 channels */ + /* decode the actual packet */ + /* reorder first 4 channels */ - /* ch1 = roll -> sumd = ch2 */ - values[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3; - /* ch2 = pitch -> sumd = ch2 */ - values[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3; - /* ch3 = throttle -> sumd = ch2 */ - values[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3; - /* ch4 = yaw -> sumd = ch2 */ - values[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; + /* ch1 = roll -> sumd = ch2 */ + values[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3; + /* ch2 = pitch -> sumd = ch2 */ + values[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3; + /* ch3 = throttle -> sumd = ch2 */ + values[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3; + /* ch4 = yaw -> sumd = ch2 */ + values[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; - /* we start at channel 5(index 4) */ - unsigned chan_index = 4; + /* we start at channel 5(index 4) */ + unsigned chan_index = 4; - for (i = 4; i < _rxpacket.length; i++) { + for (i = 4; i < _rxpacket.length; i++) { #ifdef SUMD_DEBUG hal.console->printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2], - ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, - ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); #endif - values[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; + values[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; - chan_index++; - } + chan_index++; + } if (_rxpacket.status == 0x01) { add_input(num_values, values, false); } else if (_rxpacket.status == 0x81) { - add_input(num_values, values, true); + add_input(num_values, values, true); } - } else { + } else { #ifdef SUMD_DEBUG hal.console->printf(" CRC - fail 0x%X 0x%X\n", _crc16, (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low); #endif - } + } - _decode_state = SUMD_DECODE_STATE_UNSYNCED; - break; - } + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + break; + } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h index 5c09294da6..696551d79e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -29,7 +29,7 @@ private: static uint16_t sumd_crc16(uint16_t crc, uint8_t value); static uint8_t sumd_crc8(uint8_t crc, uint8_t value); - #pragma pack(push, 1) +#pragma pack(push, 1) typedef struct { uint8_t header; ///< 0xA8 for a valid packet uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe @@ -40,7 +40,7 @@ private: uint8_t telemetry; ///< Telemetry request uint8_t crc8; ///< SUMH CRC8 } ReceiverFcPacketHoTT; - #pragma pack(pop) +#pragma pack(pop) enum SUMD_DECODE_STATE {