-
Notifications
You must be signed in to change notification settings - Fork 17.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
AP_RCProtocol: IBUS hack for FlySky IA6 receiver #28306
base: master
Are you sure you want to change the base?
Changes from all commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change | ||||||
---|---|---|---|---|---|---|---|---|
|
@@ -20,10 +20,12 @@ | |||||||
|
||||||||
#include "AP_RCProtocol_IBUS.h" | ||||||||
|
||||||||
static int ia6_hack = 0; | ||||||||
|
||||||||
// decode a full IBUS frame | ||||||||
bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) | ||||||||
bool AP_RCProtocol_IBUS::ibus_decode_std(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) | ||||||||
{ | ||||||||
uint32_t chksum = 96; | ||||||||
uint16_t chksum = 96; | ||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Are you sure this is correct? ie. the checksum definition must state that your result is modulo UINT16_MAX. If it does not then you may pass the checksum when you shouldn't. |
||||||||
|
||||||||
/* check frame boundary markers to avoid out-of-sync cases */ | ||||||||
if ((frame[0] != 0x20) || (frame[1] != 0x40)) { | ||||||||
|
@@ -52,6 +54,45 @@ bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint1 | |||||||
} | ||||||||
|
||||||||
|
||||||||
// decode a full IBUS frame | ||||||||
bool AP_RCProtocol_IBUS::ibus_decode_ia6(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) | ||||||||
{ | ||||||||
uint16_t chksum = 0; | ||||||||
|
||||||||
/* check frame boundary markers to avoid out-of-sync cases */ | ||||||||
if (frame[0] != 0x55) { | ||||||||
return false; | ||||||||
} | ||||||||
|
||||||||
/* use the decoder matrix to extract channel data */ | ||||||||
for (uint8_t channel = 0, pick=1; channel < IBUS_INPUT_CHANNELS; channel++, pick+=2) { | ||||||||
values[channel] = frame[pick] | ((frame[pick+1] & 0x0F)<<8); | ||||||||
chksum += values[channel]; | ||||||||
} | ||||||||
|
||||||||
uint16_t fr_chksum = frame[29] | (frame[30]<<8); | ||||||||
|
||||||||
if (chksum != fr_chksum) { | ||||||||
return false; | ||||||||
} | ||||||||
|
||||||||
if ((frame[2]&0xF0) || (frame[8]&0xF0)) { | ||||||||
*ibus_failsafe = true; | ||||||||
} else { | ||||||||
*ibus_failsafe = false; | ||||||||
} | ||||||||
|
||||||||
return true; | ||||||||
} | ||||||||
|
||||||||
bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) | ||||||||
{ | ||||||||
if (ia6_hack) { | ||||||||
return ibus_decode_ia6( frame, values, ibus_failsafe); | ||||||||
} | ||||||||
return ibus_decode_std( frame, values, ibus_failsafe); | ||||||||
} | ||||||||
|
||||||||
/* | ||||||||
process an IBUS input pulse of the given width | ||||||||
*/ | ||||||||
|
@@ -69,23 +110,32 @@ void AP_RCProtocol_IBUS::_process_byte(uint32_t timestamp_us, uint8_t b) | |||||||
const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U); | ||||||||
byte_input.last_byte_us = timestamp_us; | ||||||||
|
||||||||
|
||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Bad whitespace change |
||||||||
if (have_frame_gap) { | ||||||||
// if we have a frame gap then this must be the start of a new | ||||||||
// frame | ||||||||
byte_input.ofs = 0; | ||||||||
} | ||||||||
if (b != 0x20 && byte_input.ofs == 0) { | ||||||||
// definately not IBUS, missing header byte | ||||||||
|
||||||||
if ( !( (b == 0x20) || (b == 0x55) ) && byte_input.ofs == 0) { | ||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Will need a virtual method to return expected header byte |
||||||||
// definitely not IBUS, missing header byte | ||||||||
return; | ||||||||
} | ||||||||
|
||||||||
if (byte_input.ofs == 0 && !have_frame_gap) { | ||||||||
// must have a frame gap before the start of a new IBUS frame | ||||||||
return; | ||||||||
} | ||||||||
|
||||||||
if (byte_input.ofs == 0) | ||||||||
{ | ||||||||
Comment on lines
+130
to
+131
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
|
||||||||
ia6_hack = (b == 0x55)?1:0; | ||||||||
} | ||||||||
|
||||||||
byte_input.buf[byte_input.ofs++] = b; | ||||||||
|
||||||||
if (byte_input.ofs == sizeof(byte_input.buf)) { | ||||||||
if (byte_input.ofs == (sizeof(byte_input.buf)-ia6_hack)) // IA6 has one byte less | ||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Will need a callback for expected number of bytes in a packet |
||||||||
{ | ||||||||
uint16_t values[IBUS_INPUT_CHANNELS]; | ||||||||
bool ibus_failsafe = false; | ||||||||
log_data(AP_RCProtocol::IBUS, timestamp_us, byte_input.buf, byte_input.ofs); | ||||||||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Better to have an
AP_RCProtocol_IBUS_IA6
subclass so that other users can use this modified decoder by using the custom build server.