Skip to content

Commit

Permalink
Merge branch 'master' into master
Browse files Browse the repository at this point in the history
  • Loading branch information
@laozhou-fujian
laozhou-fujian authored Feb 22, 2024
2 parents 7cc9755 + b19f8ed commit da5d14e
Show file tree
Hide file tree
Showing 3 changed files with 230 additions and 59 deletions.
144 changes: 95 additions & 49 deletions libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View file
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));
_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
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

0 comments on commit da5d14e

Please sign in to comment.