From cd6ed219f78c81f55b000828548b71c63096eda5 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sun, 10 Mar 2024 16:59:52 +0000
Subject: [PATCH] AP_RCTelemetry: add CRSF binding API

---
 libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp | 33 ++++++++++++++++++++--
 libraries/AP_RCTelemetry/AP_CRSF_Telem.h   |  5 ++++
 2 files changed, 36 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
index 311f584cf1..8fad7e5605 100644
--- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
+++ b/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
diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h
index 431ceebff7..e0d557cb74 100644
--- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h
+++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h
@@ -236,6 +236,8 @@ public:
     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 @@ private:
     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 @@ private:
         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