Allow Backpack in MAVLink mode to come back to Normal mode automatica…

…lly (ExpressLRS#2867)

* Allow Backpack in MAVLink mode to come back to Normal mode automatically

* Fix build for STM32

* Fix build for ESP8285 TX

* Actually let's use a COMMAND_INT message rather than TUNNEL

* Update magic number name

* Make the TX request the Backpack to move to MAVLink mode immediately

* Remove unnecessary devices notify line

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;
 }

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
+}

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);