Skip to content
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.

Sign up for GitHub

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

Jump to bottom

Decode high duty-cycle CRSF frames using frame marker rather than timeouts #26183

Merged
merged 3 commits into from
Feb 22, 2024
Merged

Decode high duty-cycle CRSF frames using frame marker rather than timeouts #26183

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
aab580f
AP_RCProtocol: add tests for CRSF and fix protocol test
andyp1per Feb 14, 2024
c051bbe
AP_RCProtocol: decode high duty-cycle CRSF frames using frame markers…
andyp1per Feb 10, 2024
b464712
AP_RCProtocol: CRSF: use subtraction with times, not time+timedelta
peterbarker Feb 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
144 changes: 95 additions & 49 deletions libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0)
# define debug(fmt, args...) do {} while(0)
#endif

#define CRSF_FRAME_TIMEOUT_US 10000U // 10ms to account for scheduling delays
#define CRSF_FRAME_TIMEOUT_US 50000U // 50ms to account for failure of the frame sync and long scheduling delays
#define CRSF_INTER_FRAME_TIME_US_250HZ 4000U // At fastest, frames are sent by the transmitter every 4 ms, 250 Hz
#define CRSF_INTER_FRAME_TIME_US_150HZ 6667U // At medium, frames are sent by the transmitter every 6.667 ms, 150 Hz
#define CRSF_INTER_FRAME_TIME_US_50HZ 20000U // At slowest, frames are sent by the transmitter every 20ms, 50 Hz
Expand Down Expand Up @@ -212,76 +212,132 @@ uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const {
}
}

void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
// process a byte provided by a uart from rc stack
void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate)
{
// reject RC data if we have been configured for standalone mode
if ((baudrate != CRSF_BAUDRATE && baudrate != CRSF_BAUDRATE_1MBIT && baudrate != CRSF_BAUDRATE_2MBIT) || _uart) {
return;
}
_process_byte(byte);
}

// process a byte provided by a uart
void AP_RCProtocol_CRSF::_process_byte(uint8_t byte)
{
//debug("process_byte(0x%x)", byte);
// we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail,
// however thread scheduling can introduce longer delays even when the data has been received
if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > CRSF_FRAME_TIMEOUT_US) {
const uint32_t now = AP_HAL::micros();

// extra check for overflow, should never happen since it will have been handled in check_frame()
if (_frame_ofs >= sizeof(_frame)) {
_frame_ofs = 0;
}

// overflow check
if (_frame_ofs >= CRSF_FRAMELEN_MAX) {
// check for long frame gaps
// we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail,
// however thread scheduling can introduce longer delays even when the data has been received
if (_frame_ofs > 0 && (now - _start_frame_time_us) > CRSF_FRAME_TIMEOUT_US) {
_frame_ofs = 0;
}

// start of a new frame
if (_frame_ofs == 0) {
_start_frame_time_us = timestamp_us;
_start_frame_time_us = now;
}

_frame_bytes[_frame_ofs++] = byte;

if (!check_frame(now)) {
skip_to_next_frame(now);
}
}

add_to_buffer(_frame_ofs++, byte);
// check if a frame is valid. Return false if the frame is definitely
// invalid. Return true if we need more bytes
bool AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us)
{
// overflow check
if (_frame_ofs >= sizeof(_frame)) {
return false;
}

// need a header to get the length
if (_frame_ofs < CRSF_HEADER_TYPE_LEN) {
return;
return true;
}

if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
return;
}

// parse the length
if (_frame_ofs == CRSF_HEADER_TYPE_LEN) {
_frame_crc = crc8_dvb_s2(0, _frame.type);
// check for garbage frame
if (_frame.length > CRSF_FRAME_PAYLOAD_MAX) {
_frame_ofs = 0;
}
return;
}

// update crc
if (_frame_ofs < _frame.length + CRSF_HEADER_LEN) {
_frame_crc = crc8_dvb_s2(_frame_crc, byte);
return false;
}

// overflow check
if (_frame_ofs > _frame.length + CRSF_HEADER_LEN) {
_frame_ofs = 0;
return;
// check validity of the length byte if we have received it
if (_frame_ofs >= CRSF_HEADER_TYPE_LEN &&
_frame.length > CRSF_FRAME_PAYLOAD_MAX) {
return false;
}

// decode whatever we got and expect
if (_frame_ofs == _frame.length + CRSF_HEADER_LEN) {
log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CRSF_HEADER_LEN);
if (_frame_ofs >= _frame.length + CRSF_HEADER_LEN) {
const uint8_t crc = crc8_dvb_s2_update(0, &_frame_bytes[CRSF_HEADER_LEN], _frame.length - 1);

// we consumed the partial frame, reset
_frame_ofs = 0;
//debug("check_frame(0x%x, 0x%x)", _frame.device_address, _frame.length);

// bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index)
if (_frame_crc != _frame.payload[_frame.length - 2]) {
return;
if (crc != _frame.payload[_frame.length - 2]) {
return false;
}

_last_frame_time_us = _last_rx_frame_time_us = timestamp_us;
log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame.length + CRSF_HEADER_LEN);

// decode here
if (decode_crsf_packet()) {
_last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter
add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality);
}

// we consumed the frame
const auto len = _frame.length + CRSF_HEADER_LEN;
_frame_ofs -= len;
if (_frame_ofs > 0) {
memmove(_frame_bytes, _frame_bytes+len, _frame_ofs);
}

_last_frame_time_us = _last_rx_frame_time_us = timestamp_us;

return true;
}

