293 lines
8.8 KiB
C++
293 lines
8.8 KiB
C++
#include "crsf.hpp"
|
|
#include <algorithm>
|
|
|
|
#define CRSF_VERBOSE(...)
|
|
#define CRSF_DEBUG(...)
|
|
|
|
struct crsf_frame_header_t {
|
|
uint8_t device_address; ///< @see crsf_address_t
|
|
uint8_t length; ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t)
|
|
};
|
|
|
|
struct crsf_frame_t {
|
|
crsf_frame_header_t header;
|
|
uint8_t type; ///< @see crsf_frame_type_t
|
|
uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end
|
|
};
|
|
|
|
enum class crsf_frame_type_t : uint8_t {
|
|
gps = 0x02,
|
|
battery_sensor = 0x08,
|
|
link_statistics = 0x14,
|
|
rc_channels_packed = 0x16,
|
|
attitude = 0x1E,
|
|
flight_mode = 0x21,
|
|
|
|
// Extended Header Frames, range: 0x28 to 0x96
|
|
device_ping = 0x28,
|
|
device_info = 0x29,
|
|
parameter_settings_entry = 0x2B,
|
|
parameter_read = 0x2C,
|
|
parameter_write = 0x2D,
|
|
command = 0x32
|
|
};
|
|
|
|
enum class crsf_payload_size_t : uint8_t {
|
|
gps = 15,
|
|
battery_sensor = 8,
|
|
link_statistics = 10,
|
|
rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
|
|
attitude = 6,
|
|
};
|
|
|
|
enum class crsf_parser_state_t : uint8_t {
|
|
unsynced = 0,
|
|
synced
|
|
};
|
|
|
|
#pragma pack(push, 1)
|
|
struct crsf_payload_RC_channels_packed_t {
|
|
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
|
|
unsigned chan0 : 11;
|
|
unsigned chan1 : 11;
|
|
unsigned chan2 : 11;
|
|
unsigned chan3 : 11;
|
|
unsigned chan4 : 11;
|
|
unsigned chan5 : 11;
|
|
unsigned chan6 : 11;
|
|
unsigned chan7 : 11;
|
|
unsigned chan8 : 11;
|
|
unsigned chan9 : 11;
|
|
unsigned chan10 : 11;
|
|
unsigned chan11 : 11;
|
|
unsigned chan12 : 11;
|
|
unsigned chan13 : 11;
|
|
unsigned chan14 : 11;
|
|
unsigned chan15 : 11;
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
static crsf_frame_t crsf_frame;
|
|
static unsigned current_frame_position = 0;
|
|
static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;
|
|
|
|
/**
|
|
* parse the current crsf_frame buffer
|
|
*/
|
|
bool crsf_parse(const uint8_t *frame, unsigned len, uint16_t *values,
|
|
uint16_t *num_values, uint16_t max_channels);
|
|
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
|
|
uint8_t crsf_frame_CRC(const crsf_frame_t &frame);
|
|
|
|
bool crsf_parse(const uint8_t *frame, unsigned len, uint16_t *values,
|
|
uint16_t *num_values, uint16_t max_channels)
|
|
{
|
|
bool ret = false;
|
|
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
|
|
|
|
while (len > 0) {
|
|
|
|
// fill in the crsf_buffer, as much as we can
|
|
const unsigned current_len = std::min(len, sizeof(crsf_frame_t) - current_frame_position);
|
|
memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
|
|
current_frame_position += current_len;
|
|
|
|
// protection to guarantee parsing progress
|
|
if (current_len == 0) {
|
|
CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);
|
|
|
|
for (unsigned i = 0; i < current_frame_position; ++i) {
|
|
CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
|
|
}
|
|
|
|
// reset the parser
|
|
current_frame_position = 0;
|
|
parser_state = crsf_parser_state_t::unsynced;
|
|
return false;
|
|
}
|
|
|
|
len -= current_len;
|
|
frame += current_len;
|
|
|
|
if (crsf_parse_buffer(values, num_values, max_channels)) {
|
|
ret = true;
|
|
}
|
|
}
|
|
|
|
|
|
return ret;
|
|
}
|
|
|
|
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
|
|
{
|
|
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
|
|
|
|
if (parser_state == crsf_parser_state_t::unsynced) {
|
|
// there is no sync byte, try to find an RC packet by searching for a matching frame length and type
|
|
for (unsigned i = 1; i < current_frame_position - 1; ++i) {
|
|
if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
|
|
crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
|
|
parser_state = crsf_parser_state_t::synced;
|
|
unsigned frame_offset = i - 1;
|
|
CRSF_VERBOSE("RC channels found at offset %i", frame_offset);
|
|
|
|
// move the rest of the buffer to the beginning
|
|
if (frame_offset != 0) {
|
|
memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
|
|
current_frame_position -= frame_offset;
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (parser_state != crsf_parser_state_t::synced) {
|
|
if (current_frame_position >= sizeof(crsf_frame_t)) {
|
|
// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
|
|
current_frame_position = 3;
|
|
|
|
for (unsigned i = 0; i < current_frame_position; ++i) {
|
|
crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
|
|
}
|
|
|
|
CRSF_VERBOSE("Discarding buffer");
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
if (current_frame_position < 3) {
|
|
// wait until we have the header & type
|
|
return false;
|
|
}
|
|
|
|
// Now we have at least the header and the type
|
|
|
|
const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);
|
|
|
|
if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
|
|
// frame too long or bogus -> discard everything and go into unsynced state
|
|
current_frame_position = 0;
|
|
parser_state = crsf_parser_state_t::unsynced;
|
|
CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
|
|
return false;
|
|
}
|
|
|
|
if (current_frame_position < current_frame_length) {
|
|
// we don't have the full frame yet -> wait for more data
|
|
CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
|
|
return false;
|
|
}
|
|
|
|
bool ret = false;
|
|
|
|
// Now we have the full frame
|
|
|
|
if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
|
|
crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
|
|
const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];
|
|
|
|
if (crc == crsf_frame_CRC(crsf_frame)) {
|
|
const crsf_payload_RC_channels_packed_t *const rc_channels =
|
|
(crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
|
|
*num_values = std::min<uint16_t>(max_channels, 16);
|
|
|
|
if (max_channels > 0) { values[0] = (rc_channels->chan0); }
|
|
|
|
if (max_channels > 1) { values[1] = (rc_channels->chan1); }
|
|
|
|
if (max_channels > 2) { values[2] = (rc_channels->chan2); }
|
|
|
|
if (max_channels > 3) { values[3] = (rc_channels->chan3); }
|
|
|
|
if (max_channels > 4) { values[4] = (rc_channels->chan4); }
|
|
|
|
if (max_channels > 5) { values[5] = (rc_channels->chan5); }
|
|
|
|
if (max_channels > 6) { values[6] = (rc_channels->chan6); }
|
|
|
|
if (max_channels > 7) { values[7] = (rc_channels->chan7); }
|
|
|
|
if (max_channels > 8) { values[8] = (rc_channels->chan8); }
|
|
|
|
if (max_channels > 9) { values[9] = (rc_channels->chan9); }
|
|
|
|
if (max_channels > 10) { values[10] = (rc_channels->chan10); }
|
|
|
|
if (max_channels > 11) { values[11] = (rc_channels->chan11); }
|
|
|
|
if (max_channels > 12) { values[12] = (rc_channels->chan12); }
|
|
|
|
if (max_channels > 13) { values[13] = (rc_channels->chan13); }
|
|
|
|
if (max_channels > 14) { values[14] = (rc_channels->chan14); }
|
|
|
|
if (max_channels > 15) { values[15] = (rc_channels->chan15); }
|
|
|
|
CRSF_VERBOSE("Got Channels");
|
|
|
|
ret = true;
|
|
|
|
} else {
|
|
CRSF_DEBUG("CRC check failed");
|
|
}
|
|
|
|
} else {
|
|
CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
|
|
// We could check the CRC here and reset the parser into unsynced state if it fails.
|
|
// But in practise it's robust even without that.
|
|
}
|
|
|
|
// Either reset or move the rest of the buffer
|
|
if (current_frame_position > current_frame_length) {
|
|
CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
|
|
memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
|
|
current_frame_position -= current_frame_length;
|
|
|
|
} else {
|
|
current_frame_position = 0;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
|
|
{
|
|
crc ^= a;
|
|
|
|
for (int i = 0; i < 8; ++i) {
|
|
if (crc & 0x80) {
|
|
crc = (crc << 1) ^ 0xD5;
|
|
|
|
} else {
|
|
crc = crc << 1;
|
|
}
|
|
}
|
|
|
|
return crc;
|
|
}
|
|
|
|
uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
|
|
{
|
|
uint8_t crc = 0;
|
|
|
|
for (int i = 0; i < len; ++i) {
|
|
crc = crc8_dvb_s2(crc, buf[i]);
|
|
}
|
|
|
|
return crc;
|
|
}
|
|
|
|
uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
|
|
{
|
|
// CRC includes type and payload
|
|
uint8_t crc = crc8_dvb_s2(0, frame.type);
|
|
|
|
for (int i = 0; i < frame.header.length - 2; ++i) {
|
|
crc = crc8_dvb_s2(crc, frame.payload[i]);
|
|
}
|
|
|
|
return crc;
|
|
} |