From c2643c1ea9ebd7a3ab3a4af4c4cb886608e06ba2 Mon Sep 17 00:00:00 2001 From: Jye <14170229+JyeSmith@users.noreply.github.com> Date: Fri, 27 Dec 2024 20:06:11 +1000 Subject: [PATCH] Double MSP (TLM and MAVLink) throughput for Gemini hardware (#3037) * add-tlmConfirm-to-tlm_dl-ota-packet-structure * clean up naming * Auto set True Diversity Rx into Diversity or Gemini mode based on Tx antenna mode * Auto configure Rx OTA protocol from the Tx * replace second packet memcmp with crc LR1121 working * remove unnecessary GPIO_PIN_NSS_2 check * update SX1280 driver and also remove isFirstRxIrq * update sx1276 driver and remove isFirstRxIrq * update tx_main * cleanup * update after master merge * cleanup * update LR1121 * remove comment * down link data * do not switch when in GemX mode * correct tlm_dl pkt buffer * POC fix optimising the mavlink SS payload size This reduces the number of partially filled pkts OTA via the SS. This need to be done correctly and in a generic way!!! * optimise mavlink SS packet size * correct txnb for rx diversity mode * update sx1280 and sx1276 drivers * remove double if (OtaIsFullRes) check * Remove comments --- src/lib/LR1121Driver/LR1121.cpp | 28 ++++++-- src/lib/LR1121Driver/LR1121.h | 2 +- src/lib/OTA/OTA.h | 2 +- src/lib/SX127xDriver/SX127x.cpp | 12 +++- src/lib/SX127xDriver/SX127x.h | 2 +- src/lib/SX1280Driver/SX1280.cpp | 13 +++- src/lib/SX1280Driver/SX1280.h | 2 +- src/src/rx_main.cpp | 87 ++++++++++++++++++++----- src/src/tx_main.cpp | 137 +++++++++++++++++++++++++++++++++++----- 9 files changed, 241 insertions(+), 44 deletions(-) diff --git a/src/lib/LR1121Driver/LR1121.cpp b/src/lib/LR1121Driver/LR1121.cpp index 9181d1c2..9e56bdb0 100644 --- a/src/lib/LR1121Driver/LR1121.cpp +++ b/src/lib/LR1121Driver/LR1121.cpp @@ -553,7 +553,7 @@ void ICACHE_RAM_ATTR LR1121Driver::TXnbISR() TXdoneCallback(); } -void ICACHE_RAM_ATTR LR1121Driver::TXnb(uint8_t * data, uint8_t size, SX12XX_Radio_Number_t radioNumber) +void ICACHE_RAM_ATTR LR1121Driver::TXnb(uint8_t * data, uint8_t size, bool sendGeminiBuffer, uint8_t * dataGemini, SX12XX_Radio_Number_t radioNumber) { transmittingRadio = radioNumber; @@ -601,15 +601,35 @@ void ICACHE_RAM_ATTR LR1121Driver::TXnb(uint8_t * data, uint8_t size, SX12XX_Rad if (useFEC) { uint8_t FECBuffer[PayloadLength] = {0}; - FECEncode(data, FECBuffer); // 3.7.4 WriteBuffer8 - hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, FECBuffer, PayloadLength, radioNumber); + if (sendGeminiBuffer) + { + FECEncode(data, FECBuffer); + hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, FECBuffer, PayloadLength, SX12XX_Radio_1); + + uint8_t FECBufferGemini[PayloadLength] = {0}; + FECEncode(dataGemini, FECBufferGemini); + hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, FECBufferGemini, PayloadLength, SX12XX_Radio_2); + } + else + { + FECEncode(data, FECBuffer); + hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, FECBuffer, PayloadLength, radioNumber); + } } else { // 3.7.4 WriteBuffer8 - hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, data, size, radioNumber); + if (sendGeminiBuffer) + { + hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, data, size, SX12XX_Radio_1); + hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, dataGemini, size, SX12XX_Radio_2); + } + else + { + hal.WriteCommand(LR11XX_REGMEM_WRITE_BUFFER8_OC, data, size, radioNumber); + } } SetMode(LR1121_MODE_TX, radioNumber); diff --git a/src/lib/LR1121Driver/LR1121.h b/src/lib/LR1121Driver/LR1121.h index 350c144d..3e0d88fd 100644 --- a/src/lib/LR1121Driver/LR1121.h +++ b/src/lib/LR1121Driver/LR1121.h @@ -40,7 +40,7 @@ public: // bool FrequencyErrorAvailable() const { return modeSupportsFei && (LastPacketSNRRaw > 0); } bool FrequencyErrorAvailable() const { return false; } - void TXnb(uint8_t * data, uint8_t size, SX12XX_Radio_Number_t radioNumber); + void TXnb(uint8_t * data, uint8_t size, bool sendGeminiBuffer, uint8_t * dataGemini, SX12XX_Radio_Number_t radioNumber); void RXnb(lr11xx_RadioOperatingModes_t rxMode = LR1121_MODE_RX); uint32_t GetIrqStatus(SX12XX_Radio_Number_t radioNumber); diff --git a/src/lib/OTA/OTA.h b/src/lib/OTA/OTA.h index 1feefdd7..45425751 100644 --- a/src/lib/OTA/OTA.h +++ b/src/lib/OTA/OTA.h @@ -145,7 +145,7 @@ typedef struct { struct { OTA_LinkStats_s stats; uint8_t payload[ELRS8_TELEMETRY_BYTES_PER_CALL - sizeof(OTA_LinkStats_s)]; - } PACKED ul_link_stats; // containsLinkStats == true + } PACKED ul_link_stats; uint8_t payload[ELRS8_TELEMETRY_BYTES_PER_CALL]; // containsLinkStats == false }; } PACKED tlm_dl; diff --git a/src/lib/SX127xDriver/SX127x.cpp b/src/lib/SX127xDriver/SX127x.cpp index 480f02c0..b5bf3162 100644 --- a/src/lib/SX127xDriver/SX127x.cpp +++ b/src/lib/SX127xDriver/SX127x.cpp @@ -429,7 +429,7 @@ void ICACHE_RAM_ATTR SX127xDriver::TXnbISR() TXdoneCallback(); } -void ICACHE_RAM_ATTR SX127xDriver::TXnb(uint8_t * data, uint8_t size, SX12XX_Radio_Number_t radioNumber) +void ICACHE_RAM_ATTR SX127xDriver::TXnb(uint8_t * data, uint8_t size, bool sendGeminiBuffer, uint8_t * dataGemini, SX12XX_Radio_Number_t radioNumber) { // if (currOpmode == SX127x_OPMODE_TX) // { @@ -459,7 +459,15 @@ void ICACHE_RAM_ATTR SX127xDriver::TXnb(uint8_t * data, uint8_t size, SX12XX_Rad RFAMP.TXenable(radioNumber); hal.writeRegister(SX127X_REG_FIFO_ADDR_PTR, SX127X_FIFO_TX_BASE_ADDR_MAX, radioNumber); - hal.writeRegister(SX127X_REG_FIFO, data, size, radioNumber); + if (sendGeminiBuffer) + { + hal.writeRegister(SX127X_REG_FIFO, data, size, SX12XX_Radio_1); + hal.writeRegister(SX127X_REG_FIFO, dataGemini, size, SX12XX_Radio_2); + } + else + { + hal.writeRegister(SX127X_REG_FIFO, data, size, radioNumber); + } SetMode(SX127x_OPMODE_TX, radioNumber); } diff --git a/src/lib/SX127xDriver/SX127x.h b/src/lib/SX127xDriver/SX127x.h index f9754d5e..efb6f1b7 100644 --- a/src/lib/SX127xDriver/SX127x.h +++ b/src/lib/SX127xDriver/SX127x.h @@ -75,7 +75,7 @@ public: void CheckForSecondPacket(); ////////////Non-blocking TX related Functions///////////////// - void TXnb(uint8_t * data, uint8_t size, SX12XX_Radio_Number_t radioNumber); + void TXnb(uint8_t * data, uint8_t size, bool sendGeminiBuffer, uint8_t * dataGemini, SX12XX_Radio_Number_t radioNumber); /////////////Non-blocking RX related Functions/////////////// void RXnb(); diff --git a/src/lib/SX1280Driver/SX1280.cpp b/src/lib/SX1280Driver/SX1280.cpp index 60927eb7..53bc3227 100644 --- a/src/lib/SX1280Driver/SX1280.cpp +++ b/src/lib/SX1280Driver/SX1280.cpp @@ -468,7 +468,7 @@ void ICACHE_RAM_ATTR SX1280Driver::TXnbISR() TXdoneCallback(); } -void ICACHE_RAM_ATTR SX1280Driver::TXnb(uint8_t * data, uint8_t size, SX12XX_Radio_Number_t radioNumber) +void ICACHE_RAM_ATTR SX1280Driver::TXnb(uint8_t * data, uint8_t size, bool sendGeminiBuffer, uint8_t * dataGemini, SX12XX_Radio_Number_t radioNumber) { transmittingRadio = radioNumber; @@ -514,7 +514,16 @@ void ICACHE_RAM_ATTR SX1280Driver::TXnb(uint8_t * data, uint8_t size, SX12XX_Rad } RFAMP.TXenable(radioNumber); // do first to allow PA stablise - hal.WriteBuffer(0x00, data, size, radioNumber); //todo fix offset to equal fifo addr + if (sendGeminiBuffer) + { + hal.WriteBuffer(0x00, data, size, SX12XX_Radio_1); + hal.WriteBuffer(0x00, dataGemini, size, SX12XX_Radio_2); + } + else + { + hal.WriteBuffer(0x00, data, size, radioNumber); + } + instance->SetMode(SX1280_MODE_TX, radioNumber); #ifdef DEBUG_SX1280_OTA_TIMING diff --git a/src/lib/SX1280Driver/SX1280.h b/src/lib/SX1280Driver/SX1280.h index df85d2f6..3d09a9c8 100644 --- a/src/lib/SX1280Driver/SX1280.h +++ b/src/lib/SX1280Driver/SX1280.h @@ -40,7 +40,7 @@ public: bool GetFrequencyErrorbool(SX12XX_Radio_Number_t radioNumber); bool FrequencyErrorAvailable() const { return modeSupportsFei && (LastPacketSNRRaw > 0); } - void TXnb(uint8_t * data, uint8_t size, SX12XX_Radio_Number_t radioNumber); + void TXnb(uint8_t * data, uint8_t size, bool sendGeminiBuffer, uint8_t * dataGemini, SX12XX_Radio_Number_t radioNumber); void RXnb(SX1280_RadioOperatingModes_t rxMode = SX1280_MODE_RX, uint32_t incomingTimeout = 0); uint16_t GetIrqStatus(SX12XX_Radio_Number_t radioNumber); diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp index b5c8c131..1d8cc597 100644 --- a/src/src/rx_main.cpp +++ b/src/src/rx_main.cpp @@ -452,7 +452,9 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() // ESP requires word aligned buffer WORD_ALIGNED_ATTR OTA_Packet_s otaPkt = {0}; + WORD_ALIGNED_ATTR OTA_Packet_s otaPktGemini = {0}; alreadyTLMresp = true; + bool sendGeminiBuffer = false; bool tlmQueued = false; if (firmwareOptions.is_airport) @@ -469,20 +471,39 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() otaPkt.std.type = PACKET_TYPE_LINKSTATS; OTA_LinkStats_s * ls; + ls = &otaPkt.full.tlm_dl.ul_link_stats.stats; + + // Include some advanced telemetry in the extra space + // Note the use of `ul_link_stats.payload` vs just `payload` if (OtaIsFullRes) { - ls = &otaPkt.full.tlm_dl.ul_link_stats.stats; - // Include some advanced telemetry in the extra space - // Note the use of `ul_link_stats.payload` vs just `payload` - otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( - otaPkt.full.tlm_dl.ul_link_stats.payload, - sizeof(otaPkt.full.tlm_dl.ul_link_stats.payload)); + otaPkt.full.tlm_dl.tlmConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0; + + if (geminiMode) + { + sendGeminiBuffer = true; + WORD_ALIGNED_ATTR uint8_t tlmSenderDoubleBuffer[2 * sizeof(otaPkt.full.tlm_dl.ul_link_stats.payload)] = {0}; + + otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(tlmSenderDoubleBuffer, sizeof(tlmSenderDoubleBuffer)); + memcpy(otaPkt.full.tlm_dl.ul_link_stats.payload, tlmSenderDoubleBuffer, sizeof(otaPkt.full.tlm_dl.ul_link_stats.payload)); + LinkStatsToOta(ls); + + otaPktGemini = otaPkt; + memcpy(otaPktGemini.full.tlm_dl.ul_link_stats.payload, &tlmSenderDoubleBuffer[sizeof(otaPktGemini.full.tlm_dl.ul_link_stats.payload)], sizeof(otaPktGemini.full.tlm_dl.ul_link_stats.payload)); + } + else + { + otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( + otaPkt.full.tlm_dl.ul_link_stats.payload, + sizeof(otaPkt.full.tlm_dl.ul_link_stats.payload)); + LinkStatsToOta(ls); + } } else { ls = &otaPkt.std.tlm_dl.ul_link_stats.stats; + LinkStatsToOta(ls); } - LinkStatsToOta(ls); NextTelemetryType = PACKET_TYPE_DATA; // Start the count at 1 because the next will be DATA and doing +1 before checking @@ -511,29 +532,51 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() if (OtaIsFullRes) { otaPkt.full.tlm_dl.tlmConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0; - - if (TelemetrySender.IsActive()) + + if (geminiMode) { - otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( - otaPkt.full.tlm_dl.payload, - sizeof(otaPkt.full.tlm_dl.payload)); + sendGeminiBuffer = true; + WORD_ALIGNED_ATTR uint8_t tlmSenderDoubleBuffer[2 * sizeof(otaPkt.full.tlm_dl.payload)] = {0}; + + otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(tlmSenderDoubleBuffer, sizeof(tlmSenderDoubleBuffer)); + memcpy(otaPkt.full.tlm_dl.payload, tlmSenderDoubleBuffer, sizeof(otaPkt.full.tlm_dl.payload)); + + otaPktGemini = otaPkt; + memcpy(otaPktGemini.full.tlm_dl.payload, &tlmSenderDoubleBuffer[sizeof(otaPktGemini.full.tlm_dl.payload)], sizeof(otaPktGemini.full.tlm_dl.payload)); + } + else + { + otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(otaPkt.full.tlm_dl.payload, sizeof(otaPkt.full.tlm_dl.payload)); } } else { otaPkt.std.tlm_dl.tlmConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0; - if (TelemetrySender.IsActive()) + if (geminiMode) { - otaPkt.std.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( - otaPkt.std.tlm_dl.payload, - sizeof(otaPkt.std.tlm_dl.payload)); + sendGeminiBuffer = true; + WORD_ALIGNED_ATTR uint8_t tlmSenderDoubleBuffer[2 * sizeof(otaPkt.std.tlm_dl.payload)] = {0}; + + otaPkt.std.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(tlmSenderDoubleBuffer, sizeof(tlmSenderDoubleBuffer)); + memcpy(otaPkt.std.tlm_dl.payload, tlmSenderDoubleBuffer, sizeof(otaPkt.std.tlm_dl.payload)); + + otaPktGemini = otaPkt; + memcpy(otaPktGemini.std.tlm_dl.payload, &tlmSenderDoubleBuffer[sizeof(otaPktGemini.std.tlm_dl.payload)], sizeof(otaPktGemini.std.tlm_dl.payload)); + } + else + { + otaPkt.std.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(otaPkt.std.tlm_dl.payload, sizeof(otaPkt.std.tlm_dl.payload)); } } } } OtaGeneratePacketCrc(&otaPkt); + if (sendGeminiBuffer) + { + OtaGeneratePacketCrc(&otaPktGemini); + } SX12XX_Radio_Number_t transmittingRadio; if (config.GetForceTlmOff()) @@ -558,7 +601,17 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() transmittingRadio = Radio.LastPacketRSSI > Radio.LastPacketRSSI2 ? SX12XX_Radio_1 : SX12XX_Radio_2; // Pick the radio with best rf connection to the tx. } - Radio.TXnb((uint8_t*)&otaPkt, ExpressLRS_currAirRate_Modparams->PayloadLength, transmittingRadio); + // Gemini flips frequencies between radios on the rx side only. This is to help minimise antenna cross polarization. + // The payloads need to be switch when this happens. + // GemX does not switch due to the time required to reconfigure the LR1121 params. + if (((OtaNonce + 1)/ExpressLRS_currAirRate_Modparams->FHSShopInterval) % 2 == 0 || !sendGeminiBuffer || FHSSuseDualBand) + { + Radio.TXnb((uint8_t*)&otaPkt, ExpressLRS_currAirRate_Modparams->PayloadLength, sendGeminiBuffer, (uint8_t*)&otaPktGemini, transmittingRadio); + } + else + { + Radio.TXnb((uint8_t*)&otaPktGemini, ExpressLRS_currAirRate_Modparams->PayloadLength, sendGeminiBuffer, (uint8_t*)&otaPkt, transmittingRadio); + } if (transmittingRadio == SX12XX_Radio_NONE) { diff --git a/src/src/tx_main.cpp b/src/src/tx_main.cpp index fddbae1b..cc1fdd6a 100644 --- a/src/src/tx_main.cpp +++ b/src/src/tx_main.cpp @@ -64,6 +64,9 @@ extern bool webserverPreventAutoStart; //// MSP Data Handling /////// bool NextPacketIsMspData = false; // if true the next packet will contain the msp data char backpackVersion[32] = ""; +uint8_t packageIndexRadio1 = 0xFF; +uint8_t packageIndexRadio2 = 0xFF; +uint8_t tlmSenderDoubleBuffer[20] = {0}; ////////////SYNC PACKET///////// /// sync packet spamming on mode change vars /// @@ -195,15 +198,50 @@ bool ICACHE_RAM_ATTR ProcessTLMpacket(SX12xxDriverCommon::rx_status const status if (OtaIsFullRes) { OTA_Packet8_s * const ota8 = (OTA_Packet8_s * const)otaPktPtr; - uint8_t *telemPtr; - uint8_t dataLen; + OTA_Packet8_s * const ota8Second = (OTA_Packet8_s * const)otaPktPtrSecond; switch (otaPktPtr->std.type) { case PACKET_TYPE_LINKSTATS: LinkStatsFromOta(&ota8->tlm_dl.ul_link_stats.stats); - telemPtr = ota8->tlm_dl.ul_link_stats.payload; - dataLen = sizeof(ota8->tlm_dl.ul_link_stats.payload); + + if (config.GetAntennaMode() == TX_RADIO_MODE_GEMINI) + { + if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1) + { + packageIndexRadio1 = ota8->tlm_dl.packageIndex; + memcpy(tlmSenderDoubleBuffer, ota8->tlm_dl.ul_link_stats.payload, sizeof(ota8->tlm_dl.ul_link_stats.payload)); + } + else + { + packageIndexRadio2 = ota8->tlm_dl.packageIndex; + memcpy(&tlmSenderDoubleBuffer[sizeof(ota8->tlm_dl.ul_link_stats.payload)], ota8->tlm_dl.ul_link_stats.payload, sizeof(ota8->tlm_dl.ul_link_stats.payload)); + } + + if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1 && Radio.hasSecondRadioGotData) + { + packageIndexRadio2 = ota8Second->tlm_dl.packageIndex; + memcpy(&tlmSenderDoubleBuffer[sizeof(ota8Second->tlm_dl.ul_link_stats.payload)], ota8Second->tlm_dl.ul_link_stats.payload, sizeof(ota8Second->tlm_dl.ul_link_stats.payload)); + } + else if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_2 && Radio.hasSecondRadioGotData) + { + packageIndexRadio1 = ota8Second->tlm_dl.packageIndex; + memcpy(tlmSenderDoubleBuffer, ota8Second->tlm_dl.ul_link_stats.payload, sizeof(ota8Second->tlm_dl.ul_link_stats.payload)); + } + + if (packageIndexRadio1 == packageIndexRadio2 && packageIndexRadio1 != 0xFF) + { + TelemetryReceiver.ReceiveData(packageIndexRadio1 & ELRS8_TELEMETRY_MAX_PACKAGES, + tlmSenderDoubleBuffer, 2 * sizeof(ota8->tlm_dl.ul_link_stats.payload)); + packageIndexRadio1 = 0xFF; + packageIndexRadio2 = 0xFF; + } + } + else + { + TelemetryReceiver.ReceiveData(ota8->tlm_dl.packageIndex & ELRS8_TELEMETRY_MAX_PACKAGES, + ota8->tlm_dl.ul_link_stats.payload, sizeof(ota8->tlm_dl.ul_link_stats.payload)); + } break; case PACKET_TYPE_DATA: @@ -213,15 +251,48 @@ bool ICACHE_RAM_ATTR ProcessTLMpacket(SX12xxDriverCommon::rx_status const status } else { - telemPtr = ota8->tlm_dl.payload; - dataLen = sizeof(ota8->tlm_dl.payload); - MspSender.ConfirmCurrentPayload(ota8->tlm_dl.tlmConfirm); + if (config.GetAntennaMode() == TX_RADIO_MODE_GEMINI) + { + if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1) + { + packageIndexRadio1 = ota8->tlm_dl.packageIndex; + memcpy(tlmSenderDoubleBuffer, ota8->tlm_dl.payload, sizeof(ota8->tlm_dl.payload)); + } + else + { + packageIndexRadio2 = ota8->tlm_dl.packageIndex; + memcpy(&tlmSenderDoubleBuffer[sizeof(ota8->tlm_dl.payload)], ota8->tlm_dl.payload, sizeof(ota8->tlm_dl.payload)); + } + + if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1 && Radio.hasSecondRadioGotData) + { + packageIndexRadio2 = ota8Second->tlm_dl.packageIndex; + memcpy(&tlmSenderDoubleBuffer[sizeof(ota8Second->tlm_dl.payload)], ota8Second->tlm_dl.payload, sizeof(ota8Second->tlm_dl.payload)); + } + else if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_2 && Radio.hasSecondRadioGotData) + { + packageIndexRadio1 = ota8Second->tlm_dl.packageIndex; + memcpy(tlmSenderDoubleBuffer, ota8Second->tlm_dl.payload, sizeof(ota8Second->tlm_dl.payload)); + } + + if (packageIndexRadio1 == packageIndexRadio2 && packageIndexRadio1 != 0xFF) + { + MspSender.ConfirmCurrentPayload(ota8->tlm_dl.tlmConfirm); + TelemetryReceiver.ReceiveData(packageIndexRadio1 & ELRS8_TELEMETRY_MAX_PACKAGES, + tlmSenderDoubleBuffer, 2 * sizeof(ota8->tlm_dl.payload)); + packageIndexRadio1 = 0xFF; + packageIndexRadio2 = 0xFF; + } + } + else + { + MspSender.ConfirmCurrentPayload(ota8->tlm_dl.tlmConfirm); + TelemetryReceiver.ReceiveData(ota8->tlm_dl.packageIndex & ELRS8_TELEMETRY_MAX_PACKAGES, + ota8->tlm_dl.payload, sizeof(ota8->tlm_dl.payload)); + } } break; } - - //DBGLN("pi=%u len=%u", ota8->tlm_dl.packageIndex, dataLen); - TelemetryReceiver.ReceiveData(ota8->tlm_dl.packageIndex & ELRS8_TELEMETRY_MAX_PACKAGES, telemPtr, dataLen); } // Std res mode else @@ -239,10 +310,46 @@ bool ICACHE_RAM_ATTR ProcessTLMpacket(SX12xxDriverCommon::rx_status const status } else { - MspSender.ConfirmCurrentPayload(otaPktPtr->std.tlm_dl.tlmConfirm); - TelemetryReceiver.ReceiveData(otaPktPtr->std.tlm_dl.packageIndex & ELRS4_TELEMETRY_MAX_PACKAGES, - otaPktPtr->std.tlm_dl.payload, - sizeof(otaPktPtr->std.tlm_dl.payload)); + if (config.GetAntennaMode() == TX_RADIO_MODE_GEMINI) + { + if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1) + { + packageIndexRadio1 = otaPktPtr->std.tlm_dl.packageIndex; + memcpy(tlmSenderDoubleBuffer, otaPktPtr->std.tlm_dl.payload, sizeof(otaPktPtr->std.tlm_dl.payload)); + } + else + { + packageIndexRadio2 = otaPktPtr->std.tlm_dl.packageIndex; + memcpy(&tlmSenderDoubleBuffer[sizeof(otaPktPtr->std.tlm_dl.payload)], otaPktPtr->std.tlm_dl.payload, sizeof(otaPktPtr->std.tlm_dl.payload)); + } + + if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1 && Radio.hasSecondRadioGotData) + { + packageIndexRadio2 = otaPktPtrSecond->std.tlm_dl.packageIndex; + memcpy(&tlmSenderDoubleBuffer[sizeof(otaPktPtrSecond->std.tlm_dl.payload)], otaPktPtrSecond->std.tlm_dl.payload, sizeof(otaPktPtrSecond->std.tlm_dl.payload)); + } + else if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_2 && Radio.hasSecondRadioGotData) + { + packageIndexRadio1 = otaPktPtrSecond->std.tlm_dl.packageIndex; + memcpy(tlmSenderDoubleBuffer, otaPktPtrSecond->std.tlm_dl.payload, sizeof(otaPktPtrSecond->std.tlm_dl.payload)); + } + + if (packageIndexRadio1 == packageIndexRadio2 && packageIndexRadio1 != 0xFF) + { + MspSender.ConfirmCurrentPayload(otaPktPtr->std.tlm_dl.tlmConfirm); + TelemetryReceiver.ReceiveData(packageIndexRadio1 & ELRS4_TELEMETRY_MAX_PACKAGES, + tlmSenderDoubleBuffer, 2 * sizeof(otaPktPtr->std.tlm_dl.payload)); + packageIndexRadio1 = 0xFF; + packageIndexRadio2 = 0xFF; + } + } + else + { + MspSender.ConfirmCurrentPayload(otaPktPtr->std.tlm_dl.tlmConfirm); + TelemetryReceiver.ReceiveData(otaPktPtr->std.tlm_dl.packageIndex & ELRS4_TELEMETRY_MAX_PACKAGES, + otaPktPtr->std.tlm_dl.payload, + sizeof(otaPktPtr->std.tlm_dl.payload)); + } } break; } @@ -615,7 +722,7 @@ void ICACHE_RAM_ATTR SendRCdataToRF() else #endif { - Radio.TXnb((uint8_t*)&otaPkt, ExpressLRS_currAirRate_Modparams->PayloadLength, transmittingRadio); + Radio.TXnb((uint8_t*)&otaPkt, ExpressLRS_currAirRate_Modparams->PayloadLength, false, (uint8_t*)&otaPkt, transmittingRadio); } } -- 2.11.4.GIT