Skip to content

Commit eb376dc

Browse files
CapnBrymha1pkendall64
authored
Add OtaNonce to OtaCrcInitializer (ExpressLRS#3294)
* initial commit * Adjust RX TOCK to later in period for low duty cycle rates * Move all FHSS to TOCK only * Extend TXs telemetry timeout for short TOA packets * Remove incorrect comment --------- Co-authored-by: Michael <101.37584@germanynet.de> Co-authored-by: Michael <mha1@users.noreply.github.com> Co-authored-by: Paul Kendall <pkendall64@gmail.com>
1 parent a1310d7 commit eb376dc

File tree

9 files changed

+61
-82
lines changed

9 files changed

+61
-82
lines changed

src/lib/LR1121Driver/LR1121.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,7 @@ void LR1121Driver::startCWTest(uint32_t freq, SX12XX_Radio_Number_t radioNumber)
136136
}
137137

138138
void LR1121Driver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t regfreq,
139-
uint8_t PreambleLength, bool InvertIQ, uint8_t _PayloadLength, uint32_t interval,
139+
uint8_t PreambleLength, bool InvertIQ, uint8_t _PayloadLength, uint32_t rxtimeout,
140140
bool setFSKModulation, uint8_t fskSyncWord1, uint8_t fskSyncWord2,
141141
SX12XX_Radio_Number_t radioNumber)
142142
{
@@ -158,7 +158,7 @@ void LR1121Driver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t regfreq,
158158
inverted = LR11XX_RADIO_LORA_IQ_STANDARD;
159159
}
160160

161-
SetRxTimeoutUs(interval);
161+
SetRxTimeoutUs(rxtimeout);
162162

163163
SetMode(LR1121_MODE_STDBY_RC, radioNumber);
164164

src/lib/LR1121Driver/LR1121.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ class LR1121Driver: public SX12xxDriverCommon
3333
void End();
3434
void SetTxIdleMode() { SetMode(LR1121_MODE_FS, SX12XX_Radio_All); }; // set Idle mode used when switching from RX to TX
3535
void Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq,
36-
uint8_t PreambleLength, bool InvertIQ, uint8_t PayloadLength, uint32_t interval, bool setFSKModulation,
36+
uint8_t PreambleLength, bool InvertIQ, uint8_t PayloadLength, uint32_t rxtimeout, bool setFSKModulation,
3737
uint8_t fskSyncWord1, uint8_t fskSyncWord2, SX12XX_Radio_Number_t radioNumber = SX12XX_Radio_All);
3838
void SetFrequencyHz(uint32_t freq, SX12XX_Radio_Number_t radioNumber);
3939
void SetFrequencyReg(uint32_t freq, SX12XX_Radio_Number_t radioNumber = SX12XX_Radio_All);

src/lib/OTA/OTA.cpp

Lines changed: 20 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,15 @@ GeneratePacketCrc_t OtaGeneratePacketCrc;
2727

2828
void OtaUpdateCrcInitFromUid()
2929
{
30+
#if OTA_VERSION_ID > 15
31+
#error "OTA version can't be > 15"
32+
#endif
33+
3034
OtaCrcInitializer = (UID[4] << 8) | UID[5];
31-
OtaCrcInitializer ^= OTA_VERSION_ID;
35+
36+
// shift OTA_VERSION_ID to the high byte to leave room for
37+
// xor-ing in the nonce in the GenerateCRC and ValidateCRC function
38+
OtaCrcInitializer ^= (uint16_t)OTA_VERSION_ID << 8;
3239
}
3340

