diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index cfe481e17e..f559e2819c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -193,8 +193,9 @@ jobs: exit 0 fi - aws s3api head-object --endpoint-url $AWS_ENDPOINT_URL --bucket expresslrs --key ExpressLRS/$GITHUB_SHA/firmware.zip > /dev/null || NOT_EXIST=true - if [ $NOT_EXIST ]; then + aws s3api head-object --endpoint-url $AWS_ENDPOINT_URL --bucket expresslrs --key ExpressLRS/$GITHUB_SHA/firmware.zip > /dev/null || ALLOW_UPLOAD=true + git describe --tags --exact-match HEAD && ALLOW_UPLOAD=true + if [ $ALLOW_UPLOAD ]; then echo "Uploading firmware to artifactory" aws s3 cp --endpoint-url $AWS_ENDPOINT_URL firmware.zip s3://expresslrs/ExpressLRS/$GITHUB_SHA/firmware.zip diff --git a/src/include/targets.h b/src/include/targets.h index 75b958c8a0..b3eb3cb3ce 100644 --- a/src/include/targets.h +++ b/src/include/targets.h @@ -289,6 +289,9 @@ extern bool pwmSerialDefined; #endif #if defined(TARGET_UNIFIED_TX) || defined(TARGET_UNIFIED_RX) +#if defined(PLATFORM_ESP32) +#include +#endif #if !defined(U0RXD_GPIO_NUM) #define U0RXD_GPIO_NUM (3) #endif 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 db25a2ebb9..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; } } @@ -1236,6 +1235,23 @@ void RxConfig::SetBindStorage(rx_config_bindstorage_t value) } } +void RxConfig::SetTargetSysId(uint8_t value) +{ + if (m_config.targetSysId != value) + { + m_config.targetSysId = value; + m_modified = true; + } +} +void RxConfig::SetSourceSysId(uint8_t value) +{ + if (m_config.sourceSysId != value) + { + m_config.sourceSysId = value; + m_modified = true; + } +} + void RxConfig::ReturnLoan() { if (IsOnLoan()) diff --git a/src/lib/CONFIG/config.h b/src/lib/CONFIG/config.h index 2c65dfeeba..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); @@ -229,6 +235,8 @@ typedef struct __attribute__((packed)) { uint8_t teamraceChannel:4, teamracePosition:3, teamracePitMode:1; // FUTURE: Enable pit mode when disabling model + uint8_t targetSysId; + uint8_t sourceSysId; } rx_config_t; class RxConfig @@ -263,6 +271,8 @@ class RxConfig uint8_t GetTeamraceChannel() const { return m_config.teamraceChannel; } uint8_t GetTeamracePosition() const { return m_config.teamracePosition; } eFailsafeMode GetFailsafeMode() const { return (eFailsafeMode)m_config.failsafeMode; } + uint8_t GetTargetSysId() const { return m_config.targetSysId; } + uint8_t GetSourceSysId() const { return m_config.sourceSysId; } rx_config_bindstorage_t GetBindStorage() const { return (rx_config_bindstorage_t)m_config.bindStorage; } bool IsOnLoan() const; @@ -287,6 +297,8 @@ class RxConfig void SetTeamraceChannel(uint8_t teamraceChannel); void SetTeamracePosition(uint8_t teamracePosition); void SetFailsafeMode(eFailsafeMode failsafeMode); + void SetTargetSysId(uint8_t sysID); + void SetSourceSysId(uint8_t sysID); void SetBindStorage(rx_config_bindstorage_t value); void ReturnLoan(); diff --git a/src/lib/Handset/AutoDetect.cpp b/src/lib/Handset/AutoDetect.cpp index 9ae32d035a..0249ed7798 100644 --- a/src/lib/Handset/AutoDetect.cpp +++ b/src/lib/Handset/AutoDetect.cpp @@ -9,7 +9,7 @@ #include -constexpr auto RMT_TICKS_PER_US = 10; +constexpr auto RMT_TICKS_PER_US = 4; void AutoDetect::Begin() { diff --git a/src/lib/Handset/PPMHandset.cpp b/src/lib/Handset/PPMHandset.cpp index c9e2e8e892..9e9cc41560 100644 --- a/src/lib/Handset/PPMHandset.cpp +++ b/src/lib/Handset/PPMHandset.cpp @@ -9,7 +9,7 @@ #include -constexpr auto RMT_TICKS_PER_US = 10; +constexpr auto RMT_TICKS_PER_US = 4; void PPMHandset::Begin() { diff --git a/src/lib/LED/devLED.cpp b/src/lib/LED/devLED.cpp index 2f7b53695a..e9dbfb73d8 100644 --- a/src/lib/LED/devLED.cpp +++ b/src/lib/LED/devLED.cpp @@ -51,7 +51,9 @@ static const uint8_t *_durations; static uint8_t _count; static uint8_t _counter = 0; static bool hasRGBLeds = false; +#if defined(TARGET_TX) static bool hasGBLeds = false; +#endif static uint16_t updateLED() { @@ -141,6 +143,7 @@ static int timeout() return updateLED(); } +#if !defined(TARGET_RX) static void setPowerLEDs() { if (hasGBLeds) @@ -166,6 +169,7 @@ static void setPowerLEDs() } } } +#endif static int event() { diff --git a/src/lib/LUA/rx_devLUA.cpp b/src/lib/LUA/rx_devLUA.cpp index 45c99e4b60..945ef2b2ac 100644 --- a/src/lib/LUA/rx_devLUA.cpp +++ b/src/lib/LUA/rx_devLUA.cpp @@ -39,6 +39,29 @@ static struct luaItem_selection luaSBUSFailsafeMode = { STR_EMPTYSPACE }; +static struct luaItem_int8 luaTargetSysId = { + {"Target SysID", CRSF_UINT8}, + { + { + (uint8_t)1, // value - default to 1 + (uint8_t)1, // min + (uint8_t)255, // max + } + }, + STR_EMPTYSPACE +}; +static struct luaItem_int8 luaSourceSysId = { + {"Source SysID", CRSF_UINT8}, + { + { + (uint8_t)255, // value - default to 255 + (uint8_t)1, // min + (uint8_t)255, // max + } + }, + STR_EMPTYSPACE +}; + #if defined(POWER_OUTPUT_VALUES) static struct luaItem_selection luaTlmPower = { {"Tlm Power", CRSF_TEXT_SELECTION}, @@ -510,6 +533,23 @@ static void registerLuaParameters() config.SetFailsafeMode((eFailsafeMode)arg); }); +eSerialProtocol prot0 = config.GetSerialProtocol(); +bool hasMavlink = prot0 == PROTOCOL_MAVLINK; +#if defined(PLATFORM_ESP32) + eSerial1Protocol prot1 = config.GetSerial1Protocol(); + hasMavlink = hasMavlink || (prot1 == PROTOCOL_MAVLINK); +#endif + + if (hasMavlink) + { + registerLUAParameter(&luaTargetSysId, [](struct luaPropertiesCommon* item, uint8_t arg){ + config.SetTargetSysId((uint8_t)arg); + }); + registerLUAParameter(&luaSourceSysId, [](struct luaPropertiesCommon* item, uint8_t arg){ + config.SetSourceSysId((uint8_t)arg); + }); + } + if (GPIO_PIN_ANT_CTRL != UNDEF_PIN) { registerLUAParameter(&luaAntennaMode, [](struct luaPropertiesCommon* item, uint8_t arg){ @@ -627,6 +667,10 @@ static int event() } setLuaTextSelectionValue(&luaBindStorage, config.GetBindStorage()); updateBindModeLabel(); + + setLuaUint8Value(&luaSourceSysId, config.GetSourceSysId() == 0 ? 255 : config.GetSourceSysId()); //display Source sysID if 0 display 255 to mimic logic in SerialMavlink.cpp + setLuaUint8Value(&luaTargetSysId, config.GetTargetSysId() == 0 ? 1 : config.GetTargetSysId()); //display Target sysID if 0 display 1 to mimic logic in SerialMavlink.cpp + return DURATION_IMMEDIATELY; } 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/lib/OPTIONS/options.cpp b/src/lib/OPTIONS/options.cpp index 3162fc621d..12877a54a5 100644 --- a/src/lib/OPTIONS/options.cpp +++ b/src/lib/OPTIONS/options.cpp @@ -322,7 +322,7 @@ static void options_LoadFromFlashOrFile(EspFlashStream &strmFlash) { firmwareOptions.hasUID = false; } - int32_t wifiInterval = doc["wifi-on-interval"] | 60; + int32_t wifiInterval = doc["wifi-on-interval"] | -1; firmwareOptions.wifi_auto_on_interval = wifiInterval == -1 ? -1 : wifiInterval * 1000; strlcpy(firmwareOptions.home_wifi_ssid, doc["wifi-ssid"] | "", sizeof(firmwareOptions.home_wifi_ssid)); strlcpy(firmwareOptions.home_wifi_password, doc["wifi-password"] | "", sizeof(firmwareOptions.home_wifi_password)); diff --git a/src/lib/SCREEN/devScreen.cpp b/src/lib/SCREEN/devScreen.cpp index ca0c480e49..d052d52eab 100644 --- a/src/lib/SCREEN/devScreen.cpp +++ b/src/lib/SCREEN/devScreen.cpp @@ -36,14 +36,6 @@ static bool jumpToChannelSelect = false; static int handle(void) { -#if defined(JOY_ADC_VALUES) && defined(PLATFORM_ESP32) - // if we are using analog joystick then we can't cancel because WiFi is using the ADC2 (i.e. channel >= 8)! - if (connectionState == wifiUpdate && digitalPinToAnalogChannel(GPIO_PIN_JOYSTICK) >= 8) - { - return DURATION_NEVER; - } -#endif - #ifdef HAS_GSENSOR is_screen_flipped = gsensor.isFlipped(); @@ -81,7 +73,19 @@ static int handle(void) { int key; bool isLongPressed; +#if defined(JOY_ADC_VALUES) && defined(PLATFORM_ESP32) + // if we are using analog joystick then we can't cancel because WiFi is using the ADC2 (i.e. channel >= 8)! + if (connectionState == wifiUpdate && digitalPinToAnalogChannel(GPIO_PIN_JOYSTICK) >= 8) + { + key = INPUT_KEY_NO_PRESS; + } + else + { + fivewaybutton.update(&key, &isLongPressed); + } +#else fivewaybutton.update(&key, &isLongPressed); +#endif fsm_event_t fsm_event; switch (key) { diff --git a/src/lib/SCREEN/menu.cpp b/src/lib/SCREEN/menu.cpp index e7a320e901..f98559dbbe 100644 --- a/src/lib/SCREEN/menu.cpp +++ b/src/lib/SCREEN/menu.cpp @@ -577,7 +577,7 @@ fsm_state_entry_t const wifi_update_menu_fsm[] = { {STATE_LAST} }; fsm_state_event_t const wifi_menu_update_events[] = {MENU_EVENTS(wifi_update_menu_fsm)}; -fsm_state_event_t const wifi_ext_execute_events[] = {{EVENT_TIMEOUT, GOTO(STATE_WIFI_EXECUTE)}}; +fsm_state_event_t const wifi_ext_execute_events[] = {{EVENT_TIMEOUT, ACTION_POP}}; fsm_state_entry_t const wifi_ext_menu_fsm[] = { {STATE_WIFI_EXECUTE, nullptr, executeWiFi, 1000, wifi_ext_execute_events, ARRAY_SIZE(wifi_ext_execute_events)}, {STATE_LAST} diff --git a/src/lib/SCREEN/menu.h b/src/lib/SCREEN/menu.h index 8fdb496170..4dbad8c47c 100644 --- a/src/lib/SCREEN/menu.h +++ b/src/lib/SCREEN/menu.h @@ -16,7 +16,6 @@ enum fsm_state_s { STATE_BIND, STATE_WIFI, STATE_VTX, - STATE_LINKSTATS, STATE_POWER_MAX, STATE_POWER_DYNAMIC, @@ -57,6 +56,8 @@ enum fsm_state_s { STATE_VALUE_INC, STATE_VALUE_DEC, STATE_VALUE_SAVE, + + STATE_LINKSTATS }; diff --git a/src/lib/WIFI/devWIFI.cpp b/src/lib/WIFI/devWIFI.cpp index 5d55427a3c..812ec13db1 100644 --- a/src/lib/WIFI/devWIFI.cpp +++ b/src/lib/WIFI/devWIFI.cpp @@ -972,7 +972,7 @@ static void startMDNS() return; } - String options = "-DAUTO_WIFI_ON_INTERVAL=" + String(firmwareOptions.wifi_auto_on_interval / 1000); + String options = "-DAUTO_WIFI_ON_INTERVAL=" + (firmwareOptions.wifi_auto_on_interval == -1 ? "-1" : String(firmwareOptions.wifi_auto_on_interval / 1000)); #ifdef TARGET_TX if (firmwareOptions.unlock_higher_power) diff --git a/src/python/binary_flash.py b/src/python/binary_flash.py index a884a0ecee..70e2fc1913 100644 --- a/src/python/binary_flash.py +++ b/src/python/binary_flash.py @@ -91,7 +91,7 @@ def upload_esp8266_bf(args, options): if retval != ElrsUploadResult.Success: return retval try: - cmd = ['--passthrough', '--chip', 'esp8266', '--port', args.port, '--baud', str(args.baud), '--before', 'no_reset', '--after', 'soft_reset', '--no-stub', 'write_flash'] + cmd = ['--passthrough', '--chip', 'esp8266', '--port', args.port, '--baud', str(args.baud), '--before', 'no_reset', '--after', 'soft_reset', 'write_flash'] if args.erase: cmd.append('--erase-all') cmd.extend(['0x0000', args.file.name]) esptool.main(cmd) diff --git a/src/src/rx-serial/SerialMavlink.cpp b/src/src/rx-serial/SerialMavlink.cpp index 420721dc5f..cdd1d3d5a6 100644 --- a/src/src/rx-serial/SerialMavlink.cpp +++ b/src/src/rx-serial/SerialMavlink.cpp @@ -4,6 +4,7 @@ #include "device.h" #include "common.h" #include "CRSF.h" +#include "config.h" // Variables / constants for Mavlink // FIFO mavlinkInputBuffer; @@ -49,14 +50,16 @@ void SerialMavlink::sendQueuedData(uint32_t maxBytesToSend) SerialMavlink::SerialMavlink(Stream &out, Stream &in): SerialIO(&out, &in), - // 255 is typically used by the GCS, for RC override to work in ArduPilot `SYSID_MYGCS` must be set to this value (255 is the default) - this_system_id(255), - // Strictly this is not a valid source component ID - this_component_id(MAV_COMPONENT::MAV_COMP_ID_ALL), - // Assume vehicle system ID is 1, ArduPilot's `SYSID_THISMAV` parameter. (1 is the default) - target_system_id(1), - // Send to AutoPilot component - target_component_id(MAV_COMPONENT::MAV_COMP_ID_AUTOPILOT1) + + //system ID of the device component sending command to FC, can be set using lua options, 0 is the default value for initialized storage, treat it as 255 which is commonly used as GCS SysID + this_system_id(config.GetSourceSysId() ? config.GetSourceSysId() : 255), + //use telemetry radio compId as we are providing radio status messages and pass telemetry + this_component_id(MAV_COMPONENT::MAV_COMP_ID_TELEMETRY_RADIO), + + // system ID of vehicle we want to control must be the same as target vehicle, can be set using lua options, 0 is the default value for initialized storage, treat it as 1 which is commonly used as UAV SysID in 1:1 networks + target_system_id(config.GetTargetSysId() ? config.GetTargetSysId() : 1), + // Send to all components as we may have ex. gimbal that listens to RC instead of using Autopilot driver + target_component_id(MAV_COMPONENT::MAV_COMP_ID_ALL) { } diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp index 5a73d54cac..049a82de85 100644 --- a/src/src/rx_main.cpp +++ b/src/src/rx_main.cpp @@ -255,6 +255,8 @@ static uint8_t debugRcvrLinkstatsFhssIdx; #endif bool BindingModeRequest = false; +static uint32_t BindingRateChangeTime; +#define BindingRateChangeCyclePeriod 125 extern void setWifiUpdateMode(); void reconfigureSerial(); @@ -1244,11 +1246,6 @@ void MspReceiveComplete() #endif break; case MSP_ELRS_MAVLINK_TLM: // 0xFD - if (config.GetSerialProtocol() != PROTOCOL_MAVLINK) - { - config.SetSerialProtocol(PROTOCOL_MAVLINK); - reconfigureSerial(); - } // raw mavlink data mavlinkOutputBuffer.atomicPushBytes(&MspData[2], MspData[1]); break; @@ -1752,11 +1749,7 @@ static void EnterBindingMode() // Start attempting to bind // Lock the RF rate and freq while binding SetRFLinkRate(enumRatetoIndex(RATE_BINDING), true); - Radio.SetFrequencyReg(FHSSgetInitialFreq()); - if (geminiMode) - { - Radio.SetFrequencyReg(FHSSgetInitialGeminiFreq(), SX12XX_Radio_2); - } + // If the Radio Params (including InvertIQ) parameter changed, need to restart RX to take effect Radio.RXnb(); @@ -1799,7 +1792,7 @@ static void ExitBindingMode() devicesTriggerEvent(); } -static void updateBindingMode() +static void updateBindingMode(unsigned long now) { // Exit binding mode if the config has been modified, indicating UID has been set if (InBindingMode && config.IsModified()) @@ -1807,6 +1800,24 @@ static void updateBindingMode() ExitBindingMode(); } +#if defined(RADIO_LR1121) + // Change frequency domains every 500ms. This will allow single LR1121 receivers to receive bind packets from SX12XX Tx modules. + else if (InBindingMode && (now - BindingRateChangeTime) > BindingRateChangeCyclePeriod) + { + BindingRateChangeTime = now; + if (ExpressLRS_currAirRate_Modparams->index == RATE_DUALBAND_BINDING) + { + SetRFLinkRate(enumRatetoIndex(RATE_BINDING), true); + } + else + { + SetRFLinkRate(RATE_DUALBAND_BINDING, true); + } + + Radio.RXnb(); + } +#endif + // If the power on counter is >=3, enter binding, the counter will be reset after 2s else if (!InBindingMode && config.GetPowerOnCounter() >= 3) { @@ -2229,7 +2240,7 @@ void loop() } updateTelemetryBurst(); - updateBindingMode(); + updateBindingMode(now); updateSwitchMode(); checkGeminiMode(); DynamicPower_UpdateRx(false); diff --git a/src/src/tx_main.cpp b/src/src/tx_main.cpp index cb038be7b9..874582fb3f 100644 --- a/src/src/tx_main.cpp +++ b/src/src/tx_main.cpp @@ -77,6 +77,7 @@ volatile bool busyTransmitting; static volatile bool ModelUpdatePending; uint8_t MSPDataPackage[5]; +#define BindingSpamAmount 25 static uint8_t BindingSendCount; bool RxWiFiReadyToSend = false; @@ -1006,20 +1007,6 @@ static void EnterBindingMode() // Lock the RF rate and freq while binding SetRFLinkRate(enumRatetoIndex(RATE_BINDING)); -#if defined(RADIO_LR1121) - FHSSuseDualBand = true; - expresslrs_mod_settings_s *const dualBandBindingModParams = get_elrs_airRateConfig(RATE_DUALBAND_BINDING); // 2.4GHz 50Hz - Radio.Config(dualBandBindingModParams->bw2, dualBandBindingModParams->sf2, dualBandBindingModParams->cr2, FHSSgetInitialGeminiFreq(), - dualBandBindingModParams->PreambleLen2, true, dualBandBindingModParams->PayloadLength, dualBandBindingModParams->interval, - (dualBandBindingModParams->radio_type == RADIO_TYPE_LR1121_GFSK_900 || dualBandBindingModParams->radio_type == RADIO_TYPE_LR1121_GFSK_2G4), - (uint8_t)UID[5], (uint8_t)UID[4], SX12XX_Radio_2); -#endif - - Radio.SetFrequencyReg(FHSSgetInitialFreq()); - if (isDualRadio() && config.GetAntennaMode() == TX_RADIO_MODE_GEMINI) // Gemini mode - { - Radio.SetFrequencyReg(FHSSgetInitialGeminiFreq(), SX12XX_Radio_2); - } // Start transmitting again hwTimer::resume(); @@ -1048,7 +1035,6 @@ void EnterBindingModeSafely() EnterBindingMode(); } - void ProcessMSPPacket(uint32_t now, mspPacket_t *packet) { #if !defined(CRITICAL_FLASH) @@ -1099,6 +1085,18 @@ void ProcessMSPPacket(uint32_t now, mspPacket_t *packet) } } +void ParseMSPData(uint8_t *buf, uint8_t size) +{ + for (uint8_t i = 0; i < size; ++i) + { + if (msp.processReceivedByte(buf[i])) + { + ProcessMSPPacket(millis(), msp.getReceivedPacket()); + msp.markPacketReceived(); + } + } +} + static void HandleUARTout() { if (firmwareOptions.is_airport) @@ -1122,7 +1120,7 @@ static void HandleUARTin() { if (firmwareOptions.is_airport) { - auto size = std::min(AP_MAX_BUF_LEN - apInputBuffer.size(), TxUSB->available()); + auto size = std::min(apInputBuffer.free(), (uint16_t)TxUSB->available()); if (size > 0) { uint8_t buf[size]; @@ -1134,7 +1132,7 @@ static void HandleUARTin() } else { - auto size = std::min(UART_INPUT_BUF_LEN - uartInputBuffer.size(), TxUSB->available()); + auto size = std::min(uartInputBuffer.free(), (uint16_t)TxUSB->available()); if (size > 0) { uint8_t buf[size]; @@ -1160,30 +1158,32 @@ static void HandleUARTin() // Read from the Backpack serial port if (TxBackpack->available()) { - if (config.GetLinkMode() == TX_MAVLINK_MODE) + auto size = std::min(uartInputBuffer.free(), (uint16_t)TxBackpack->available()); + if (size > 0) { - auto size = std::min(UART_INPUT_BUF_LEN - uartInputBuffer.size(), TxBackpack->available()); - if (size > 0) + uint8_t buf[size]; + TxBackpack->readBytes(buf, size); + + // If the TX is in Mavlink mode, push the bytes into the fifo buffer + if (config.GetLinkMode() == TX_MAVLINK_MODE) { - uint8_t buf[size]; - TxBackpack->readBytes(buf, size); uartInputBuffer.lock(); uartInputBuffer.pushBytes(buf, size); uartInputBuffer.unlock(); - // The tx is in Mavlink mode and receiving data from the Backpack (mavesp). + // The tx is in Mavlink mode and receiving data from the Backpack. // Start the hwTimer since the user might be operating the module as a standalone unit without a handset. if (connectionState == noCrossfire) { - UARTconnected(); + if (isThisAMavPacket(buf, size)) + { + UARTconnected(); + } } } - } - else if (msp.processReceivedByte(TxBackpack->read())) - { - // Finished processing a complete packet - ProcessMSPPacket(millis(), msp.getReceivedPacket()); - msp.markPacketReceived(); + + // Try to parse any MSP packets from the Backpack + ParseMSPData(buf, size); } } } @@ -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(); } @@ -1553,8 +1553,16 @@ void loop() static bool mspTransferActive = false; if (InBindingMode) { +#if defined(RADIO_LR1121) + // Send half of the bind packets on the 2.4GHz domain + if (BindingSendCount == BindingSpamAmount / 2) { + SetRFLinkRate(RATE_DUALBAND_BINDING); + // Increment BindingSendCount so that SetRFLinkRate is only called once. + BindingSendCount++; + } +#endif // exit bind mode if package after some repeats - if (BindingSendCount > 6) { + if (BindingSendCount > BindingSpamAmount) { ExitBindingMode(); } }