From b972f6ac7f02145834358aa57d5da0d35329b280 Mon Sep 17 00:00:00 2001 From: Jye <14170229+JyeSmith@users.noreply.github.com> Date: Fri, 27 Dec 2024 09:49:54 +1000 Subject: [PATCH] Set SetPPMoffsetReg for both radios on Team900 diversity hardware (#3019) * 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 * SetPPMoffsetReg for both radios on diversity hardware * change from double to int arithmetic --- src/lib/LR1121Driver/LR1121.cpp | 2 +- src/lib/LR1121Driver/LR1121.h | 2 +- src/lib/SX127xDriver/SX127x.cpp | 10 +++++----- src/lib/SX127xDriver/SX127x.h | 4 ++-- src/lib/SX1280Driver/SX1280.cpp | 4 ++-- src/lib/SX1280Driver/SX1280.h | 2 +- src/src/rx_main.cpp | 32 ++++++++++++++++++++++---------- 7 files changed, 34 insertions(+), 22 deletions(-) diff --git a/src/lib/LR1121Driver/LR1121.cpp b/src/lib/LR1121Driver/LR1121.cpp index 75e52d7c..9181d1c2 100644 --- a/src/lib/LR1121Driver/LR1121.cpp +++ b/src/lib/LR1121Driver/LR1121.cpp @@ -655,7 +655,7 @@ void ICACHE_RAM_ATTR LR1121Driver::RXnb(lr11xx_RadioOperatingModes_t rxMode) SetMode(LR1121_MODE_RX, SX12XX_Radio_All); } -bool ICACHE_RAM_ATTR LR1121Driver::GetFrequencyErrorbool() +bool ICACHE_RAM_ATTR LR1121Driver::GetFrequencyErrorbool(SX12XX_Radio_Number_t radioNumber) { return false; } diff --git a/src/lib/LR1121Driver/LR1121.h b/src/lib/LR1121Driver/LR1121.h index 904e3c48..350c144d 100644 --- a/src/lib/LR1121Driver/LR1121.h +++ b/src/lib/LR1121Driver/LR1121.h @@ -36,7 +36,7 @@ public: void startCWTest(uint32_t freq, SX12XX_Radio_Number_t radioNumber); - bool GetFrequencyErrorbool(); + bool GetFrequencyErrorbool(SX12XX_Radio_Number_t radioNumber); // bool FrequencyErrorAvailable() const { return modeSupportsFei && (LastPacketSNRRaw > 0); } bool FrequencyErrorAvailable() const { return false; } diff --git a/src/lib/SX127xDriver/SX127x.cpp b/src/lib/SX127xDriver/SX127x.cpp index 1449d293..480f02c0 100644 --- a/src/lib/SX127xDriver/SX127x.cpp +++ b/src/lib/SX127xDriver/SX127x.cpp @@ -660,15 +660,15 @@ uint32_t ICACHE_RAM_ATTR SX127xDriver::GetCurrBandwidthNormalisedShifted() // th * @param offset is in Hz or FREQ_STEP (FREQ_HZ_TO_REG_VAL) units, whichever * was used to SetFrequencyHz/SetFrequencyReg */ -void ICACHE_RAM_ATTR SX127xDriver::SetPPMoffsetReg(int32_t offset) +void ICACHE_RAM_ATTR SX127xDriver::SetPPMoffsetReg(int32_t offset, SX12XX_Radio_Number_t radioNumber) { - int8_t offsetPPM = (offset * 1e6 / currFreq) * 95 / 100; - hal.writeRegister(SX127x_PPMOFFSET, (uint8_t)offsetPPM, processingPacketRadio); + int8_t offsetPPM = (offset * 1000000 / currFreq) * 95 / 100; + hal.writeRegister(SX127x_PPMOFFSET, (uint8_t)offsetPPM, radioNumber); } -bool ICACHE_RAM_ATTR SX127xDriver::GetFrequencyErrorbool() +bool ICACHE_RAM_ATTR SX127xDriver::GetFrequencyErrorbool(SX12XX_Radio_Number_t radioNumber) { - return (hal.readRegister(SX127X_REG_FEI_MSB, processingPacketRadio) & 0b1000) >> 3; // returns true if pos freq error, neg if false + return (hal.readRegister(SX127X_REG_FEI_MSB, radioNumber) & 0b1000) >> 3; // returns true if pos freq error, neg if false } int32_t ICACHE_RAM_ATTR SX127xDriver::GetFrequencyError() diff --git a/src/lib/SX127xDriver/SX127x.h b/src/lib/SX127xDriver/SX127x.h index a02f6a2a..f9754d5e 100644 --- a/src/lib/SX127xDriver/SX127x.h +++ b/src/lib/SX127xDriver/SX127x.h @@ -54,8 +54,8 @@ public: void SetFrequencyReg(uint32_t freq, SX12XX_Radio_Number_t radioNumber = SX12XX_Radio_All); bool FrequencyErrorAvailable() const { return true; } int32_t GetFrequencyError(); - bool GetFrequencyErrorbool(); - void SetPPMoffsetReg(int32_t offset); + bool GetFrequencyErrorbool(SX12XX_Radio_Number_t radioNumber); + void SetPPMoffsetReg(int32_t offset, SX12XX_Radio_Number_t radioNumber); //////////////////////////////////////////////////// diff --git a/src/lib/SX1280Driver/SX1280.cpp b/src/lib/SX1280Driver/SX1280.cpp index 0d902d38..60927eb7 100644 --- a/src/lib/SX1280Driver/SX1280.cpp +++ b/src/lib/SX1280Driver/SX1280.cpp @@ -570,10 +570,10 @@ void ICACHE_RAM_ATTR SX1280Driver::GetStatus(SX12XX_Radio_Number_t radioNumber) DBGLN("Status: %x, %x, %x", (0b11100000 & status) >> 5, (0b00011100 & status) >> 2, 0b00000001 & status); } -bool ICACHE_RAM_ATTR SX1280Driver::GetFrequencyErrorbool() +bool ICACHE_RAM_ATTR SX1280Driver::GetFrequencyErrorbool(SX12XX_Radio_Number_t radioNumber) { // Only need the highest bit of the 20-bit FEI to determine the direction - uint8_t feiMsb = hal.ReadRegister(SX1280_REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB, lastSuccessfulPacketRadio); + uint8_t feiMsb = hal.ReadRegister(SX1280_REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB, radioNumber); // fei & (1 << 19) and flip sign if IQinverted if (feiMsb & 0x08) return IQinverted; diff --git a/src/lib/SX1280Driver/SX1280.h b/src/lib/SX1280Driver/SX1280.h index 40caea86..df85d2f6 100644 --- a/src/lib/SX1280Driver/SX1280.h +++ b/src/lib/SX1280Driver/SX1280.h @@ -37,7 +37,7 @@ public: void startCWTest(uint32_t freq, SX12XX_Radio_Number_t radioNumber); - bool GetFrequencyErrorbool(); + 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); diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp index 8bdf8f26..b5c8c131 100644 --- a/src/src/rx_main.cpp +++ b/src/src/rx_main.cpp @@ -570,10 +570,10 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() return true; } -int32_t ICACHE_RAM_ATTR HandleFreqCorr(bool value) +int32_t ICACHE_RAM_ATTR HandleFreqCorr(bool value, SX12XX_Radio_Number_t radio) { int32_t tempFC = FreqCorrection; - if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_2) + if (radio == SX12XX_Radio_2) { tempFC = FreqCorrection_2; } @@ -601,7 +601,7 @@ int32_t ICACHE_RAM_ATTR HandleFreqCorr(bool value) } } - if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1) + if (radio == SX12XX_Radio_1) { FreqCorrection = tempFC; } @@ -1171,17 +1171,29 @@ bool ICACHE_RAM_ATTR ProcessRFPacket(SX12xxDriverCommon::rx_status const status) Radio.GetLastPacketStats(); getRFlinkInfo(); + // Adjusts FreqCorrection for RX freq offset if (Radio.FrequencyErrorAvailable()) { #if defined(RADIO_SX127X) - // Adjusts FreqCorrection for RX freq offset - int32_t tempFreqCorrection = HandleFreqCorr(Radio.GetFrequencyErrorbool()); + int32_t tempFreqCorrection = HandleFreqCorr(Radio.GetFrequencyErrorbool(Radio.GetProcessingPacketRadio()), Radio.GetProcessingPacketRadio()); // Teamp900 also needs to adjust its demood PPM - Radio.SetPPMoffsetReg(tempFreqCorrection); - #else /* !RADIO_SX127X */ - // Adjusts FreqCorrection for RX freq offset - HandleFreqCorr(Radio.GetFrequencyErrorbool()); - #endif /* RADIO_SX127X */ + Radio.SetPPMoffsetReg(tempFreqCorrection, Radio.GetProcessingPacketRadio()); + + if (Radio.hasSecondRadioGotData) + { + SX12XX_Radio_Number_t secondRadio = Radio.GetProcessingPacketRadio() == SX12XX_Radio_1 ? SX12XX_Radio_2 : SX12XX_Radio_1; + tempFreqCorrection = HandleFreqCorr(Radio.GetFrequencyErrorbool(secondRadio), secondRadio); + Radio.SetPPMoffsetReg(tempFreqCorrection, secondRadio); + } + #else + HandleFreqCorr(Radio.GetFrequencyErrorbool(Radio.GetProcessingPacketRadio()), Radio.GetProcessingPacketRadio()); + + if (Radio.hasSecondRadioGotData) + { + SX12XX_Radio_Number_t secondRadio = Radio.GetProcessingPacketRadio() == SX12XX_Radio_1 ? SX12XX_Radio_2 : SX12XX_Radio_1; + HandleFreqCorr(Radio.GetFrequencyErrorbool(secondRadio), secondRadio); + } + #endif } // Received a packet, that's the definition of LQ -- 2.11.4.GIT