ArduPilot开源代码之AP_RCProtocol_CRSF

1. 源由

目前使用的是ELRS接收机，本章将就AP_RCProtocol_CRSF进行一些研读，针对这一独特接收机的处理方法。

2. 设计

重点关注方法：

• AP_RCProtocol_CRSF(AP_RCProtocol &_frontend);
• void process_handshake(uint32_t baudrate) override;
• void process_byte(uint8_t byte, uint32_t baudrate) override;
• void update(void) override;
• uint32_t get_bootstrap_baud_rate()

class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
public:
    AP_RCProtocol_CRSF(AP_RCProtocol &_frontend);
    virtual ~AP_RCProtocol_CRSF();
    void process_byte(uint8_t byte, uint32_t baudrate) override;
    void process_handshake(uint32_t baudrate) override;
    void update(void) override;
    // support for CRSF v3
    bool change_baud_rate(uint32_t baudrate);
    // bootstrap baudrate
    uint32_t get_bootstrap_baud_rate() const {
#if AP_RC_CHANNEL_ENABLED
        return rc().use_420kbaud_for_elrs() ? ELRS_BAUDRATE : CRSF_BAUDRATE;
#else
        return CRSF_BAUDRATE;
#endif
    }

    // is the receiver active, used to detect power loss and baudrate changes
    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;
    }

    // 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;
    }

    // get singleton instance
    static AP_RCProtocol_CRSF* get_singleton() {
        return _singleton;
    }

    enum FrameType {
        CRSF_FRAMETYPE_GPS = 0x02,
        CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
        CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
        CRSF_FRAMETYPE_VTX = 0x0F,
        CRSF_FRAMETYPE_VTX_TELEM = 0x10,
        CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
        CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
        CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED = 0x17,
        CRSF_FRAMETYPE_RC_CHANNELS_PACKED_11BIT = 0x18,
        CRSF_FRAMETYPE_LINK_STATISTICS_RX = 0x1C,
        CRSF_FRAMETYPE_LINK_STATISTICS_TX = 0x1D,
        CRSF_FRAMETYPE_ATTITUDE = 0x1E,
        CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
        // Extended Header Frames, range: 0x28 to 0x96
        CRSF_FRAMETYPE_PARAM_DEVICE_PING = 0x28,
        CRSF_FRAMETYPE_PARAM_DEVICE_INFO = 0x29,
        CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B,
        CRSF_FRAMETYPE_PARAMETER_READ = 0x2C,
        CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D,
        CRSF_FRAMETYPE_COMMAND = 0x32,
        // Custom Telemetry Frames 0x7F,0x80
        CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY = 0x7F,   // as suggested by Remo Masina for fw < 4.06
        CRSF_FRAMETYPE_AP_CUSTOM_TELEM = 0x80,          // reserved for ArduPilot by TBS, requires fw >= 4.06
    };

    // Command IDs for CRSF_FRAMETYPE_COMMAND
    enum CommandID {
        CRSF_COMMAND_FC = 0x01,
        CRSF_COMMAND_BLUETOOTH = 0x03,
        CRSF_COMMAND_OSD = 0x05,
        CRSF_COMMAND_VTX = 0x08,
        CRSF_COMMAND_LED = 0x09,
        CRSF_COMMAND_GENERAL = 0x0A,
        CRSF_COMMAND_RX = 0x10,
    };

    // Commands for CRSF_COMMAND_FC
    enum CommandFC {
        CRSF_COMMAND_FC_DISARM = 0x01,
        CRSF_COMMAND_SCALE_CHANNEL = 0x02,
    };

    // Commands for CRSF_COMMAND_BLUETOOTH
    enum CommandBluetooth {
        CRSF_COMMAND_BLUETOOTH_RESET = 0x01,
        CRSF_COMMAND_BLUETOOTH_ENABLE = 0x02,
        CRSF_COMMAND_BLUETOOTH_ECHO = 0x64,
    };

    // Commands for CRSF_COMMAND_OSD
    enum CommandOSD {
        CRSF_COMMAND_OSD_SEND = 0x01,
    };

    // Commands for CRSF_COMMAND_VTX
    enum CommandVTX {
        CRSF_COMMAND_VTX_CHANNEL = 0x01,
        CRSF_COMMAND_VTX_FREQ = 0x02,
        CRSF_COMMAND_VTX_POWER = 0x03,
        CRSF_COMMAND_VTX_PITMODE = 0x04,
        CRSF_COMMAND_VTX_PITMODE_POWERUP = 0x05,
        CRSF_COMMAND_VTX_POWER_DBM = 0x08,
    };

    // Commands for CRSF_COMMAND_LED
    enum CommandLED {
        CRSF_COMMAND_LED_SET_DEFAULT = 0x01,
        CRSF_COMMAND_LED_COLOR = 0x02,
        CRSF_COMMAND_LED_PULSE = 0x03,
        CRSF_COMMAND_LED_BLINK = 0x04,
        CRSF_COMMAND_LED_SHIFT = 0x05,
    };

    // Commands for CRSF_COMMAND_RX
    enum CommandRX {
        CRSF_COMMAND_RX_BIND = 0x01,
    };

    // Commands for CRSF_COMMAND_GENERAL
    enum CommandGeneral {
        CRSF_COMMAND_GENERAL_CHILD_DEVICE_REQUEST = 0x04,
        CRSF_COMMAND_GENERAL_CHILD_DEVICE_FRAME = 0x05,
        CRSF_COMMAND_GENERAL_FIRMWARE_UPDATE_BOOTLOADER = 0x0A,
        CRSF_COMMAND_GENERAL_FIRMWARE_UPDATE_ERASE = 0x0B,
        CRSF_COMMAND_GENERAL_WRITE_SERIAL_NUMBER = 0x13,
        CRSF_COMMAND_GENERAL_USER_ID = 0x15,
        CRSF_COMMAND_GENERAL_SOFTWARE_PRODUCT_KEY = 0x60,
        CRSF_COMMAND_GENERAL_CRSF_SPEED_PROPOSAL = 0x70,    // proposed new CRSF port speed
        CRSF_COMMAND_GENERAL_CRSF_SPEED_RESPONSE = 0x71,    // response to the proposed CRSF port speed
    };

    // SubType IDs for CRSF_FRAMETYPE_CUSTOM_TELEM
    enum CustomTelemSubTypeID : uint8_t {
        CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH = 0xF0,
        CRSF_AP_CUSTOM_TELEM_STATUS_TEXT = 0xF1,
        CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH = 0xF2,
    };

    enum DeviceAddress {
        CRSF_ADDRESS_BROADCAST = 0x00,
        CRSF_ADDRESS_USB = 0x10,
        CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80,
        CRSF_ADDRESS_RESERVED1 = 0x8A,
        CRSF_ADDRESS_PNP_PRO_CURRENT_SENSOR = 0xC0,
        CRSF_ADDRESS_PNP_PRO_GPS = 0xC2,
        CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
        CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8,
        CRSF_ADDRESS_RESERVED2 = 0xCA,
        CRSF_ADDRESS_RACE_TAG = 0xCC,
        CRSF_ADDRESS_VTX = 0xCE,
        CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
        CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
        CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
    };

    enum ExtendedFrameOffset {
        CRSF_EXTENDED_FRAME_LENGTH_OFFSET = 1,
        CRSF_EXTENDED_FRAME_TYPE_OFFSET = 2,
        CRSF_EXTENDED_FRAME_DESTINATION_OFFSET = 3,
        CRSF_EXTENDED_FRAME_ORIGIN_OFFSET = 4,
        CRSF_EXTENDED_FRAME_PAYLOAD_OFFSET = 5,
    };

    struct Frame {
        uint8_t device_address;
        uint8_t length;
        uint8_t type;
        uint8_t payload[CRSF_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for
    } PACKED;

    struct LinkStatisticsFrame {
        uint8_t uplink_rssi_ant1; // ( dBm * -1 )
        uint8_t uplink_rssi_ant2; // ( dBm * -1 )
        uint8_t uplink_status; // Package success rate / Link quality ( % )
        int8_t uplink_snr; // ( db )
        uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
        uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
        uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
        uint8_t downlink_rssi; // ( dBm * -1 )
        uint8_t downlink_status; // Downlink package success rate / Link quality ( % )
        int8_t downlink_dnr; // ( db )
    } PACKED;

    struct LinkStatisticsRXFrame {
        uint8_t rssi_db;        // RSSI(dBm*-1)
        uint8_t rssi_percent;   // RSSI in percent
        uint8_t link_quality;   // Package success rate / Link quality ( % )
        int8_t snr;             // SNR(dB)
        uint8_t rf_power_db;    // rf power in dBm
    } PACKED;

    struct LinkStatisticsTXFrame {
        uint8_t rssi_db;        // RSSI(dBm*-1)
        uint8_t rssi_percent;   // RSSI in percent
        uint8_t link_quality;   // Package success rate / Link quality ( % )
        int8_t snr;             // SNR(dB)
        uint8_t rf_power_db;    // rf power in dBm
        uint8_t fps;            // rf frames per second (fps / 10)
    } PACKED;

    struct SubsetChannelsFrame {
#if __BYTE_ORDER != __LITTLE_ENDIAN
#error "Only supported on little-endian architectures"
#endif
        uint8_t starting_channel:5;     // which channel number is the first one in the frame
        uint8_t res_configuration:2;    // configuration for the RC data resolution (10 - 13 bits)
        uint8_t digital_switch_flag:1;  // configuration bit for digital channel
        uint8_t channels[CRSF_FRAME_PAYLOAD_MAX - 2]; // payload less byte above
        // uint16_t channel[]:res;      // variable amount of channels (with variable resolution based
                                        // on the res_configuration) based on the frame size
        // uint16_t digital_switch_channel[]:10; // digital switch channel
    } PACKED;

    enum class ProtocolType {
        PROTOCOL_CRSF,
        PROTOCOL_TRACER,
        PROTOCOL_ELRS
    };

    enum RFMode {
        CRSF_RF_MODE_4HZ = 0,
        CRSF_RF_MODE_50HZ,
        CRSF_RF_MODE_150HZ,
        CRSF_RF_MODE_250HZ,
        ELRS_RF_MODE_4HZ,
        ELRS_RF_MODE_25HZ,
        ELRS_RF_MODE_50HZ,
        ELRS_RF_MODE_100HZ,
        ELRS_RF_MODE_150HZ,
        ELRS_RF_MODE_200HZ,
        ELRS_RF_MODE_250HZ,
        ELRS_RF_MODE_500HZ,
        RF_MODE_MAX_MODES,
        RF_MODE_UNKNOWN,
    };

    struct LinkStatus {
        int16_t rssi = -1;
        int16_t link_quality = -1;
        uint8_t rf_mode;
    };

    // this will be used by AP_CRSF_Telem to access link status data
    // from within AP_RCProtocol_CRSF thread so no need for cross-thread synch
    const volatile LinkStatus& get_link_status() const {
        return _link_status;
    }

    // return the link rate as defined by the LinkStatistics
    uint16_t get_link_rate(ProtocolType protocol) const;

    // return the protocol string
    const char* get_protocol_string(ProtocolType protocol) const;

private:
    struct Frame _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);
    bool decode_crsf_packet();
    bool process_telemetry(bool check_constraint = true);
    void process_link_stats_frame(const void* data);
    void process_link_stats_rx_frame(const void* data);
    void process_link_stats_tx_frame(const void* data);
    // crsf v3 decoding
    void decode_variable_bit_channels(const uint8_t* data, uint8_t frame_length, uint8_t nchannels, uint16_t *values);

    void write_frame(Frame* frame);
    void start_uart();
    AP_HAL::UARTDriver* get_current_UART() { return (_uart ? _uart : get_available_UART()); }

    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;
    uint32_t _last_rx_frame_time_us;
    uint32_t _start_frame_time_us;
    bool telem_available;
    uint32_t _new_baud_rate;
    bool _crsf_v3_active;

    bool _use_lq_for_rssi;
    int16_t derive_scaled_lq_value(uint8_t uplink_lq);

    volatile struct LinkStatus _link_status;

    static const uint16_t RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES];

    AP_HAL::UARTDriver *_uart;
};

