From 8feb36e79f5382e5aee1a85f2ea01f975d33dbf0 Mon Sep 17 00:00:00 2001 From: Wez <59873510+wvarty@users.noreply.github.com> Date: Tue, 20 Aug 2024 16:56:20 +1000 Subject: [PATCH] Extend Backpack telemetry forwarding options (#2885) * Extend tlm forwarding option for backpack --- src/lib/Backpack/devBackpack.cpp | 55 +++++++++++++++++++++++--------- src/lib/Backpack/devBackpack.h | 5 ++- src/lib/CONFIG/config.cpp | 11 +++---- src/lib/CONFIG/config.h | 14 +++++--- src/lib/LUA/tx_devLUA.cpp | 8 +++-- src/lib/MSP/msptypes.h | 3 ++ src/src/tx_main.cpp | 6 ++-- 7 files changed, 70 insertions(+), 32 deletions(-) diff --git a/src/lib/Backpack/devBackpack.cpp b/src/lib/Backpack/devBackpack.cpp index f4753fb39e..449b46e321 100644 --- a/src/lib/Backpack/devBackpack.cpp +++ b/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; } diff --git a/src/lib/Backpack/devBackpack.h b/src/lib/Backpack/devBackpack.h index c67609cd5c..3ebe748521 100644 --- a/src/lib/Backpack/devBackpack.h +++ b/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; diff --git a/src/lib/CONFIG/config.cpp b/src/lib/CONFIG/config.cpp index 1af666765c..a33916327b 100644 --- a/src/lib/CONFIG/config.cpp +++ b/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; itlm = 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; } } diff --git a/src/lib/CONFIG/config.h b/src/lib/CONFIG/config.h index 3fed6ddb85..6b251bb614 100644 --- a/src/lib/CONFIG/config.h +++ b/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); diff --git a/src/lib/LUA/tx_devLUA.cpp b/src/lib/LUA/tx_devLUA.cpp index e909ecad96..7b66647611 100644 --- a/src/lib/LUA/tx_devLUA.cpp +++ b/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) diff --git a/src/lib/MSP/msptypes.h b/src/lib/MSP/msptypes.h index 690e6b9343..a1ba080ebf 100644 --- a/src/lib/MSP/msptypes.h +++ b/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 diff --git a/src/src/tx_main.cpp b/src/src/tx_main.cpp index f35e470f3b..874582fb3f 100644 --- a/src/src/tx_main.cpp +++ b/src/src/tx_main.cpp @@ -1518,7 +1518,7 @@ void loop() CRSFHandset::makeLinkStatisticsPacket(linkStatisticsFrame); handset->sendTelemetryToTX(linkStatisticsFrame); - crsfTelemToMSPOut(linkStatisticsFrame); + sendCRSFTelemetryToBackpack(linkStatisticsFrame); TLMpacketReported = now; } @@ -1536,7 +1536,7 @@ void loop() // If we have a backpack if (TxUSB != TxBackpack) { - TxBackpack->write(CRSFinBuffer + CRSF_FRAME_NOT_COUNTED_BYTES, count); + sendMAVLinkTelemetryToBackpack(CRSFinBuffer); } } } @@ -1544,7 +1544,7 @@ void loop() { // Send all other tlm to handset handset->sendTelemetryToTX(CRSFinBuffer); - crsfTelemToMSPOut(CRSFinBuffer); + sendCRSFTelemetryToBackpack(CRSFinBuffer); } TelemetryReceiver.Unlock(); }