Implement RX bind for CRSF

libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp

@@ -680,6 +680,17 @@ int16_t AP_RCProtocol_CRSF::derive_scaled_lq_value(uint8_t uplink_lq)
     return int16_t(roundf(constrain_float(uplink_lq*2.5f,0,255)));
 }
 
+// start bind
+#if HAL_CRSF_TELEM_ENABLED
+void AP_RCProtocol_CRSF::start_bind(void)
+{
+    AP_CRSF_Telem* telem = AP::crsf_telem();
+    if (telem != nullptr) {
+        telem->start_bind();
+    }
+}
+#endif
+
 namespace AP {
     AP_RCProtocol_CRSF* crsf() {
         return AP_RCProtocol_CRSF::get_singleton();

libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include "AP_RCProtocol_config.h"
+#include <AP_RCTelemetry/AP_RCTelemetry_config.h>
 
 #if AP_RCPROTOCOL_CRSF_ENABLED
 
@@ -43,6 +44,9 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
     void process_byte(uint8_t byte, uint32_t baudrate) override;
     void process_handshake(uint32_t baudrate) override;
     void update(void) override;
+#if HAL_CRSF_TELEM_ENABLED
+    void start_bind(void) override;
+#endif
     // support for CRSF v3
     bool change_baud_rate(uint32_t baudrate);
     // bootstrap baudrate
@@ -149,6 +153,8 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
     // Commands for CRSF_COMMAND_RX
     enum CommandRX {
         CRSF_COMMAND_RX_BIND = 0x01,
+        CRSF_COMMAND_RX_CANCEL_BIND = 0x02,
+        CRSF_COMMAND_RX_SET_BIND_ID = 0x03,
     };
 
     // Commands for CRSF_COMMAND_GENERAL

libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp

@@ -405,7 +405,7 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
     case STATUS_TEXT:
         return rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY) && !queue_empty;
     case GENERAL_COMMAND:
-        return _baud_rate_request.pending;
+        return _baud_rate_request.pending || _bind_request_pending;
     case VERSION_PING:
         return _crsf_version.pending && AP::crsf()->is_detected(); // only send pings if protocol has been detected
     case HEARTBEAT:
@@ -461,7 +461,11 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
             calc_status_text();
             break;
         case GENERAL_COMMAND:
-            calc_command_response();
+            if (_bind_request_pending) {
+                calc_bind();
+            } else {
+                calc_command_response();
+            }
             break;
         case VERSION_PING:
             // to get crossfire firmware version we send an RX device ping
@@ -1039,6 +1043,31 @@ void AP_CRSF_Telem::calc_command_response() {
     _telem_pending = true;
 }
 
+// send a command response
+void AP_CRSF_Telem::calc_bind() {
+    _telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
+    _telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
+    _telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_RX;
+    _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_RX_BIND;
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND;
+
+    // calculate command crc
+    uint8_t len = 4;
+    uint8_t* crcptr = &_telem.ext.command.destination;
+    uint8_t crc = crc8_dvb(0, AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND, 0xBA);
+    for (uint8_t i = 0; i < len; i++) {
+        crc = crc8_dvb(crc, crcptr[i], 0xBA);
+    }
+    crcptr[len] = crc;
+    _telem_size = len + 1;
+
+    _pending_request.frame_type = 0;
+    _bind_request_pending = false;
+
+    debug("sent bind request");
+    _telem_pending = true;
+}
+
 // return parameter information
 void AP_CRSF_Telem::calc_parameter() {
 #if OSD_PARAM_ENABLED

libraries/AP_RCTelemetry/AP_CRSF_Telem.h

@@ -236,6 +236,8 @@ class AP_CRSF_Telem : public AP_RCTelemetry {
     void update();
     // get next telemetry data for external consumers of SPort data
     static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame, bool is_tx_active);
+    // start bind request
+    void start_bind() { _bind_request_pending = true; }
 
 private:
 
@@ -271,6 +273,7 @@ class AP_CRSF_Telem : public AP_RCTelemetry {
     void calc_device_info();
     void calc_device_ping(uint8_t destination);
     void calc_command_response();
+    void calc_bind();
     void calc_parameter();
 #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
     void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk);
@@ -350,6 +353,8 @@ class AP_CRSF_Telem : public AP_RCTelemetry {
         uint8_t port_id;
     } _baud_rate_request;
 
+    bool _bind_request_pending;
+
     // vtx state
     bool _vtx_freq_update;  // update using the frequency method or not
     bool _vtx_dbm_update; // update using the dbm method or not