diff --git a/src/lib/Backpack/devBackpack.cpp b/src/lib/Backpack/devBackpack.cpp index d1c7771b9f..f4753fb39e 100644 --- a/src/lib/Backpack/devBackpack.cpp +++ b/src/lib/Backpack/devBackpack.cpp @@ -7,6 +7,7 @@ #include "CRSFHandset.h" #include "config.h" #include "logging.h" +#include "MAVLink.h" #define BACKPACK_TIMEOUT 20 // How often to check for backpack commands @@ -18,6 +19,7 @@ bool VRxBackpackWiFiReadyToSend = false; bool HTEnableFlagReadyToSend = false; bool lastRecordingState = false; +uint8_t lastLinkMode; // will get set in start() and used in event() #if defined(GPIO_PIN_BACKPACK_EN) @@ -304,6 +306,7 @@ static void initialize() static int start() { + lastLinkMode = config.GetLinkMode(); if (OPT_USE_TX_BACKPACK) { return DURATION_IMMEDIATELY; @@ -362,6 +365,17 @@ static int event() // EN should be HIGH to be active digitalWrite(GPIO_PIN_BACKPACK_EN, config.GetBackpackDisable() ? LOW : HIGH); } +#endif +#if !defined(PLATFORM_STM32) + // Update the backpack operating mode when the link mode changes + uint8_t newMode = config.GetLinkMode(); + if (lastLinkMode != newMode) + { + uint8_t mavlinkOutputBuffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t len = buildMAVLinkELRSModeChange(newMode, mavlinkOutputBuffer); + TxBackpack->write(mavlinkOutputBuffer, len); + } + lastLinkMode = config.GetLinkMode(); #endif return DURATION_IGNORE; } diff --git a/src/lib/MAVLink/MAVLink.cpp b/src/lib/MAVLink/MAVLink.cpp index 6bfbb70eaa..d576a200f5 100644 --- a/src/lib/MAVLink/MAVLink.cpp +++ b/src/lib/MAVLink/MAVLink.cpp @@ -130,3 +130,22 @@ bool isThisAMavPacket(uint8_t *buffer, uint16_t bufferSize) #endif return false; } + +uint16_t buildMAVLinkELRSModeChange(uint8_t mode, uint8_t *buffer) +{ +#if !defined(PLATFORM_STM32) + constexpr uint8_t ELRS_MODE_CHANGE = 0x8; + mavlink_command_int_t commandMsg; + commandMsg.target_system = 255; + commandMsg.target_component = MAV_COMP_ID_UDP_BRIDGE; + commandMsg.command = MAV_CMD_USER_1; + commandMsg.param1 = ELRS_MODE_CHANGE; + commandMsg.param2 = mode; + mavlink_message_t msg; + mavlink_msg_command_int_encode(255, MAV_COMP_ID_TELEMETRY_RADIO, &msg, &commandMsg); + uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); + return len; +#else + return 0; +#endif +} \ No newline at end of file diff --git a/src/lib/MAVLink/MAVLink.h b/src/lib/MAVLink/MAVLink.h index 335ba36c1c..577fbecd58 100644 --- a/src/lib/MAVLink/MAVLink.h +++ b/src/lib/MAVLink/MAVLink.h @@ -9,3 +9,4 @@ void convert_mavlink_to_crsf_telem(uint8_t *CRSFinBuffer, uint8_t count, Handset *handset); bool isThisAMavPacket(uint8_t *buffer, uint16_t bufferSize); +uint16_t buildMAVLinkELRSModeChange(uint8_t mode, uint8_t *buffer); \ No newline at end of file