3. 初始&运行

3.1 初始化

AP_Vehicle::setup
 └──> Copter::init_ardupilot
     └──> AP_BoardConfig::init
         └──> AP_BoardConfig::board_setup
             └──> hal.rcin->init
                 └──> RCInput::init
                     └──> AP_RCProtocol::init
                         └──> AP_RCProtocol_CRSF::AP_RCProtocol_CRSF

3.2 运行

main_loop
 └──> Scheduler::init
     └──> Scheduler::_rcin_thread
         └──> RCInput::_timer_tick
             └──> AP_RCProtocol::new_input
                 ├──> AP_RCProtocol::check_added_uart()
                 │   ├──> [uart checking] if not using uart, return
                 │   ├──> AP_RCProtocol::process_handshake  //backend[i]->process_handshake(baudrate)
                 │   └──> AP_RCProtocol::process_byte
                 │       ├──> backend[_detected_protocol]->process_byte(byte, baudrate)
                 │       └──> backend[_detected_protocol]->new_input()
                 └──> backend[i]->update()  //AP_RCProtocol_CRSF::update

4. 重要方法

4.1 uint32_t get_bootstrap_baud_rate()

CRSF协议针对具体的协议其波特率有差异。

get_bootstrap_baud_rate
 └──> return rc().use_420kbaud_for_elrs() ? ELRS_BAUDRATE : CRSF_BAUDRATE