3441
static inline uint8_t ICACHE_RAM_ATTR HybridWideNonceToSwitchIndex(uint8_t const nonce)
@@ -455,51 +462,36 @@ bool ICACHE_RAM_ATTR UnpackChannelData8ch(OTA_Packet_s const * const otaPktPtr,
455462

456463
bool ICACHE_RAM_ATTR ValidatePacketCrcFull(OTA_Packet_s * const otaPktPtr)
457464
{
465+
uint16_t nonceValidator = (otaPktPtr->std.type == PACKET_TYPE_SYNC) ? 0 : OtaNonce;
458466
uint16_t const calculatedCRC =
459-
ota_crc.calc((uint8_t*)otaPktPtr, OTA8_CRC_CALC_LEN, OtaCrcInitializer);
467+
ota_crc.calc((uint8_t*)otaPktPtr, OTA8_CRC_CALC_LEN, OtaCrcInitializer ^ nonceValidator);
460468
return otaPktPtr->full.crc == calculatedCRC;
461469
}
462470

463471
bool ICACHE_RAM_ATTR ValidatePacketCrcStd(OTA_Packet_s * const otaPktPtr)
464472
{
465-
uint8_t backupCrcHigh = otaPktPtr->std.crcHigh;
466-
467473
uint16_t const inCRC = ((uint16_t)otaPktPtr->std.crcHigh << 8) + otaPktPtr->std.crcLow;
468-
// For smHybrid the CRC only has the packet type in byte 0
469-
// For smWide the FHSS slot is added to the CRC in byte 0 on PACKET_TYPE_RCDATAs
470-
#if defined(TARGET_RX)
471-
if (otaPktPtr->std.type == PACKET_TYPE_RCDATA && OtaSwitchModeCurrent == smWideOr8ch)
472-
{
473-
otaPktPtr->std.crcHigh = (OtaNonce % ExpressLRS_currAirRate_Modparams->FHSShopInterval) + 1;
474-
}
475-
else
476-
#endif
477-
{
478-
otaPktPtr->std.crcHigh = 0;
479-
}
480-
uint16_t const calculatedCRC =
481-
ota_crc.calc((uint8_t*)otaPktPtr, OTA4_CRC_CALC_LEN, OtaCrcInitializer);
482474

483-
otaPktPtr->std.crcHigh = backupCrcHigh;
475+
// Zero the crcHigh bits, as the CRC is calculated before it is ORed in
476+
otaPktPtr->std.crcHigh = 0;
477+
478+
uint16_t nonceValidator = (otaPktPtr->std.type == PACKET_TYPE_SYNC) ? 0 : OtaNonce;
479+
uint16_t const calculatedCRC =
480+
ota_crc.calc((uint8_t*)otaPktPtr, OTA4_CRC_CALC_LEN, OtaCrcInitializer ^ nonceValidator);
484481

485482
return inCRC == calculatedCRC;
486483
}
487484

488485
void ICACHE_RAM_ATTR GeneratePacketCrcFull(OTA_Packet_s * const otaPktPtr)
489486
{
490-
otaPktPtr->full.crc = ota_crc.calc((uint8_t*)otaPktPtr, OTA8_CRC_CALC_LEN, OtaCrcInitializer);
487+
uint16_t nonceValidator = (otaPktPtr->std.type == PACKET_TYPE_SYNC) ? 0 : OtaNonce;
488+
otaPktPtr->full.crc = ota_crc.calc((uint8_t*)otaPktPtr, OTA8_CRC_CALC_LEN, OtaCrcInitializer ^ nonceValidator);
491489
}
492490

493491
void ICACHE_RAM_ATTR GeneratePacketCrcStd(OTA_Packet_s * const otaPktPtr)
494492
{
495-
#if defined(TARGET_TX)
496-
// artificially inject the low bits of the nonce on data packets, this will be overwritten with the CRC after it's calculated
497-
if (otaPktPtr->std.type == PACKET_TYPE_RCDATA && OtaSwitchModeCurrent == smWideOr8ch)
498-
{
499-
otaPktPtr->std.crcHigh = (OtaNonce % ExpressLRS_currAirRate_Modparams->FHSShopInterval) + 1;
500-
}
501-
#endif
502-
uint16_t crc = ota_crc.calc((uint8_t*)otaPktPtr, OTA4_CRC_CALC_LEN, OtaCrcInitializer);
493+
uint16_t nonceValidator = (otaPktPtr->std.type == PACKET_TYPE_SYNC) ? 0 : OtaNonce;
494+
uint16_t crc = ota_crc.calc((uint8_t*)otaPktPtr, OTA4_CRC_CALC_LEN, OtaCrcInitializer ^ nonceValidator);
503495
otaPktPtr->std.crcHigh = (crc >> 8);
504496
otaPktPtr->std.crcLow = crc;
505497
}

src/lib/SX127xDriver/SX127x.cpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -618,20 +618,20 @@ void ICACHE_RAM_ATTR SX127xDriver::SetMode(SX127x_RadioOPmodes mode, SX12XX_Radi
618618
currOpmode = mode;
619619
}
620620

621-
void SX127xDriver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, bool InvertIQ, uint8_t _PayloadLength, uint32_t interval)
621+
void SX127xDriver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, bool InvertIQ, uint8_t _PayloadLength, uint32_t rxtimeout)
622622
{
623-
Config(bw, sf, cr, freq, preambleLen, currSyncWord, InvertIQ, _PayloadLength, interval);
623+
Config(bw, sf, cr, freq, preambleLen, currSyncWord, InvertIQ, _PayloadLength, rxtimeout);
624624
}
625625

