mirror of https://github.com/ArduPilot/ardupilot
AP_RCProtocol: cleanup code style
using Tools/CodeStyle/ardupilot-astyle.sh
This commit is contained in:
parent
f9ab7e54d4
commit
1dcac14b1e
|
@ -24,7 +24,7 @@
|
|||
class AP_RCProtocol_Backend;
|
||||
class AP_RCProtocol {
|
||||
public:
|
||||
enum rcprotocol_t{
|
||||
enum rcprotocol_t {
|
||||
PPM = 0,
|
||||
SBUS,
|
||||
SBUS_NI,
|
||||
|
@ -35,10 +35,16 @@ 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();
|
||||
|
|
|
@ -19,8 +19,7 @@
|
|||
|
||||
#include "AP_RCProtocol.h"
|
||||
|
||||
class AP_RCProtocol_Backend
|
||||
{
|
||||
class AP_RCProtocol_Backend {
|
||||
friend class AP_RCProtcol;
|
||||
|
||||
public:
|
||||
|
|
|
@ -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,87 +155,92 @@ 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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -245,121 +251,126 @@ bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[
|
|||
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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -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<<bits_s0)-1) << bit_ofs;
|
||||
sbus_state.bit_ofs += bits_s0;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
@ -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<VARIANT>*/
|
||||
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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
} 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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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 {
|
||||
|
|
Loading…
Reference in New Issue