#define CRSF_BAUDRATE      416666U
#define ELRS_BAUDRATE      420000U

4.2 AP_RCProtocol_CRSF(AP_RCProtocol &_frontend)

根据具体设置协议进行串口初始化。

AP_RCProtocol_CRSF
 ├──> _uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_CRSF, 0)
 └──> <_uart>
     └──> start_uart()

以下是串口初始化方法。

void AP_RCProtocol_CRSF::start_uart()
{
    _uart->configure_parity(0);
    _uart->set_stop_bits(1);
    _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
    _uart->set_blocking_writes(false);
    _uart->set_options(_uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV);
    _uart->begin(get_bootstrap_baud_rate());
}

4.3 void process_handshake(uint32_t baudrate)

注：默认配置115，这里对于CRSF协议来说，相当于无需处理。详见：ArduPilot之H743+BMI270x2+First Normal Takeoff

process_handshake
{
    AP_HAL::UARTDriver *uart = get_current_UART();

    // only change the baudrate if we are bootstrapping CRSF
    if (uart == nullptr
        || baudrate != CRSF_BAUDRATE
        || baudrate == get_bootstrap_baud_rate()
        || uart->get_baud_rate() == get_bootstrap_baud_rate()
        || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::CRSF)+1))+1)) == 0) {
        return;
    }

    uart->begin(get_bootstrap_baud_rate());
}