626-
void SX127xDriver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, uint8_t syncWord, bool InvertIQ, uint8_t _PayloadLength, uint32_t interval)
626+
void SX127xDriver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, uint8_t syncWord, bool InvertIQ, uint8_t _PayloadLength, uint32_t rxtimeout)
627627
{
628628
PayloadLength = _PayloadLength;
629629
ConfigLoraDefaults();
630630
SetPreambleLength(preambleLen);
631631
SetSpreadingFactor((SX127x_SpreadingFactor)sf);
632632
SetBandwidthCodingRate((SX127x_Bandwidth)bw, (SX127x_CodingRate)cr);
633633
SetFrequencyReg(freq);
634-
SetRxTimeoutUs(interval);
634+
SetRxTimeoutUs(rxtimeout);
635635
}
636636

637637
uint32_t ICACHE_RAM_ATTR SX127xDriver::GetCurrBandwidth()

src/lib/SX127xDriver/SX127x.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,8 @@ class SX127xDriver: public SX12xxDriverCommon
2929
bool Begin(uint32_t minimumFrequency, uint32_t maximumFrequency);
3030
void End();
3131
bool DetectChip(SX12XX_Radio_Number_t radioNumber);
32-
void Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, uint8_t syncWord, bool InvertIQ, uint8_t _PayloadLength, uint32_t interval);
33-
void Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, bool InvertIQ, uint8_t _PayloadLength, uint32_t interval);
32+
void Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, uint8_t syncWord, bool InvertIQ, uint8_t _PayloadLength, uint32_t rxtimeout);
33+
void Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq, uint8_t preambleLen, bool InvertIQ, uint8_t _PayloadLength, uint32_t rxtimeout);
3434
void SetMode(SX127x_RadioOPmodes mode, SX12XX_Radio_Number_t radioNumber);
3535
void SetTxIdleMode() { SetMode(SX127x_OPMODE_STANDBY, SX12XX_Radio_All); } // set Idle mode used when switching from RX to TX
3636
void ConfigLoraDefaults();

src/lib/SX1280Driver/SX1280.cpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,7 @@ void SX1280Driver::startCWTest(uint32_t freq, SX12XX_Radio_Number_t radioNumber)
145145
}
146146

147147
void SX1280Driver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t regfreq,
148-
uint8_t PreambleLength, bool InvertIQ, uint8_t _PayloadLength, uint32_t interval,
148+
uint8_t PreambleLength, bool InvertIQ, uint8_t _PayloadLength, uint32_t rxtimeout,
149149
uint32_t flrcSyncWord, uint16_t flrcCrcSeed, uint8_t flrc)
150150
{
151151
uint8_t const mode = (flrc) ? SX1280_PACKET_TYPE_FLRC : SX1280_PACKET_TYPE_LORA;
@@ -173,7 +173,7 @@ void SX1280Driver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t regfreq,
173173
SetPacketParamsLoRa(PreambleLength, packetLengthType, _PayloadLength, InvertIQ);
174174
}
175175
SetFrequencyReg(regfreq);
176-
SetRxTimeoutUs(interval);
176+
SetRxTimeoutUs(rxtimeout);
177177

