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 {