4.4 void process_byte(uint8_t byte, uint32_t baudrate) override

通过串口字节流以一包数据长度为粒度进行报文整理。

从代码设计上可以看出：

1. 该协议有一个timeout check来同步报文；
2. 使用了CRC以及超长len检查，丢弃非法报文；

注：因为timeout是以10ms时间间隔，因此如果报文密度非常高（小于10ms）；且代码中存在非法报文；此时就体现SOF的重要性。

process_byte
 ├──> <baudrate != CRSF_BAUDRATE || _uart> return   // reject RC data if we have been configured for standalone mode
 └──> _process_byte(AP_HAL::micros(), byte)


_process_byte
 ├──> [timeout check]
 ├──> [overflow check]
 ├──> <_frame_ofs < CSRF_HEADER_TYPE_LEN> add_to_buffer; return
 ├──> <_frame_ofs == CSRF_HEADER_TYPE_LEN> _frame_crc = crc8_dvb_s2(0, _frame.type)
 ├──> <_frame_ofs < _frame.length + CSRF_HEADER_LEN> _frame_crc = crc8_dvb_s2(_frame_crc, byte)
 └──> <_frame_ofs == _frame.length + CSRF_HEADER_LEN>
     ├──> <_frame_crc != _frame.payload[_frame.length - 2]> return // CRC check
     └──> <decode_crsf_packet()> add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality)

4.5 void update(void) override

通过串口字节流以一包数据长度为粒度进行报文整理。

CRSF提供一个电传报文反馈途径，且间隔CRSF_TX_TIMEOUT(500ms)。

