From 69381f22b87bdc1056bcb8b2f4ecd08fd214d356 Mon Sep 17 00:00:00 2001 From: Paul Kendall Date: Sat, 27 Jan 2024 12:28:43 +1300 Subject: [PATCH] Support CRSF bind command from betaflight (#2523) * Support CRSF bind command from betaflight * Refactor to processInternalTelemetryPackage * Redundant call to setIsBound(false) --------- Co-authored-by: Bryan Mayland --- src/lib/CRSF/CRSF.cpp | 2 +- src/lib/CrsfProtocol/crsf_protocol.h | 6 ++- src/lib/Telemetry/telemetry.cpp | 61 ++++++++++++++++++++-------- src/lib/Telemetry/telemetry.h | 1 + src/src/rx_main.cpp | 2 +- 5 files changed, 50 insertions(+), 22 deletions(-) diff --git a/src/lib/CRSF/CRSF.cpp b/src/lib/CRSF/CRSF.cpp index 5e70b4a90c..1e12a17a5b 100644 --- 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 index a4ac5aa816..ca7b22525e 100644 --- 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 index 46d0b2eef6..b055fc68b8 100644 --- 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 index 94c800ecc2..c90d2376b2 100644 --- 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 index 11964f20dc..504f7fbaec 100644 --- 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