Support CRSF bind command from betaflight (ExpressLRS#2523)

* Support CRSF bind command from betaflight * Refactor to processInternalTelemetryPackage * Redundant call to setIsBound(false) --------- Co-authored-by: Bryan Mayland <[email protected]>
ot0tot · Jan 26, 2024 · 69381f2 · 69381f2
1 parent 758b97b
commit 69381f2
Show file tree

Hide file tree

Showing 5 changed files with 50 additions and 22 deletions.
diff --git a/src/lib/CRSF/CRSF.cpp b/src/lib/CRSF/CRSF.cpp
@@ -411,7 +411,7 @@ bool ICACHE_RAM_ATTR CRSF::ProcessPacket()
     {
         elrsLUAmode = SerialInBuffer[4] == CRSF_ADDRESS_ELRS_LUA;
 
-        if (packetType == CRSF_FRAMETYPE_COMMAND && SerialInBuffer[5] == SUBCOMMAND_CRSF && SerialInBuffer[6] == COMMAND_MODEL_SELECT_ID)
+        if (packetType == CRSF_FRAMETYPE_COMMAND && SerialInBuffer[5] == CRSF_COMMAND_SUBCMD_RX && SerialInBuffer[6] == CRSF_COMMAND_MODEL_SELECT_ID)
         {
             modelId = SerialInBuffer[7];
             #if defined(PLATFORM_ESP32)

diff --git a/src/lib/CrsfProtocol/crsf_protocol.h b/src/lib/CrsfProtocol/crsf_protocol.h
@@ -92,11 +92,12 @@ typedef enum
 } crsf_frame_type_e;
 
 typedef enum {
-    SUBCOMMAND_CRSF = 0x10
+    CRSF_COMMAND_SUBCMD_RX = 0x10
 } crsf_command_e;
 
 typedef enum {
-    COMMAND_MODEL_SELECT_ID = 0x05
+    CRSF_COMMAND_SUBCMD_RX_BIND = 0x01,
+    CRSF_COMMAND_MODEL_SELECT_ID = 0x05
 } crsf_subcommand_e;
 
 enum {
@@ -183,6 +184,7 @@ typedef struct crsf_ext_header_s
     // Extended fields
     uint8_t dest_addr;
     uint8_t orig_addr;
+    uint8_t payload[0];
 } PACKED crsf_ext_header_t;
 
 /**

diff --git a/src/lib/Telemetry/telemetry.cpp b/src/lib/Telemetry/telemetry.cpp
@@ -231,36 +231,61 @@ bool Telemetry::RXhandleUARTin(uint8_t data)
     return true;
 }
 
-bool Telemetry::AppendTelemetryPackage(uint8_t *package)
+/**
+ * @brief: Check the CRSF frame for commands that should not be passed on
+ * @return: true if packet was internal and should not be processed further
+*/
+bool Telemetry::processInternalTelemetryPackage(uint8_t *package)
 {
-    const crsf_header_t *header = (crsf_header_t *) package;
+    const crsf_ext_header_t *header = (crsf_ext_header_t *)package;
 
-    if (header->type == CRSF_FRAMETYPE_COMMAND && package[3] == 'b' && package[4] == 'l')
-    {
-        callBootloader = true;
-        return true;
-    }
-    if (header->type == CRSF_FRAMETYPE_COMMAND && package[3] == 'b' && package[4] == 'd')
-    {
-        callEnterBind = true;
-        return true;
-    }
-    if (header->type == CRSF_FRAMETYPE_COMMAND && package[3] == 'm' && package[4] == 'm')
+    if (header->type == CRSF_FRAMETYPE_COMMAND)
     {
-        callUpdateModelMatch = true;
-        modelMatchId = package[5];
-        return true;
+        // Non CRSF, dest=b src=l -> reboot to bootloader
+        if (package[3] == 'b' && package[4] == 'l')
+        {
+            callBootloader = true;
+            return true;
+        }
+        // 1. Non CRSF, dest=b src=b -> bind mode
+        // 2. CRSF bind command
+        if ((package[3] == 'b' && package[4] == 'd') ||
+            (header->frame_size >= 6 // official CRSF is 7 bytes with two CRCs
+            && header->dest_addr == CRSF_ADDRESS_CRSF_RECEIVER
+            && header->orig_addr == CRSF_ADDRESS_FLIGHT_CONTROLLER
+            && header->payload[0] == CRSF_COMMAND_SUBCMD_RX
+            && header->payload[1] == CRSF_COMMAND_SUBCMD_RX_BIND))
+        {
+            callEnterBind = true;
+            return true;
+        }
+        // Non CRSF, dest=b src=m -> set modelmatch
+        if (package[3] == 'm' && package[4] == 'm')
+        {
+            callUpdateModelMatch = true;
+            modelMatchId = package[5];
+            return true;
+        }
     }
-    if (header->type == CRSF_FRAMETYPE_DEVICE_PING && package[CRSF_TELEMETRY_TYPE_INDEX + 1] == CRSF_ADDRESS_CRSF_RECEIVER)
+
+    if (header->type == CRSF_FRAMETYPE_DEVICE_PING && header->dest_addr == CRSF_ADDRESS_CRSF_RECEIVER)
     {
         sendDeviceFrame = true;
         return true;
     }
 
+    return false;
+}
+
+bool Telemetry::AppendTelemetryPackage(uint8_t *package)
+{
+    if (processInternalTelemetryPackage(package))
+        return true;
+
+    const crsf_header_t *header = (crsf_header_t *) package;
     uint8_t targetIndex = 0;
     bool targetFound = false;
 
-
     if (header->type >= CRSF_FRAMETYPE_DEVICE_PING)
     {
         const crsf_ext_header_t *extHeader = (crsf_ext_header_t *) package;

diff --git a/src/lib/Telemetry/telemetry.h b/src/lib/Telemetry/telemetry.h
@@ -69,6 +69,7 @@ class Telemetry
     uint8_t ReceivedPackagesCount();
     bool AppendTelemetryPackage(uint8_t *package);
 private:
+    bool processInternalTelemetryPackage(uint8_t *package);
     void AppendToPackage(volatile crsf_telemetry_package_t *current);
     uint8_t CRSFinBuffer[CRSF_MAX_PACKET_LEN];
     telemetry_state_s telemetry_state;

diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp
@@ -1541,7 +1541,6 @@ static void updateBindingMode(unsigned long now)
         config.Commit();
 
         DBGLN("Power on counter >=3, enter binding mode...");
-        config.SetIsBound(false);
         EnterBindingMode();
     }
 }
@@ -1917,6 +1916,7 @@ void EnterBindingMode()
     UID[5] = BindingUID[5];
 
     OtaCrcInitializer = 0;
+    config.SetIsBound(false);
     InBindingMode = true;
 
     // Start attempting to bind