Extend Backpack telemetry forwarding options (ExpressLRS#2885)

* Extend tlm forwarding option for backpack

src/lib/Backpack/devBackpack.cpp

@@ -17,9 +17,9 @@ extern bool headTrackingEnabled;
 bool TxBackpackWiFiReadyToSend = false;
 bool VRxBackpackWiFiReadyToSend = false;
 bool HTEnableFlagReadyToSend = false;
+bool BackpackTelemReadyToSend = false;
 
 bool lastRecordingState = false;
-uint8_t lastLinkMode; // will get set in start() and used in event()
 
 #if defined(GPIO_PIN_BACKPACK_EN)
 
@@ -258,14 +258,20 @@ static void AuxStateToMSPOut()
 #endif // USE_TX_BACKPACK
 }
 
-void crsfTelemToMSPOut(uint8_t *data)
+void sendCRSFTelemetryToBackpack(uint8_t *data)
 {
-    if (config.GetBackpackTlmEnabled() == 0)
+    if (config.GetBackpackTlmMode() == BACKPACK_TELEM_MODE_OFF)
     {
         // Backpack telem is off
         return;
     }
 
+    if (config.GetLinkMode() == TX_MAVLINK_MODE)
+    {
+        // Tx is in MAVLink mode, don't forward CRSF telemetry
+        return;
+    }
+
     mspPacket_t packet;
     packet.reset();
     packet.makeCommand();
@@ -286,6 +292,30 @@ void crsfTelemToMSPOut(uint8_t *data)
     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 }
 
+void sendMAVLinkTelemetryToBackpack(uint8_t *data)
+{
+    if (config.GetBackpackTlmMode() == BACKPACK_TELEM_MODE_OFF)
+    {
+        // Backpack telem is off
+        return;
+    }
+
+    uint8_t count = data[1];
+    TxBackpack->write(data + CRSF_FRAME_NOT_COUNTED_BYTES, count);
+}
+
+void sendConfigToBackpack()
+{
+    // Send any config values to the tx-backpack, as one key/value pair per MSP msg
+    mspPacket_t packet;
+    packet.reset();
+    packet.makeCommand();
+    packet.function = MSP_ELRS_BACKPACK_CONFIG;
+    packet.addByte(MSP_ELRS_BACKPACK_CONFIG_TLM_MODE); // Backpack tlm mode
+    packet.addByte(config.GetBackpackTlmMode());
+    MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
+}
+
 static void initialize()
 {
 #if defined(GPIO_PIN_BACKPACK_EN)
@@ -306,7 +336,6 @@ static void initialize()
 
 static int start()
 {
-    lastLinkMode = config.GetLinkMode();
     if (OPT_USE_TX_BACKPACK)
     {
         return DURATION_IMMEDIATELY;
@@ -354,6 +383,12 @@ static int timeout()
         BackpackHTFlagToMSPOut(headTrackingEnabled);
     }
 
+    if (BackpackTelemReadyToSend && connectionState < MODE_STATES)
+    {
+        BackpackTelemReadyToSend = false;
+        sendConfigToBackpack();
+    }
+
     return BACKPACK_TIMEOUT;
 }
 
@@ -366,17 +401,7 @@ static int event()
         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/Backpack/devBackpack.h

@@ -3,7 +3,10 @@
 #include "device.h"
 
 void checkBackpackUpdate();
-void crsfTelemToMSPOut(uint8_t *data);
+void sendCRSFTelemetryToBackpack(uint8_t *data);
+void sendMAVLinkTelemetryToBackpack(uint8_t *data);
+
 extern bool HTEnableFlagReadyToSend;
+extern bool BackpackTelemReadyToSend;
 
 extern device_t Backpack_device;

src/lib/CONFIG/config.cpp

@@ -180,7 +180,7 @@ void TxConfig::Load()
         if (nvs_get_u8(handle, "backpackdisable", &value8) == ESP_OK)
             m_config.backpackDisable = value8;
         if (nvs_get_u8(handle, "backpacktlmen", &value8) == ESP_OK)
-            m_config.backpackTlmEnabled = value8;
+            m_config.backpackTlmMode = value8;
     }
 
     for(unsigned i=0; i<CONFIG_TX_MODEL_CNT; i++)