update
 ├──> <_uart>
 │   ├──> <now - _last_uart_start_time_ms > 1000U && _last_frame_time_us == 0> start_uart()  //for some reason it's necessary to keep trying to start the uart until we get data
 │   ├──> int16_t b = _uart->read()  //read _uart->available() data
 │   └──> process_byte(AP_HAL::micros(), uint8_t(b))
 └──> <_last_frame_time_us > 0 && (!get_rc_frame_count() || !is_tx_active()> //never received RC frames, but have received CRSF frames so make sure we give the telemetry opportunity to run
     └──> process_telemetry(false)

5. CRSF协议

5.1 CRSF协议特点

• 最大长度不超过CRSF_FRAMELEN_MAX(64)字节
• 字节头长度CSRF_HEADER_LEN(2)字节
• 最大报文长度不超过CRSF_FRAME_PAYLOAD_MAX(62)字节
• Payload最后两个字节CRC

5.2 CRSF协议格式

struct Frame {
    uint8_t device_address;
    uint8_t length;
    uint8_t type;
    uint8_t payload[CRSF_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for
} PACKED;

5.3 bool AP_RCProtocol_CRSF::decode_crsf_packet()

协议解析支持以下报文

• CRSF_FRAMETYPE_RC_CHANNELS_PACKED
• CRSF_FRAMETYPE_LINK_STATISTICS
• CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED
• CRSF_FRAMETYPE_LINK_STATISTICS_RX
• CRSF_FRAMETYPE_LINK_STATISTICS_TX

decode_crsf_packet
 ├──> [_frame.type]
 │   ├──> <case CRSF_FRAMETYPE_RC_CHANNELS_PACKED>
 │   │   └──> decode_11bit_channels((const uint8_t*)(&_frame.payload), CRSF_MAX_CHANNELS, _channels, 5U, 8U, 880U);
 │   ├──> <case CRSF_FRAMETYPE_LINK_STATISTICS>
 │   │   └──> process_link_stats_frame((uint8_t*)&_frame.payload);
 │   ├──> <case CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED>
 │   │   └──> decode_variable_bit_channels((const uint8_t*)(&_frame.payload), _frame.length, CRSF_MAX_CHANNELS, _channels);
 │   ├──> <case CRSF_FRAMETYPE_LINK_STATISTICS_RX>
 │   │   └──> process_link_stats_rx_frame((uint8_t*)&_frame.payload);
 │   └──> <case CRSF_FRAMETYPE_LINK_STATISTICS_TX>
 │       └──> process_link_stats_tx_frame((uint8_t*)&_frame.payload);
 └──> <HAL_CRSF_TELEM_ENABLED>
     ├──> <AP_CRSF_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload)>
     │   └──> process_telemetry()
     └──> [_new_baud_rate > 0]
         └──> [wait for all the pending data to be sent, change the baud rate]

5.4 CRSF_FRAMETYPE_RC_CHANNELS_PACKED

/*
  decode channels from the standard 11bit format (used by CRSF, SBUS, FPort and FPort2)
  must be used on multiples of 8 channels
*/
void AP_RCProtocol_CRSF::decode_variable_bit_channels(const uint8_t* payload, uint8_t frame_length, uint8_t nchannels, uint16_t *values)
{
    const SubsetChannelsFrame* channel_data = (const SubsetChannelsFrame*)payload;

    // get the channel resolution settings
    uint8_t channelBits;
    uint16_t channelMask;
    float channelScale;

    switch (channel_data->res_configuration) {
    case CRSF_SUBSET_RC_RES_CONF_10B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_10B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_10B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_10B;
        break;
    default:
    case CRSF_SUBSET_RC_RES_CONF_11B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_11B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_11B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_11B;
        break;
    case CRSF_SUBSET_RC_RES_CONF_12B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_12B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_12B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_12B;
        break;
    case CRSF_SUBSET_RC_RES_CONF_13B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_13B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_13B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_13B;
        break;
    }

    // calculate the number of channels packed
    uint8_t numOfChannels = MIN(uint8_t(((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits), CRSF_MAX_CHANNELS);

    // unpack the channel data
    uint8_t bitsMerged = 0;
    uint32_t readValue = 0;
    uint8_t readByteIndex = 1;

    for (uint8_t n = 0; n < numOfChannels; n++) {
        while (bitsMerged < channelBits) {
            // check for corrupt frame
            if (readByteIndex >= CRSF_FRAME_PAYLOAD_MAX) {
                return;
            }
            uint8_t readByte = payload[readByteIndex++];
            readValue |= ((uint32_t) readByte) << bitsMerged;
            bitsMerged += 8;
        }
        // check for corrupt frame
        if (uint8_t(channel_data->starting_channel + n) >= CRSF_MAX_CHANNELS) {
            return;
        }
        _channels[channel_data->starting_channel + n] =
            uint16_t(channelScale * float(uint16_t(readValue & channelMask)) + 988);
        readValue >>= channelBits;
        bitsMerged -= channelBits;
    }
}

5.5 CRSF_FRAMETYPE_LINK_STATISTICS

// process link statistics to get RSSI
void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
{
    const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data;

    uint8_t rssi_dbm;
    if (link->active_antenna == 0) {
        rssi_dbm = link->uplink_rssi_ant1;
    } else {
        rssi_dbm = link->uplink_rssi_ant2;
    }
    _link_status.link_quality = link->uplink_status;
    if (_use_lq_for_rssi) {
        _link_status.rssi = derive_scaled_lq_value(link->uplink_status);
    } else{
        // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
        if (rssi_dbm < 50) {
            _link_status.rssi = 255;
        } else if (rssi_dbm > 120) {
            _link_status.rssi = 0;
        } else {
            // this is an approximation recommended by Remo from TBS
            _link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
        }
    }

    _link_status.rf_mode = MIN(link->rf_mode, 7U);
}

5.6 CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED

/*
  decode channels from the standard 11bit format (used by CRSF, SBUS, FPort and FPort2)
  must be used on multiples of 8 channels
*/
void AP_RCProtocol_CRSF::decode_variable_bit_channels(const uint8_t* payload, uint8_t frame_length, uint8_t nchannels, uint16_t *values)
{
    const SubsetChannelsFrame* channel_data = (const SubsetChannelsFrame*)payload;

    // get the channel resolution settings
    uint8_t channelBits;
    uint16_t channelMask;
    float channelScale;

    switch (channel_data->res_configuration) {
    case CRSF_SUBSET_RC_RES_CONF_10B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_10B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_10B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_10B;
        break;
    default:
    case CRSF_SUBSET_RC_RES_CONF_11B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_11B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_11B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_11B;
        break;
    case CRSF_SUBSET_RC_RES_CONF_12B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_12B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_12B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_12B;
        break;
    case CRSF_SUBSET_RC_RES_CONF_13B:
        channelBits = CRSF_SUBSET_RC_RES_BITS_13B;
        channelMask = CRSF_SUBSET_RC_RES_MASK_13B;
        channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_13B;
        break;
    }

    // calculate the number of channels packed
    uint8_t numOfChannels = MIN(uint8_t(((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits), CRSF_MAX_CHANNELS);

    // unpack the channel data
    uint8_t bitsMerged = 0;
    uint32_t readValue = 0;
    uint8_t readByteIndex = 1;

    for (uint8_t n = 0; n < numOfChannels; n++) {
        while (bitsMerged < channelBits) {
            // check for corrupt frame
            if (readByteIndex >= CRSF_FRAME_PAYLOAD_MAX) {
                return;
            }
            uint8_t readByte = payload[readByteIndex++];
            readValue |= ((uint32_t) readByte) << bitsMerged;
            bitsMerged += 8;
        }
        // check for corrupt frame
        if (uint8_t(channel_data->starting_channel + n) >= CRSF_MAX_CHANNELS) {
            return;
        }
        _channels[channel_data->starting_channel + n] =
            uint16_t(channelScale * float(uint16_t(readValue & channelMask)) + 988);
        readValue >>= channelBits;
        bitsMerged -= channelBits;
    }
}

5.7 CRSF_FRAMETYPE_LINK_STATISTICS_RX

// process link statistics to get RX RSSI
void AP_RCProtocol_CRSF::process_link_stats_rx_frame(const void* data)
{
    const LinkStatisticsRXFrame* link = (const LinkStatisticsRXFrame*)data;

    if (_use_lq_for_rssi) {
        _link_status.rssi = derive_scaled_lq_value(link->link_quality);
    } else {
        _link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
    }
}

5.8 CRSF_FRAMETYPE_LINK_STATISTICS_TX

// process link statistics to get TX RSSI
void AP_RCProtocol_CRSF::process_link_stats_tx_frame(const void* data)
{
    const LinkStatisticsTXFrame* link = (const LinkStatisticsTXFrame*)data;

    if (_use_lq_for_rssi) {
        _link_status.rssi = derive_scaled_lq_value(link->link_quality);
    } else {
        _link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
    }
}

6. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot飞控之ubuntu22.04-SITL安装
【4】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【5】ArduPilot飞控之Mission Planner模拟
【6】ArduPilot飞控AOCODARC-H7DUAL固件编译
【7】ArduPilot之开源代码Library&Sketches设计
【8】ArduPilot之开源代码Sensor Drivers设计
【9】ArduPilot之开源代码基础知识&Threading概念
【10】ArduPilot之开源代码UARTs and the Console使用
【11】ArduPilot飞控启动&运行过程简介
【11】ArduPilot之开源代码Task介绍
【12】ArduPilot开源代码之AP_Param
【13】ArduPilot开源代码之AP_Scheduler
【14】ArduPilot开源代码之AP_VideoTX
【15】ArduPilot开源代码之AP_InertialSensor_Backend
【16】ArduPilot开源代码之AP_InertialSensor
【17】ArduPilot开源代码之AP_Logger
【18】ArduPilot开源代码之AP_GPS
【19】ArduPilot开源代码之RCInput