// more bytes to come
return true;
}

// called when parsing or CRC fails on a frame
void AP_RCProtocol_CRSF::skip_to_next_frame(uint32_t timestamp_us)
{
if (_frame_ofs <= 1) {
return;
}

/*
look for a frame header in the remaining bytes
*/
const uint8_t *new_header = (const uint8_t *)memchr(&_frame_bytes[1], DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER, _frame_ofs - 1);
if (new_header == nullptr) {
_frame_ofs = 0;
return;
}

/*
setup the current state as the remaining bytes, if any
*/
_frame_ofs -= (new_header - _frame_bytes);
if (_frame_ofs > 0) {
memmove(_frame_bytes, new_header, _frame_ofs);
}

_start_frame_time_us = timestamp_us;

// we could now have a good frame
check_frame(timestamp_us);
}

void AP_RCProtocol_CRSF::update(void)
Expand All @@ -299,7 +355,7 @@ void AP_RCProtocol_CRSF::update(void)
for (uint8_t i = 0; i < n; i++) {
int16_t b = _uart->read();
if (b >= 0) {
_process_byte(AP_HAL::micros(), uint8_t(b));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

could you explain what the "standalone" mode is for? It seems to be setup by setting SERIALn_PROTOCOL to CRSF instead of RCIN, but I don't see how it fits in. It has clearly been broken for a long time so I wonder if it is really needed at all?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pretty much all TBS kit supports CRSF control without RC packets. e.g. like SmartAudio for VTX or I have a TBS VTX that supports OSD natively via CRSF - the standalone mode allows you to support these devices. The format is pretty much the same, its just how you integrate that is not - you can't do it via the RC protocol because that essentially assumes that you have data always coming in that you need to reply to. These devices are genuine full-duplex so the FC is often initiating the comms. This was the original CRSF support, I have several copters using it I just haven't flown them for a while - I'm not planning on losing it.

_process_byte(uint8_t(b));
}
}
}
Expand Down Expand Up @@ -570,16 +626,6 @@ void AP_RCProtocol_CRSF::process_link_stats_tx_frame(const void* data)
}
}

// process a byte provided by a uart
void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate)
{
// reject RC data if we have been configured for standalone mode
if ((baudrate != CRSF_BAUDRATE && baudrate != CRSF_BAUDRATE_1MBIT && baudrate != CRSF_BAUDRATE_2MBIT) || _uart) {
return;
}
_process_byte(AP_HAL::micros(), byte);
}

// start the uart if we have one
void AP_RCProtocol_CRSF::start_uart()
{
Expand Down
12 changes: 6 additions & 6 deletions libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,13 +58,13 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
bool is_rx_active() const override {
// later versions of CRSFv3 will send link rate frames every 200ms
// but only before an initial failsafe
return AP_HAL::micros() < _last_rx_frame_time_us + CRSF_RX_TIMEOUT;
return _last_rx_frame_time_us != 0 && AP_HAL::micros() - _last_rx_frame_time_us < CRSF_RX_TIMEOUT;
}

// is the transmitter active, used to adjust telemetry data
bool is_tx_active() const {
// this is the same as the Copter failsafe timeout
return AP_HAL::micros() < _last_tx_frame_time_us + CRSF_TX_TIMEOUT;
return _last_tx_frame_time_us != 0 && AP_HAL::micros() - _last_tx_frame_time_us < CRSF_TX_TIMEOUT;
}

// get singleton instance
Expand Down Expand Up @@ -289,15 +289,17 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {

private:
struct Frame _frame;
uint8_t *_frame_bytes = (uint8_t*)&_frame;
struct Frame _telemetry_frame;
uint8_t _frame_ofs;
uint8_t _frame_crc;

const uint8_t MAX_CHANNELS = MIN((uint8_t)CRSF_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS);

static AP_RCProtocol_CRSF* _singleton;

void _process_byte(uint32_t timestamp_us, uint8_t byte);
void _process_byte(uint8_t byte);
bool check_frame(uint32_t timestamp_us);
void skip_to_next_frame(uint32_t timestamp_us);
bool decode_crsf_packet();
bool process_telemetry(bool check_constraint = true);
void process_link_stats_frame(const void* data);
Expand All @@ -312,8 +314,6 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {

uint16_t _channels[CRSF_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */

void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; }

uint32_t _last_frame_time_us;
uint32_t _last_tx_frame_time_us;
uint32_t _last_uart_start_time_ms;
Expand Down
Loading
Loading