@@ -339,7 +339,7 @@ TxConfig::Commit()
         nvs_set_u8(handle, "fanthresh", m_config.powerFanThreshold);
 
         nvs_set_u8(handle, "backpackdisable", m_config.backpackDisable);
-        nvs_set_u8(handle, "backpacktlmen", m_config.backpackTlmEnabled);
+        nvs_set_u8(handle, "backpacktlmen", m_config.backpackTlmMode);
         nvs_set_u8(handle, "dvraux", m_config.dvrAux);
         nvs_set_u8(handle, "dvrstartdelay", m_config.dvrStartDelay);
         nvs_set_u8(handle, "dvrstopdelay", m_config.dvrStopDelay);
@@ -441,7 +441,6 @@ TxConfig::SetLinkMode(uint8_t linkMode)
         {
             m_model->tlm = TLM_RATIO_1_2;
             m_model->switchMode = smHybridOr16ch; // Force Hybrid / 16ch/2 switch modes for mavlink
-            m_config.backpackTlmEnabled = false; // Disable backpack telemetry since it'd be MSP mixed with MAVLink
         }
         m_modified |= MODEL_CHANGED | MAIN_CHANGED;
     }
@@ -577,11 +576,11 @@ TxConfig::SetBackpackDisable(bool backpackDisable)
 }
 
 void
-TxConfig::SetBackpackTlmEnabled(bool enabled)
+TxConfig::SetBackpackTlmMode(uint8_t mode)
 {
-    if (m_config.backpackTlmEnabled != enabled)
+    if (m_config.backpackTlmMode != mode)
     {
-        m_config.backpackTlmEnabled = enabled;
+        m_config.backpackTlmMode = mode;
         m_modified |= MAIN_CHANGED;
     }
 }

src/lib/CONFIG/config.h

@@ -73,6 +73,13 @@ typedef union {
     uint32_t raw;
 } tx_button_color_t;
 