178178
uint16_t dio1Mask = SX1280_IRQ_TX_DONE | SX1280_IRQ_RX_DONE;
179179
uint16_t irqMask = SX1280_IRQ_TX_DONE | SX1280_IRQ_RX_DONE | SX1280_IRQ_SYNCWORD_VALID | SX1280_IRQ_SYNCWORD_ERROR | SX1280_IRQ_CRC_ERROR;
@@ -471,7 +471,7 @@ void ICACHE_RAM_ATTR SX1280Driver::TXnbISR()
471471
void ICACHE_RAM_ATTR SX1280Driver::TXnb(uint8_t * data, uint8_t size, bool sendGeminiBuffer, uint8_t * dataGemini, SX12XX_Radio_Number_t radioNumber)
472472
{
473473
transmittingRadio = radioNumber;
474-
474+
475475
//catch TX timeout
476476
if (currOpmode == SX1280_MODE_TX)
477477
{
@@ -523,7 +523,7 @@ void ICACHE_RAM_ATTR SX1280Driver::TXnb(uint8_t * data, uint8_t size, bool sendG
523523
{
524524
hal.WriteBuffer(0x00, data, size, radioNumber);
525525
}
526-
526+
527527
instance->SetMode(SX1280_MODE_TX, radioNumber);
528528

529529
#ifdef DEBUG_SX1280_OTA_TIMING

src/lib/SX1280Driver/SX1280.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ class SX1280Driver: public SX12xxDriverCommon
2828
void End();
2929
void SetTxIdleMode() { SetMode(SX1280_MODE_FS, SX12XX_Radio_All); }; // set Idle mode used when switching from RX to TX
3030
void Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t freq,
31-
uint8_t PreambleLength, bool InvertIQ, uint8_t PayloadLength, uint32_t interval,
31+
uint8_t PreambleLength, bool InvertIQ, uint8_t PayloadLength, uint32_t rxtimeout,
3232
uint32_t flrcSyncWord=0, uint16_t flrcCrcSeed=0, uint8_t flrc=0);
3333
void SetFrequencyHz(uint32_t freq, SX12XX_Radio_Number_t radioNumber);
3434
void SetFrequencyReg(uint32_t freq, SX12XX_Radio_Number_t radioNumber = SX12XX_Radio_All);

src/src/rx_main.cpp

Lines changed: 22 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -168,9 +168,7 @@ bool doStartTimer = false;
168168

169169
///////////////////////////////////////////////
170170

171-
bool didFHSS = false;
172-
bool alreadyFHSS = false;
173-
bool alreadyTLMresp = false;
171+
static bool alreadyTLMresp = false;
174172

175173
//////////////////////////////////////////////////////////////
176174

@@ -369,20 +367,19 @@ void SetRFLinkRate(uint8_t index, bool bindMode) // Set speed of RF link
369367
EnableLBT();
370368
}
371369

372-
bool ICACHE_RAM_ATTR HandleFHSS()
370+
static void ICACHE_RAM_ATTR HandleFHSS()
373371
{
374-
uint8_t modresultFHSS = (OtaNonce + 1) % ExpressLRS_currAirRate_Modparams->FHSShopInterval;
372+
uint8_t modresultFHSS = OtaNonce % ExpressLRS_currAirRate_Modparams->FHSShopInterval;
375373

376-
if ((ExpressLRS_currAirRate_Modparams->FHSShopInterval == 0) || alreadyFHSS == true || InBindingMode || (modresultFHSS != 0) || (connectionState == disconnected))
374+
if ((ExpressLRS_currAirRate_Modparams->FHSShopInterval == 0) || InBindingMode || (modresultFHSS != 0) || (connectionState == disconnected))
377375
{
378-
return false;
376+
return;
379377
}
380378

381-
alreadyFHSS = true;
382-
383379
if (geminiMode)
384380
{
385-
if ((((OtaNonce + 1)/ExpressLRS_currAirRate_Modparams->FHSShopInterval) % 2 == 0) || FHSSuseDualBand) // When in DualBand do not switch between radios. The OTA modulation parameters and HighFreq/LowFreq Tx amps are set during Config.
381+
if (((OtaNonce / ExpressLRS_currAirRate_Modparams->FHSShopInterval) % 2 == 0) || FHSSuseDualBand) // When in DualBand do not switch between radios. The OTA modulation paramters and HighFreq/LowFreq Tx amps are set during Config.
382+
386383
{
387384
Radio.SetFrequencyReg(FHSSgetNextFreq(), SX12XX_Radio_1);
388385
Radio.SetFrequencyReg(FHSSgetGeminiFreq(), SX12XX_Radio_2);
@@ -402,7 +399,7 @@ bool ICACHE_RAM_ATTR HandleFHSS()
402399

403400
#if defined(RADIO_SX127X)
404401
// SX127x radio has to reset receive mode after hopping
405-
uint8_t modresultTLM = (OtaNonce + 1) % ExpressLRS_currTlmDenom;
402+
uint8_t modresultTLM = OtaNonce % ExpressLRS_currTlmDenom;
406403
if (modresultTLM != 0 || ExpressLRS_currTlmDenom == 1) // if we are about to send a tlm response don't bother going back to rx
407404
{
408405
Radio.RXnb();
@@ -411,7 +408,6 @@ bool ICACHE_RAM_ATTR HandleFHSS()
411408
#if defined(Regulatory_Domain_EU_CE_2400)
412409
SetClearChannelAssessmentTime();
413410
#endif
414-
return true;
415411
}
416412

417413
void ICACHE_RAM_ATTR LinkStatsToOta(OTA_LinkStats_s * const ls)
@@ -441,7 +437,7 @@ void ICACHE_RAM_ATTR LinkStatsToOta(OTA_LinkStats_s * const ls)
441437

442438
bool ICACHE_RAM_ATTR HandleSendTelemetryResponse()
443439
{
444-
uint8_t modresult = (OtaNonce + 1) % ExpressLRS_currTlmDenom;
440+
uint8_t modresult = OtaNonce % ExpressLRS_currTlmDenom;
445441

446442
if ((connectionState == disconnected) || (ExpressLRS_currTlmDenom == 1) || (alreadyTLMresp == true) || (modresult != 0) || !teamraceHasModelMatch)
447443
{
@@ -605,7 +601,7 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse()
605601
// Gemini flips frequencies between radios on the rx side only. This is to help minimise antenna cross polarization.
606602
// The payloads need to be switch when this happens.
607603
// GemX does not switch due to the time required to reconfigure the LR1121 params.
608-
if (((OtaNonce + 1)/ExpressLRS_currAirRate_Modparams->FHSShopInterval) % 2 == 0 || !sendGeminiBuffer || FHSSuseDualBand)
604+
if ((OtaNonce/ExpressLRS_currAirRate_Modparams->FHSShopInterval) % 2 == 0 || !sendGeminiBuffer || FHSSuseDualBand)
609605
{
610606
Radio.TXnb((uint8_t*)&otaPkt, ExpressLRS_currAirRate_Modparams->PayloadLength, sendGeminiBuffer, (uint8_t*)&otaPktGemini, transmittingRadio);
611607
}
@@ -678,10 +674,8 @@ void ICACHE_RAM_ATTR updatePhaseLock()
678674

679675
if (RXtimerState == tim_locked)
680676
{
681-
// limit rate of freq offset adjustment, use slot 1
682-
// because telemetry can fall on slot 1 and will
683-
// never get here
684-
if (OtaNonce % 8 == 1)
677+
// limit rate of freq offset adjustment
678+
if (OtaNonce % 8 == 0)
685679
{
686680
if (Offset > 0)
687681
{
@@ -712,15 +706,6 @@ void ICACHE_RAM_ATTR updatePhaseLock()
712706

713707
void ICACHE_RAM_ATTR HWtimerCallbackTick() // this is 180 out of phase with the other callback, occurs mid-packet reception
714708
{
715-
updatePhaseLock();
716-
OtaNonce++;
717-
718-
// if (!alreadyTLMresp && !alreadyFHSS && !LQCalc.currentIsSet()) // packet timeout AND didn't DIDN'T just hop or send TLM
719-
// {
720-
// Radio.RXnb(); // put the radio cleanly back into RX in case of garbage data
721-
// }
722-
723-
724709
if (ExpressLRS_currAirRate_Modparams->numOfSends == 1)
725710
{
726711
// Save the LQ value before the inc() reduces it by 1
@@ -738,7 +723,6 @@ void ICACHE_RAM_ATTR HWtimerCallbackTick() // this is 180 out of phase with the
738723
LQCalc.inc();
739724

740725
alreadyTLMresp = false;
741-
alreadyFHSS = false;
742726
}
743727

744728
//////////////////////////////////////////////////////////////
@@ -857,14 +841,11 @@ void ICACHE_RAM_ATTR HWtimerCallbackTock()
857841
// For any serial drivers that need to send on a regular cadence (i.e. CRSF to betaflight)
858842
sendImmediateRC();
859843

860-
if (!didFHSS)
861-
{
862-
HandleFHSS();
863-
}
864-
didFHSS = false;
865-
844+
OtaNonce++;
845+
HandleFHSS();
866846
updateDiversity();
867847
tlmSent = HandleSendTelemetryResponse();
848+
updatePhaseLock();
868849

869850
#if defined(DEBUG_RX_SCOREBOARD)
870851
static bool lastPacketWasTelemetry = false;
@@ -890,7 +871,6 @@ void LostConnection(bool resumeRx)
890871
LPF_Offset.init(0);
891872
LPF_OffsetDx.init(0);
892873
alreadyTLMresp = false;
893-
alreadyFHSS = false;
894874

895875
if (!InBindingMode)
896876
{
@@ -1148,7 +1128,7 @@ static bool ICACHE_RAM_ATTR ProcessRfPacket_SYNC(uint32_t const now, OTA_Sync_s
11481128
|| FHSSgetCurrIndex() != otaSync->fhssIndex
11491129
|| connectionHasModelMatch != modelMatched)
11501130
{
1151-
//DBGLN("\r\n%ux%ux%u", OtaNonce, otaPktPtr->sync.nonce, otaPktPtr->sync.fhssIndex);
1131+
//DBGLN("\r\n%ux%ux%u", OtaNonce, otaSync->nonce, otaSync->fhssIndex);
11521132
FHSSsetCurrIndex(otaSync->fhssIndex);
11531133
OtaNonce = otaSync->nonce;
11541134
TentativeConnection(now);
@@ -1184,7 +1164,12 @@ bool ICACHE_RAM_ATTR ProcessRFPacket(SX12xxDriverCommon::rx_status const status)
11841164
return false;
11851165
}
11861166

1187-
PFDloop.extEvent(beginProcessing + PACKET_TO_TOCK_SLACK);
1167+
// The extEvent defines where TOCK timer ISR is to be synced to, i.e. where the packet period begins.
1168+
// For rates where the TOA is longer than half the packet period schedule the TOCK for rougly 1x TOA before
1169+
// the TX's end of the period so telemetry is received by the TX in the correct period. For all others,
1170+
// schedule TOCK to be PACKET_TO_TOCK_SLACK (us) after RX packet reception.
1171+
int32_t slack = std::max(ExpressLRS_currAirRate_Modparams->interval - 2 * ExpressLRS_currAirRate_RFperfParams->TOA, (int32_t)PACKET_TO_TOCK_SLACK);
1172+
PFDloop.extEvent(beginProcessing + slack);
11881173

11891174
doStartTimer = false;
11901175
unsigned long now = millis();
@@ -1275,8 +1260,6 @@ bool ICACHE_RAM_ATTR RXdoneISR(SX12xxDriverCommon::rx_status const status)
12751260

12761261
if (ProcessRFPacket(status))
12771262
{
1278-
didFHSS = HandleFHSS();
1279-
12801263
if (doStartTimer)
12811264
{
12821265
doStartTimer = false;

src/src/tx_main.cpp

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -508,8 +508,12 @@ void SetRFLinkRate(uint8_t index) // Set speed of RF link
508508
FHSSusePrimaryFreqBand = !(ModParams->radio_type == RADIO_TYPE_LR1121_LORA_2G4) && !(ModParams->radio_type == RADIO_TYPE_LR1121_GFSK_2G4);
509509
FHSSuseDualBand = ModParams->radio_type == RADIO_TYPE_LR1121_LORA_DUAL;
510510

511+
// timeout is used to take the RF out of receive mode during a telemetry slot before the end of the period,
512+
// reducing turnaround time on the following TX. For packet rates with TOA < half interval, extend the timeout
513+
// as it begins at the end of the TX, not from TOCK
514+
uint32_t timeout = std::max(interval, interval * 3 / 2 - RFperf->TOA);
511515
Radio.Config(ModParams->bw, ModParams->sf, ModParams->cr, FHSSgetInitialFreq(),
512-
ModParams->PreambleLen, invertIQ, ModParams->PayloadLength, ModParams->interval
516+
ModParams->PreambleLen, invertIQ, ModParams->PayloadLength, timeout
513517
#if defined(RADIO_SX128X)
514518
, uidMacSeedGet(), OtaCrcInitializer, (ModParams->radio_type == RADIO_TYPE_SX128x_FLRC)
515519
#endif

0 commit comments

Comments
 (0)