+typedef enum {
+    BACKPACK_TELEM_MODE_OFF,
+    BACKPACK_TELEM_MODE_ESPNOW,
+    BACKPACK_TELEM_MODE_WIFI,
+    BACKPACK_TELEM_MODE_BLUETOOTH,
+} telem_mode_t;
+
 typedef struct {
     uint32_t        version;
     uint8_t         vtxBand;    // 0=Off, else band number
@@ -85,8 +92,7 @@ typedef struct {
     uint8_t         motionMode:2,       // bool, but space for 2 more modes
                     dvrStopDelay:3,
                     backpackDisable:1,  // bool, disable backpack via EN pin if available
-                    backpackTlmEnabled:1,  // bool, enable telemetry via ESPNOW from backpack
-                    unused:1;          // FUTURE available
+                    backpackTlmMode:2;  // 0=Off, 1=Fwd tlm via espnow, 2=fwd tlm via wifi 3=(FUTURE) bluetooth
     uint8_t         dvrStartDelay:3,
                     dvrAux:5;
     tx_button_color_t buttonColors[2];  // FUTURE: TX RGB color / mode (sets color of TX, can be a static color or standard)
@@ -123,7 +129,7 @@ class TxConfig
     uint8_t  GetDvrStartDelay() const { return m_config.dvrStartDelay; }
     uint8_t  GetDvrStopDelay() const { return m_config.dvrStopDelay; }
     bool     GetBackpackDisable() const { return m_config.backpackDisable; }
-    bool     GetBackpackTlmEnabled() const { return m_config.backpackTlmEnabled; }
+    uint8_t  GetBackpackTlmMode() const { return m_config.backpackTlmMode; }
     tx_button_color_t const *GetButtonActions(uint8_t button) const { return &m_config.buttonColors[button]; }
     model_config_t const &GetModelConfig(uint8_t model) const { return m_config.model_config[model]; }
     uint8_t GetPTRStartChannel() const { return m_model->ptrStartChannel; }
@@ -153,7 +159,7 @@ class TxConfig
     void SetDvrStopDelay(uint8_t dvrStopDelay);
     void SetButtonActions(uint8_t button, tx_button_color_t actions[2]);
     void SetBackpackDisable(bool backpackDisable);
-    void SetBackpackTlmEnabled(bool enabled);
+    void SetBackpackTlmMode(uint8_t mode);
     void SetPTRStartChannel(uint8_t ptrStartChannel);
     void SetPTREnableChannel(uint8_t ptrEnableChannel);
 

src/lib/LUA/tx_devLUA.cpp

@@ -289,7 +289,7 @@ static struct luaItem_selection luaHeadTrackingStartChannel = {
 static struct luaItem_selection luaBackpackTelemetry = {
     {"Telemetry", CRSF_TEXT_SELECTION},
     0, // value
-    luastrOffOn,
+    "Off;ESPNOW;WiFi",
     STR_EMPTYSPACE};
 
 static struct luaItem_string luaBackpackVersion = {
@@ -307,6 +307,7 @@ extern void ResetPower();
 extern uint8_t adjustPacketRateForBaud(uint8_t rate);
 extern void SetSyncSpam();
 extern bool RxWiFiReadyToSend;
+extern bool BackpackTelemReadyToSend;
 #if defined(USE_TX_BACKPACK)
 extern bool TxBackpackWiFiReadyToSend;
 extern bool VRxBackpackWiFiReadyToSend;
@@ -832,7 +833,8 @@ static void registerLuaParameters()
           luaBackpackFolder.common.id);
       registerLUAParameter(
             &luaBackpackTelemetry, [](luaPropertiesCommon *item, uint8_t arg) {
-                config.SetBackpackTlmEnabled(arg);
+                config.SetBackpackTlmMode(arg);
+                BackpackTelemReadyToSend = true;
             }, luaBackpackFolder.common.id);
 
       registerLUAParameter(&luaBackpackVersion, nullptr, luaBackpackFolder.common.id);
@@ -915,7 +917,7 @@ static int event()
     setLuaTextSelectionValue(&luaDvrStopDelay, config.GetBackpackDisable() ? 0 : config.GetDvrStopDelay());
     setLuaTextSelectionValue(&luaHeadTrackingEnableChannel, config.GetBackpackDisable() ? 0 : config.GetPTREnableChannel());
     setLuaTextSelectionValue(&luaHeadTrackingStartChannel, config.GetBackpackDisable() ? 0 : config.GetPTRStartChannel());
-    setLuaTextSelectionValue(&luaBackpackTelemetry, config.GetBackpackTlmEnabled() ? 1 : 0);
+    setLuaTextSelectionValue(&luaBackpackTelemetry, config.GetBackpackDisable() ? 0 : config.GetBackpackTlmMode());
     setLuaStringValue(&luaBackpackVersion, backpackVersion);
   }
 #if defined(TARGET_TX_FM30)

src/lib/MSP/msptypes.h

@@ -33,6 +33,9 @@
 
 #define MSP_ELRS_MAVLINK_TLM                0xFD
 
+#define MSP_ELRS_BACKPACK_CONFIG            0x30
+#define MSP_ELRS_BACKPACK_CONFIG_TLM_MODE   0x31
+
 // CRSF encapsulated msp defines
 #define ENCAPSULATED_MSP_HEADER_CRC_LEN     4
 #define ENCAPSULATED_MSP_MAX_PAYLOAD_SIZE   4

src/src/tx_main.cpp

@@ -1518,7 +1518,7 @@ void loop()
 
     CRSFHandset::makeLinkStatisticsPacket(linkStatisticsFrame);
     handset->sendTelemetryToTX(linkStatisticsFrame);
-    crsfTelemToMSPOut(linkStatisticsFrame);
+    sendCRSFTelemetryToBackpack(linkStatisticsFrame);
     TLMpacketReported = now;
   }
 
@@ -1536,15 +1536,15 @@ void loop()
           // If we have a backpack
           if (TxUSB != TxBackpack)
           {
-            TxBackpack->write(CRSFinBuffer + CRSF_FRAME_NOT_COUNTED_BYTES, count);
+            sendMAVLinkTelemetryToBackpack(CRSFinBuffer);
           }
         }
       }
       else
       {
         // Send all other tlm to handset
         handset->sendTelemetryToTX(CRSFinBuffer);
-        crsfTelemToMSPOut(CRSFinBuffer);
+        sendCRSFTelemetryToBackpack(CRSFinBuffer);
       }
       TelemetryReceiver.Unlock();
   }