src/src/rx_main.cpp

   1 #include "rxtx_common.h"
   2 #include "LowPassFilter.h"
   3
   4 #include "crc.h"
   5 #include "telemetry_protocol.h"
   6 #include "telemetry.h"
   7 #include "stubborn_sender.h"
   8 #include "stubborn_receiver.h"
   9
  10 #include "lua.h"
  11 #include "msp.h"
  12 #include "msptypes.h"
  13 #include "PFD.h"
  14 #include "options.h"
  15 #include "dynpower.h"
  16 #include "MeanAccumulator.h"
  17 #include "freqTable.h"
  18
  19 #include "rx-serial/SerialIO.h"
  20 #include "rx-serial/SerialNOOP.h"
  21 #include "rx-serial/SerialCRSF.h"
  22 #include "rx-serial/SerialSBUS.h"
  23 #include "rx-serial/SerialSUMD.h"
  24 #include "rx-serial/SerialAirPort.h"
  25 #include "rx-serial/SerialHoTT_TLM.h"
  26 #include "rx-serial/SerialMavlink.h"
  27 #include "rx-serial/SerialTramp.h"
  28 #include "rx-serial/SerialSmartAudio.h"
  29
  30 #include "rx-serial/devSerialIO.h"
  31 #include "devLED.h"
  32 #include "devLUA.h"
  33 #include "devWIFI.h"
  34 #include "devButton.h"
  35 #include "devServoOutput.h"
  36 #include "devVTXSPI.h"
  37 #include "devAnalogVbat.h"
  38 #include "devSerialUpdate.h"
  39 #include "devBaro.h"
  40 #include "devMSPVTX.h"
  41 #include "devThermal.h"
  42
  43 #if defined(PLATFORM_ESP8266)
  44 #include <user_interface.h>
  45 #include <FS.h>
  46 #elif defined(PLATFORM_ESP32)
  47 #include <SPIFFS.h>
  48 #include "esp_task_wdt.h"
  49 #endif
  50
  51 //
  52 // Code encapsulated by the ARDUINO_CORE_INVERT_FIX #ifdef temporarily fixes EpressLRS issue #2609 which is caused
  53 // by the Arduino core (see https://github.com/espressif/arduino-esp32/issues/9896) and fixed
  54 // by Espressif with Arduino core release 3.0.3 (see https://github.com/espressif/arduino-esp32/pull/9950)
  55 //
  56 // With availability of Arduino core 3.0.3 and upgrading ExpressLRS to Arduino core 3.0.3 the temporary fix
  57 // should be deleted again
  58 //
  59 // ARDUINO_CORE_INVERT_FIX PT1
  60 #define ARDUINO_CORE_INVERT_FIX
  61
  62 #if defined(PLATFORM_ESP32) && defined(ARDUINO_CORE_INVERT_FIX)
  63 #include "driver/uart.h"
  64 #endif
  65 // ARDUINO_CORE_INVERT_FIX PT1 end
  66
  67 ///LUA///
  68 #define LUA_MAX_PARAMS 32
  69 ////
  70
  71 //// CONSTANTS ////
  72 #define SEND_LINK_STATS_TO_FC_INTERVAL 100
  73 #define DIVERSITY_ANTENNA_INTERVAL 5
  74 #define DIVERSITY_ANTENNA_RSSI_TRIGGER 5
  75 #define PACKET_TO_TOCK_SLACK 200 // Desired buffer time between Packet ISR and Tock ISR
  76 ///////////////////
  77
  78 device_affinity_t ui_devices[] = {
  79   {&Serial0_device, 1},
  80 #if defined(PLATFORM_ESP32)
  81   {&Serial1_device, 1},
  82 #endif
  83 #if defined(PLATFORM_ESP32)
  84   {&SerialUpdate_device, 1},
  85 #endif
  86 #ifdef HAS_LED
  87   {&LED_device, 0},
  88 #endif
  89   {&LUA_device, 0},
  90 #ifdef HAS_RGB
  91   {&RGB_device, 0},
  92 #endif
  93 #ifdef HAS_WIFI
  94   {&WIFI_device, 0},
  95 #endif
  96 #ifdef HAS_BUTTON
  97   {&Button_device, 0},
  98 #endif
  99 #ifdef HAS_VTX_SPI
 100   {&VTxSPI_device, 0},
 101 #endif
 102 #ifdef USE_ANALOG_VBAT
 103   {&AnalogVbat_device, 0},
 104 #endif
 105 #ifdef HAS_SERVO_OUTPUT
 106   {&ServoOut_device, 1},
 107 #endif
 108 #ifdef HAS_BARO
 109   {&Baro_device, 0}, // must come after AnalogVbat_device to slow updates
 110 #endif
 111 #ifdef HAS_MSP_VTX
 112   {&MSPVTx_device, 0}, // dependency on VTxSPI_device
 113 #endif
 114 #if defined(HAS_THERMAL) || defined(HAS_FAN)
 115   {&Thermal_device, 0},
 116 #endif
 117 };
 118
 119 uint8_t antenna = 0;    // which antenna is currently in use
 120 uint8_t geminiMode = 0;
 121
 122 PFD PFDloop;
 123 Crc2Byte ota_crc;
 124 ELRS_EEPROM eeprom;
 125 RxConfig config;
 126 Telemetry telemetry;
 127 Stream *SerialLogger;
 128 bool hardwareConfigured = true;
 129
 130 #if defined(USE_MSP_WIFI)
 131 #include "crsf2msp.h"
 132 #include "msp2crsf.h"
 133
 134 CROSSFIRE2MSP crsf2msp;
 135 MSP2CROSSFIRE msp2crsf;
 136 #endif
 137
 138 #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
 139 unsigned long rebootTime = 0;
 140 extern bool webserverPreventAutoStart;
 141 bool pwmSerialDefined = false;
 142 #endif
 143 uint32_t serialBaud;
 144
 145 /* SERIAL_PROTOCOL_TX is used by CRSF output */
 146 #if defined(TARGET_RX_FM30_MINI)
 147     HardwareSerial SERIAL_PROTOCOL_TX(USART2);
 148 #elif defined(TARGET_DIY_900_RX_STM32)
 149     HardwareSerial SERIAL_PROTOCOL_TX(USART1);
 150 #else
 151     #define SERIAL_PROTOCOL_TX Serial
 152
 153     #if defined(PLATFORM_ESP32)
 154         #define SERIAL1_PROTOCOL_TX Serial1
 155
 156         // SBUS driver needs to distinguish stream for SBUS/DJI protocol
 157         const Stream *serial_protocol_tx = &(SERIAL_PROTOCOL_TX);
 158         const Stream *serial1_protocol_tx = &(SERIAL1_PROTOCOL_TX);
 159
 160         SerialIO *serial1IO = nullptr;
 161     #endif
 162 #endif
 163
 164 SerialIO *serialIO = nullptr;
 165
 166 /* SERIAL_PROTOCOL_RX is used by telemetry receiver and can be on a different peripheral */
 167 #if defined(TARGET_RX_GHOST_ATTO_V1) /* !TARGET_RX_GHOST_ATTO_V1 */
 168     #define SERIAL_PROTOCOL_RX CrsfRxSerial
 169     HardwareSerial CrsfRxSerial(USART1, HALF_DUPLEX_ENABLED);
 170 #elif defined(TARGET_R9SLIMPLUS_RX) /* !TARGET_R9SLIMPLUS_RX */
 171     #define SERIAL_PROTOCOL_RX CrsfRxSerial
 172     HardwareSerial CrsfRxSerial(USART3);
 173 #elif defined(TARGET_RX_FM30_MINI)
 174     #define SERIAL_PROTOCOL_RX SERIAL_PROTOCOL_TX
 175 #else
 176     #define SERIAL_PROTOCOL_RX Serial
 177     #define SERIAL1_PROTOCOL_RX Serial1
 178 #endif
 179
 180 StubbornSender TelemetrySender;
 181 static uint8_t telemetryBurstCount;
 182 static uint8_t telemetryBurstMax;
 183
 184 StubbornReceiver MspReceiver;
 185 uint8_t MspData[ELRS_MSP_BUFFER];
 186
 187 uint8_t mavlinkSSBuffer[CRSF_MAX_PACKET_LEN]; // Buffer for current stubbon sender packet (mavlink only)
 188
 189 static bool tlmSent = false;
 190 static uint8_t NextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
 191 static bool telemBurstValid;
 192 /// PFD Filters ////////////////
 193 LPF LPF_Offset(2);
 194 LPF LPF_OffsetDx(4);
 195
 196 /// LQ/RSSI/SNR Calculation //////////
 197 LQCALC<100> LQCalc;
 198 LQCALC<100> LQCalcDVDA;
 199 uint8_t uplinkLQ;
 200 LPF LPF_UplinkRSSI0(5);  // track rssi per antenna
 201 LPF LPF_UplinkRSSI1(5);
 202 MeanAccumulator<int32_t, int8_t, -16> SnrMean;
 203
 204 static uint8_t scanIndex;
 205 uint8_t ExpressLRS_nextAirRateIndex;
 206 int8_t SwitchModePending;
 207
 208 int32_t PfdPrevRawOffset;
 209 RXtimerState_e RXtimerState;
 210 uint32_t GotConnectionMillis = 0;
 211 const uint32_t ConsiderConnGoodMillis = 1000; // minimum time before we can consider a connection to be 'good'
 212 bool doStartTimer = false;
 213
 214 ///////////////////////////////////////////////
 215
 216 bool didFHSS = false;
 217 bool alreadyFHSS = false;
 218 bool alreadyTLMresp = false;
 219
 220 //////////////////////////////////////////////////////////////
 221
 222 ///////Variables for Telemetry and Link Quality///////////////
 223 uint32_t LastValidPacket = 0;           //Time the last valid packet was recv
 224 uint32_t LastSyncPacket = 0;            //Time the last valid packet was recv
 225
 226 static uint32_t SendLinkStatstoFCintervalLastSent;
 227 static uint8_t SendLinkStatstoFCForcedSends;
 228
 229 int16_t RFnoiseFloor; //measurement of the current RF noise floor
 230 #if defined(DEBUG_RX_SCOREBOARD)
 231 static bool lastPacketCrcError;
 232 #endif
 233 ///////////////////////////////////////////////////////////////
 234
 235 /// Variables for Sync Behaviour ////
 236 uint32_t cycleInterval; // in ms
 237 uint32_t RFmodeLastCycled = 0;
 238 #define RFmodeCycleMultiplierSlow 10
 239 uint8_t RFmodeCycleMultiplier;
 240 bool LockRFmode = false;
 241 ///////////////////////////////////////
 242
 243 #if defined(DEBUG_BF_LINK_STATS)
 244 // Debug vars
 245 uint8_t debug1 = 0;
 246 uint8_t debug2 = 0;
 247 uint8_t debug3 = 0;
 248 int8_t debug4 = 0;
 249 ///////////////////////////////////////
 250 #endif
 251
 252 #if defined(DEBUG_RCVR_LINKSTATS)
 253 static bool debugRcvrLinkstatsPending;
 254 static uint8_t debugRcvrLinkstatsFhssIdx;
 255 #endif
 256
 257 bool BindingModeRequest = false;
 258 static uint32_t BindingRateChangeTime;
 259 #define BindingRateChangeCyclePeriod 125
 260
 261 extern void setWifiUpdateMode();
 262 void reconfigureSerial();
 263
 264 uint8_t getLq()
 265 {
 266     return LQCalc.getLQ();
 267 }
 268
 269 static inline void checkGeminiMode()
 270 {
 271     if (isDualRadio())
 272     {
 273         geminiMode = config.GetAntennaMode() || FHSSuseDualBand; // Force Gemini when in DualBand mode.  Whats the point of using a single frequency!
 274     }
 275 }
 276
 277 static uint8_t minLqForChaos()
 278 {
 279     // Determine the most number of CRC-passing packets we could receive on
 280     // a single channel out of 100 packets that fill the LQcalc span.
 281     // The LQ must be GREATER THAN this value, not >=
 282     // The amount of time we coexist on the same channel is
 283     // 100 divided by the total number of packets in a FHSS loop (rounded up)
 284     // and there would be 4x packets received each time it passes by so
 285     // FHSShopInterval * ceil(100 / FHSShopInterval * numfhss) or
 286     // FHSShopInterval * trunc((100 + (FHSShopInterval * numfhss) - 1) / (FHSShopInterval * numfhss))
 287     // With a interval of 4 this works out to: 2.4=4, FCC915=4, AU915=8, EU868=8, EU/AU433=36
 288     const uint32_t numfhss = FHSSgetChannelCount();
 289     const uint8_t interval = ExpressLRS_currAirRate_Modparams->FHSShopInterval;
 290     return interval * ((interval * numfhss + 99) / (interval * numfhss));
 291 }
 292
 293 void ICACHE_RAM_ATTR getRFlinkInfo()
 294 {
 295     int32_t rssiDBM = Radio.LastPacketRSSI;
 296
 297     if (GPIO_PIN_NSS_2 != UNDEF_PIN)
 298     {
 299         int32_t rssiDBM2 = Radio.LastPacketRSSI2;
 300
 301         #if !defined(DEBUG_RCVR_LINKSTATS)
 302         rssiDBM = LPF_UplinkRSSI0.update(rssiDBM);
 303         rssiDBM2 = LPF_UplinkRSSI1.update(rssiDBM2);
 304         #endif
 305         rssiDBM = (rssiDBM > 0) ? 0 : rssiDBM;
 306         rssiDBM2 = (rssiDBM2 > 0) ? 0 : rssiDBM2;
 307
 308         // BetaFlight/iNav expect positive values for -dBm (e.g. -80dBm -> sent as 80)
 309         CRSF::LinkStatistics.uplink_RSSI_1 = -rssiDBM;
 310         CRSF::LinkStatistics.uplink_RSSI_2 = -rssiDBM2;
 311         antenna = (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1) ? 0 : 1;
 312     }
 313     else if (antenna == 0)
 314     {
 315         #if !defined(DEBUG_RCVR_LINKSTATS)
 316         rssiDBM = LPF_UplinkRSSI0.update(rssiDBM);
 317         #endif
 318         if (rssiDBM > 0) rssiDBM = 0;
 319         // BetaFlight/iNav expect positive values for -dBm (e.g. -80dBm -> sent as 80)
 320         CRSF::LinkStatistics.uplink_RSSI_1 = -rssiDBM;
 321     }
 322     else
 323     {
 324         #if !defined(DEBUG_RCVR_LINKSTATS)
 325         rssiDBM = LPF_UplinkRSSI1.update(rssiDBM);
 326         #endif
 327         if (rssiDBM > 0) rssiDBM = 0;
 328         // BetaFlight/iNav expect positive values for -dBm (e.g. -80dBm -> sent as 80)
 329         // May be overwritten below if DEBUG_BF_LINK_STATS is set
 330         CRSF::LinkStatistics.uplink_RSSI_2 = -rssiDBM;
 331     }
 332
 333     SnrMean.add(Radio.LastPacketSNRRaw);
 334
 335     CRSF::LinkStatistics.active_antenna = antenna;
 336     CRSF::LinkStatistics.uplink_SNR = SNR_DESCALE(Radio.LastPacketSNRRaw); // possibly overriden below
 337     //CRSF::LinkStatistics.uplink_Link_quality = uplinkLQ; // handled in Tick
 338     CRSF::LinkStatistics.rf_Mode = ExpressLRS_currAirRate_Modparams->enum_rate;
 339     //DBGLN(CRSF::LinkStatistics.uplink_RSSI_1);
 340     #if defined(DEBUG_BF_LINK_STATS)
 341     CRSF::LinkStatistics.downlink_RSSI_1 = debug1;
 342     CRSF::LinkStatistics.downlink_Link_quality = debug2;
 343     CRSF::LinkStatistics.downlink_SNR = debug3;
 344     CRSF::LinkStatistics.uplink_RSSI_2 = debug4;
 345     #endif
 346
 347     #if defined(DEBUG_RCVR_LINKSTATS)
 348     // DEBUG_RCVR_LINKSTATS gets full precision SNR, override the value
 349     CRSF::LinkStatistics.uplink_SNR = Radio.LastPacketSNRRaw;
 350     debugRcvrLinkstatsFhssIdx = FHSSsequence[FHSSptr];
 351     #endif
 352 }
 353
 354 void SetRFLinkRate(uint8_t index, bool bindMode) // Set speed of RF link
 355 {
 356     expresslrs_mod_settings_s *const ModParams = get_elrs_airRateConfig(index);
 357     expresslrs_rf_pref_params_s *const RFperf = get_elrs_RFperfParams(index);
 358     // Binding always uses invertIQ
 359     bool invertIQ = bindMode || (UID[5] & 0x01);
 360
 361     uint32_t interval = ModParams->interval;
 362 #if defined(DEBUG_FREQ_CORRECTION) && defined(RADIO_SX128X)
 363     interval = interval * 12 / 10; // increase the packet interval by 20% to allow adding packet header
 364 #endif
 365
 366     hwTimer::updateInterval(interval);
 367
 368     FHSSusePrimaryFreqBand = !(ModParams->radio_type == RADIO_TYPE_LR1121_LORA_2G4) && !(ModParams->radio_type == RADIO_TYPE_LR1121_GFSK_2G4);
 369     FHSSuseDualBand = ModParams->radio_type == RADIO_TYPE_LR1121_LORA_DUAL;
 370
 371     Radio.Config(ModParams->bw, ModParams->sf, ModParams->cr, FHSSgetInitialFreq(),
 372                  ModParams->PreambleLen, invertIQ, ModParams->PayloadLength, 0
 373 #if defined(RADIO_SX128X)
 374                  , uidMacSeedGet(), OtaCrcInitializer, (ModParams->radio_type == RADIO_TYPE_SX128x_FLRC)
 375 #endif
 376 #if defined(RADIO_LR1121)
 377                , ModParams->radio_type == RADIO_TYPE_LR1121_GFSK_900 || ModParams->radio_type == RADIO_TYPE_LR1121_GFSK_2G4, (uint8_t)UID[5], (uint8_t)UID[4]
 378 #endif
 379                  );
 380
 381 #if defined(RADIO_LR1121)
 382     if (FHSSuseDualBand)
 383     {
 384         Radio.Config(ModParams->bw2, ModParams->sf2, ModParams->cr2, FHSSgetInitialGeminiFreq(),
 385                     ModParams->PreambleLen2, invertIQ, ModParams->PayloadLength, 0,
 386                     ModParams->radio_type == RADIO_TYPE_LR1121_GFSK_900 || ModParams->radio_type == RADIO_TYPE_LR1121_GFSK_2G4,
 387                     (uint8_t)UID[5], (uint8_t)UID[4], SX12XX_Radio_2);
 388     }
 389 #endif
 390
 391     Radio.FuzzySNRThreshold = (RFperf->DynpowerSnrThreshUp == DYNPOWER_SNR_THRESH_NONE) ? 0 : (RFperf->DynpowerSnrThreshDn - RFperf->DynpowerSnrThreshUp);
 392
 393     checkGeminiMode();
 394     if (geminiMode)
 395     {
 396         Radio.SetFrequencyReg(FHSSgetInitialGeminiFreq(), SX12XX_Radio_2);
 397     }
 398
 399     OtaUpdateSerializers(smWideOr8ch, ModParams->PayloadLength);
 400     MspReceiver.setMaxPackageIndex(ELRS_MSP_MAX_PACKAGES);
 401     TelemetrySender.setMaxPackageIndex(OtaIsFullRes ? ELRS8_TELEMETRY_MAX_PACKAGES : ELRS4_TELEMETRY_MAX_PACKAGES);
 402
 403     // Wait for (11/10) 110% of time it takes to cycle through all freqs in FHSS table (in ms)
 404     cycleInterval = ((uint32_t)11U * FHSSgetChannelCount() * ModParams->FHSShopInterval * interval) / (10U * 1000U);
 405
 406     ExpressLRS_currAirRate_Modparams = ModParams;
 407     ExpressLRS_currAirRate_RFperfParams = RFperf;
 408     ExpressLRS_nextAirRateIndex = index; // presumably we just handled this
 409     telemBurstValid = false;
 410 }
 411
 412 bool ICACHE_RAM_ATTR HandleFHSS()
 413 {
 414     uint8_t modresultFHSS = (OtaNonce + 1) % ExpressLRS_currAirRate_Modparams->FHSShopInterval;
 415
 416     if ((ExpressLRS_currAirRate_Modparams->FHSShopInterval == 0) || alreadyFHSS == true || InBindingMode || (modresultFHSS != 0) || (connectionState == disconnected))
 417     {
 418         return false;
 419     }
 420
 421     alreadyFHSS = true;
 422
 423     if (geminiMode)
 424     {
 425         if ((((OtaNonce + 1)/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.
 426         {
 427             Radio.SetFrequencyReg(FHSSgetNextFreq(), SX12XX_Radio_1);
 428             Radio.SetFrequencyReg(FHSSgetGeminiFreq(), SX12XX_Radio_2);
 429         }
 430         else
 431         {
 432             // Write radio1 first. This optimises the SPI traffic order.
 433             uint32_t freqRadio2 = FHSSgetNextFreq();
 434             Radio.SetFrequencyReg(FHSSgetGeminiFreq(), SX12XX_Radio_1);
 435             Radio.SetFrequencyReg(freqRadio2, SX12XX_Radio_2);
 436         }
 437     }
 438     else
 439     {
 440         Radio.SetFrequencyReg(FHSSgetNextFreq());
 441     }
 442
 443 #if defined(RADIO_SX127X)
 444     // SX127x radio has to reset receive mode after hopping
 445     uint8_t modresultTLM = (OtaNonce + 1) % ExpressLRS_currTlmDenom;
 446     if (modresultTLM != 0 || ExpressLRS_currTlmDenom == 1) // if we are about to send a tlm response don't bother going back to rx
 447     {
 448         Radio.RXnb();
 449     }
 450 #endif
 451 #if defined(Regulatory_Domain_EU_CE_2400)
 452     SetClearChannelAssessmentTime();
 453 #endif
 454     return true;
 455 }
 456
 457 void ICACHE_RAM_ATTR LinkStatsToOta(OTA_LinkStats_s * const ls)
 458 {
 459     // The value in linkstatistics is "positivized" (inverted polarity)
 460     // and must be inverted on the TX side. Positive values are used
 461     // so save a bit to encode which antenna is in use
 462     ls->uplink_RSSI_1 = CRSF::LinkStatistics.uplink_RSSI_1;
 463     ls->uplink_RSSI_2 = CRSF::LinkStatistics.uplink_RSSI_2;
 464     ls->antenna = antenna;
 465     ls->modelMatch = connectionHasModelMatch;
 466     ls->lq = CRSF::LinkStatistics.uplink_Link_quality;
 467     ls->mspConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0;
 468 #if defined(DEBUG_FREQ_CORRECTION)
 469     ls->SNR = FreqCorrection * 127 / FreqCorrectionMax;
 470 #else
 471     if (SnrMean.getCount())
 472     {
 473         ls->SNR = SnrMean.mean();
 474     }
 475     else
 476     {
 477         ls->SNR = SnrMean.previousMean();
 478     }
 479 #endif
 480 }
 481
 482 bool ICACHE_RAM_ATTR HandleSendTelemetryResponse()
 483 {
 484     uint8_t modresult = (OtaNonce + 1) % ExpressLRS_currTlmDenom;
 485
 486     if ((connectionState == disconnected) || (ExpressLRS_currTlmDenom == 1) || (alreadyTLMresp == true) || (modresult != 0) || !teamraceHasModelMatch)
 487     {
 488         return false; // don't bother sending tlm if disconnected or TLM is off
 489     }
 490
 491     // ESP requires word aligned buffer
 492     WORD_ALIGNED_ATTR OTA_Packet_s otaPkt = {0};
 493     alreadyTLMresp = true;
 494     otaPkt.std.type = PACKET_TYPE_TLM;
 495
 496     bool noAirportDataQueued = firmwareOptions.is_airport && apOutputBuffer.size() == 0;
 497     bool noTlmQueued = !TelemetrySender.IsActive() && noAirportDataQueued;
 498
 499     if (NextTelemetryType == ELRS_TELEMETRY_TYPE_LINK || noTlmQueued)
 500     {
 501         OTA_LinkStats_s * ls;
 502         if (OtaIsFullRes)
 503         {
 504             otaPkt.full.tlm_dl.containsLinkStats = 1;
 505             ls = &otaPkt.full.tlm_dl.ul_link_stats.stats;
 506             // Include some advanced telemetry in the extra space
 507             // Note the use of `ul_link_stats.payload` vs just `payload`
 508             otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(
 509                 otaPkt.full.tlm_dl.ul_link_stats.payload,
 510                 sizeof(otaPkt.full.tlm_dl.ul_link_stats.payload));
 511         }
 512         else
 513         {
 514             otaPkt.std.tlm_dl.type = ELRS_TELEMETRY_TYPE_LINK;
 515             ls = &otaPkt.std.tlm_dl.ul_link_stats.stats;
 516         }
 517         LinkStatsToOta(ls);
 518
 519         NextTelemetryType = ELRS_TELEMETRY_TYPE_DATA;
 520         // Start the count at 1 because the next will be DATA and doing +1 before checking
 521         // against Max below is for some reason 10 bytes more code
 522         telemetryBurstCount = 1;
 523     }
 524     else
 525     {
 526         if (telemetryBurstCount < telemetryBurstMax)
 527         {
 528             telemetryBurstCount++;
 529         }
 530         else
 531         {
 532             NextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
 533         }
 534
 535         if (TelemetrySender.IsActive())
 536         {
 537             if (OtaIsFullRes)
 538             {
 539                 otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(
 540                     otaPkt.full.tlm_dl.payload,
 541                     sizeof(otaPkt.full.tlm_dl.payload));
 542             }
 543             else
 544             {
 545                 otaPkt.std.tlm_dl.type = ELRS_TELEMETRY_TYPE_DATA;
 546                 otaPkt.std.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload(
 547                     otaPkt.std.tlm_dl.payload,
 548                     sizeof(otaPkt.std.tlm_dl.payload));
 549             }
 550         }
 551         else if (firmwareOptions.is_airport)
 552         {
 553             OtaPackAirportData(&otaPkt, &apInputBuffer);
 554         }
 555     }
 556
 557     OtaGeneratePacketCrc(&otaPkt);
 558
 559     SX12XX_Radio_Number_t transmittingRadio;
 560     if (config.GetForceTlmOff())
 561     {
 562         transmittingRadio = SX12XX_Radio_NONE;
 563     }
 564     else if (isDualRadio())
 565     {
 566         transmittingRadio = SX12XX_Radio_All;
 567     }
 568     else
 569     {
 570         transmittingRadio = Radio.GetLastSuccessfulPacketRadio();
 571     }
 572
 573 #if defined(Regulatory_Domain_EU_CE_2400)
 574     transmittingRadio &= ChannelIsClear(transmittingRadio);   // weed out the radio(s) if channel in use
 575 #endif
 576
 577     if (!geminiMode && transmittingRadio == SX12XX_Radio_All) // If the receiver is in diversity mode, only send TLM on a single radio.
 578     {
 579         transmittingRadio = Radio.LastPacketRSSI > Radio.LastPacketRSSI2 ? SX12XX_Radio_1 : SX12XX_Radio_2; // Pick the radio with best rf connection to the tx.
 580     }
 581
 582     Radio.TXnb((uint8_t*)&otaPkt, ExpressLRS_currAirRate_Modparams->PayloadLength, transmittingRadio);
 583
 584     if (transmittingRadio == SX12XX_Radio_NONE)
 585     {
 586         // No packet will be sent due to LBT / Telem forced off.
 587         // Defer TXdoneCallback() to prepare for TLM when the IRQ is normally triggered.
 588         deferExecutionMicros(ExpressLRS_currAirRate_RFperfParams->TOA, Radio.TXdoneCallback);
 589     }
 590
 591     return true;
 592 }
 593
 594 int32_t ICACHE_RAM_ATTR HandleFreqCorr(bool value)
 595 {
 596     int32_t tempFC = FreqCorrection;
 597     if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_2)
 598     {
 599         tempFC = FreqCorrection_2;
 600     }
 601
 602     if (value)
 603     {
 604         if (tempFC > FreqCorrectionMin)
 605         {
 606             tempFC--; // FREQ_STEP units
 607             if (tempFC == FreqCorrectionMin)
 608             {
 609                 DBGLN("Max -FreqCorrection reached!");
 610             }
 611         }
 612     }
 613     else
 614     {
 615         if (tempFC < FreqCorrectionMax)
 616         {
 617             tempFC++; // FREQ_STEP units
 618             if (tempFC == FreqCorrectionMax)
 619             {
 620                 DBGLN("Max +FreqCorrection reached!");
 621             }
 622         }
 623     }
 624
 625     if (Radio.GetProcessingPacketRadio() == SX12XX_Radio_1)
 626     {
 627         FreqCorrection = tempFC;
 628     }
 629     else
 630     {
 631         FreqCorrection_2 = tempFC;
 632     }
 633
 634     return tempFC;
 635 }
 636
 637 void ICACHE_RAM_ATTR updatePhaseLock()
 638 {
 639     if (connectionState != disconnected && PFDloop.hasResult())
 640     {
 641         int32_t RawOffset = PFDloop.calcResult();
 642         int32_t Offset = LPF_Offset.update(RawOffset);
 643         int32_t OffsetDx = LPF_OffsetDx.update(RawOffset - PfdPrevRawOffset);
 644         PfdPrevRawOffset = RawOffset;
 645
 646         if (RXtimerState == tim_locked)
 647         {
 648             // limit rate of freq offset adjustment, use slot 1
 649             // because telemetry can fall on slot 1 and will
 650             // never get here
 651             if (OtaNonce % 8 == 1)
 652             {
 653                 if (Offset > 0)
 654                 {
 655                     hwTimer::incFreqOffset();
 656                 }
 657                 else if (Offset < 0)
 658                 {
 659                     hwTimer::decFreqOffset();
 660                 }
 661             }
 662         }
 663
 664         if (connectionState != connected)
 665         {
 666             hwTimer::phaseShift(RawOffset >> 1);
 667         }
 668         else
 669         {
 670             hwTimer::phaseShift(Offset >> 2);
 671         }
 672
 673         DBGVLN("%d:%d:%d:%d:%d", Offset, RawOffset, OffsetDx, hwTimer::getFreqOffset(), uplinkLQ);
 674         UNUSED(OffsetDx); // complier warning if no debug
 675     }
 676
 677     PFDloop.reset();
 678 }
 679
 680 void ICACHE_RAM_ATTR HWtimerCallbackTick() // this is 180 out of phase with the other callback, occurs mid-packet reception
 681 {
 682     updatePhaseLock();
 683     OtaNonce++;
 684
 685     // if (!alreadyTLMresp && !alreadyFHSS && !LQCalc.currentIsSet()) // packet timeout AND didn't DIDN'T just hop or send TLM
 686     // {
 687     //     Radio.RXnb(); // put the radio cleanly back into RX in case of garbage data
 688     // }
 689
 690
 691     if (ExpressLRS_currAirRate_Modparams->numOfSends == 1)
 692     {
 693         // Save the LQ value before the inc() reduces it by 1
 694         uplinkLQ = LQCalc.getLQ();
 695     } else
 696     if (!((OtaNonce - 1) % ExpressLRS_currAirRate_Modparams->numOfSends))
 697     {
 698         uplinkLQ = LQCalcDVDA.getLQ();
 699         LQCalcDVDA.inc();
 700     }
 701
 702     CRSF::LinkStatistics.uplink_Link_quality = uplinkLQ;
 703     // Only advance the LQI period counter if we didn't send Telemetry this period
 704     if (!alreadyTLMresp)
 705         LQCalc.inc();
 706
 707     alreadyTLMresp = false;
 708     alreadyFHSS = false;
 709 }
 710
 711 //////////////////////////////////////////////////////////////
 712 // flip to the other antenna
 713 // no-op if GPIO_PIN_ANT_CTRL not defined
 714 static inline void switchAntenna()
 715 {
 716     if (GPIO_PIN_ANT_CTRL != UNDEF_PIN && config.GetAntennaMode() == 2)
 717     {
 718         // 0 and 1 is use for gpio_antenna_select
 719         // 2 is diversity
 720         antenna = !antenna;
 721         (antenna == 0) ? LPF_UplinkRSSI0.reset() : LPF_UplinkRSSI1.reset(); // discard the outdated value after switching
 722         digitalWrite(GPIO_PIN_ANT_CTRL, antenna);
 723         if (GPIO_PIN_ANT_CTRL_COMPL != UNDEF_PIN)
 724         {
 725             digitalWrite(GPIO_PIN_ANT_CTRL_COMPL, !antenna);
 726         }
 727     }
 728 }
 729
 730 static void ICACHE_RAM_ATTR updateDiversity()
 731 {
 732
 733     if (GPIO_PIN_ANT_CTRL != UNDEF_PIN)
 734     {
 735         if(config.GetAntennaMode() == 2)
 736         {
 737             // 0 and 1 is use for gpio_antenna_select
 738             // 2 is diversity
 739             static int32_t prevRSSI;        // saved rssi so that we can compare if switching made things better or worse
 740             static int32_t antennaLQDropTrigger;
 741             static int32_t antennaRSSIDropTrigger;
 742             int32_t rssi = (antenna == 0) ? LPF_UplinkRSSI0.value() : LPF_UplinkRSSI1.value();
 743             int32_t otherRSSI = (antenna == 0) ? LPF_UplinkRSSI1.value() : LPF_UplinkRSSI0.value();
 744
 745             //if rssi dropped by the amount of DIVERSITY_ANTENNA_RSSI_TRIGGER
 746             if ((rssi < (prevRSSI - DIVERSITY_ANTENNA_RSSI_TRIGGER)) && antennaRSSIDropTrigger >= DIVERSITY_ANTENNA_INTERVAL)
 747             {
 748                 switchAntenna();
 749                 antennaLQDropTrigger = 1;
 750                 antennaRSSIDropTrigger = 0;
 751             }
 752             else if (rssi > prevRSSI || antennaRSSIDropTrigger < DIVERSITY_ANTENNA_INTERVAL)
 753             {
 754                 prevRSSI = rssi;
 755                 antennaRSSIDropTrigger++;
 756             }
 757
 758             // if we didn't get a packet switch the antenna
 759             if (!LQCalc.currentIsSet() && antennaLQDropTrigger == 0)
 760             {
 761                 switchAntenna();
 762                 antennaLQDropTrigger = 1;
 763                 antennaRSSIDropTrigger = 0;
 764             }
 765             else if (antennaLQDropTrigger >= DIVERSITY_ANTENNA_INTERVAL)
 766             {
 767                 // We switched antenna on the previous packet, so we now have relatively fresh rssi info for both antennas.
 768                 // We can compare the rssi values and see if we made things better or worse when we switched
 769                 if (rssi < otherRSSI)
 770                 {
 771                     // things got worse when we switched, so change back.
 772                     switchAntenna();
 773                     antennaLQDropTrigger = 1;
 774                     antennaRSSIDropTrigger = 0;
 775                 }
 776                 else
 777                 {
 778                     // all good, we can stay on the current antenna. Clear the flag.
 779                     antennaLQDropTrigger = 0;
 780                 }
 781             }
 782             else if (antennaLQDropTrigger > 0)
 783             {
 784                 antennaLQDropTrigger ++;
 785             }
 786         }
 787         else
 788         {
 789             digitalWrite(GPIO_PIN_ANT_CTRL, config.GetAntennaMode());
 790             if (GPIO_PIN_ANT_CTRL_COMPL != UNDEF_PIN)
 791             {
 792                 digitalWrite(GPIO_PIN_ANT_CTRL_COMPL, !config.GetAntennaMode());
 793             }
 794             antenna = config.GetAntennaMode();
 795         }
 796     }
 797 }
 798
 799 void ICACHE_RAM_ATTR HWtimerCallbackTock()
 800 {
 801     PFDloop.intEvent(micros()); // our internal osc just fired
 802
 803     if (ExpressLRS_currAirRate_Modparams->numOfSends > 1 && !(OtaNonce % ExpressLRS_currAirRate_Modparams->numOfSends))
 804     {
 805         if (LQCalcDVDA.currentIsSet())
 806         {
 807             crsfRCFrameAvailable();
 808             if (teamraceHasModelMatch)
 809                 servoNewChannelsAvailable();
 810         }
 811         else
 812         {
 813             crsfRCFrameMissed();
 814         }
 815     }
 816     else if (ExpressLRS_currAirRate_Modparams->numOfSends == 1)
 817     {
 818         if (!LQCalc.currentIsSet())
 819         {
 820             crsfRCFrameMissed();
 821         }
 822     }
 823
 824     if (!didFHSS)
 825     {
 826         HandleFHSS();
 827     }
 828     didFHSS = false;
 829
 830     Radio.isFirstRxIrq = true;
 831     updateDiversity();
 832     tlmSent = HandleSendTelemetryResponse();
 833
 834     #if defined(DEBUG_RX_SCOREBOARD)
 835     static bool lastPacketWasTelemetry = false;
 836     if (!LQCalc.currentIsSet() && !lastPacketWasTelemetry)
 837         DBGW(lastPacketCrcError ? '.' : '_');
 838     lastPacketCrcError = false;
 839     lastPacketWasTelemetry = tlmSent;
 840     #endif
 841 }
 842
 843 void LostConnection(bool resumeRx)
 844 {
 845     DBGLN("lost conn fc=%d fo=%d", FreqCorrection, hwTimer::getFreqOffset());
 846
 847     // Use this rate as the initial rate next time if we connected on it
 848     if (connectionState == connected)
 849         config.SetRateInitialIdx(ExpressLRS_nextAirRateIndex);
 850
 851     RFmodeCycleMultiplier = 1;
 852     connectionState = disconnected; //set lost connection
 853     RXtimerState = tim_disconnected;
 854     hwTimer::resetFreqOffset();
 855     PfdPrevRawOffset = 0;
 856     GotConnectionMillis = 0;
 857     uplinkLQ = 0;
 858     LQCalc.reset();
 859     LQCalcDVDA.reset();
 860     LPF_Offset.init(0);
 861     LPF_OffsetDx.init(0);
 862     alreadyTLMresp = false;
 863     alreadyFHSS = false;
 864
 865     if (!InBindingMode)
 866     {
 867         if (hwTimer::running)
 868         {
 869             while(micros() - PFDloop.getIntEventTime() > 250); // time it just after the tock()
 870             hwTimer::stop();
 871         }
 872         SetRFLinkRate(ExpressLRS_nextAirRateIndex, false); // also sets to initialFreq
 873         // If not resumRx, Radio will be left in SX127x_OPMODE_STANDBY / SX1280_MODE_STDBY_XOSC
 874         if (resumeRx)
 875         {
 876             Radio.RXnb();
 877         }
 878     }
 879 }
 880
 881 void ICACHE_RAM_ATTR TentativeConnection(unsigned long now)
 882 {
 883     PFDloop.reset();
 884     connectionState = tentative;
 885     connectionHasModelMatch = false;
 886     RXtimerState = tim_disconnected;
 887     DBGLN("tentative conn");
 888     PfdPrevRawOffset = 0;
 889     LPF_Offset.init(0);
 890     SnrMean.reset();
 891     RFmodeLastCycled = now; // give another 3 sec for lock to occur
 892
 893     // The caller MUST call hwTimer::resume(). It is not done here because
 894     // the timer ISR will fire immediately and preempt any other code
 895 }
 896
 897 void GotConnection(unsigned long now)
 898 {
 899     if (connectionState == connected)
 900     {
 901         return; // Already connected
 902     }
 903
 904     LockRFmode = firmwareOptions.lock_on_first_connection;
 905
 906     connectionState = connected; //we got a packet, therefore no lost connection
 907     RXtimerState = tim_tentative;
 908     GotConnectionMillis = now;
 909     #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
 910     webserverPreventAutoStart = true;
 911     #endif
 912
 913     if (firmwareOptions.is_airport)
 914     {
 915         apInputBuffer.flush();
 916         apOutputBuffer.flush();
 917     }
 918
 919     DBGLN("got conn");
 920 }
 921
 922 static void ICACHE_RAM_ATTR ProcessRfPacket_RC(OTA_Packet_s const * const otaPktPtr)
 923 {
 924     // Must be fully connected to process RC packets, prevents processing RC
 925     // during sync, where packets can be received before connection
 926     if (connectionState != connected || SwitchModePending)
 927         return;
 928
 929     bool telemetryConfirmValue = OtaUnpackChannelData(otaPktPtr, ChannelData, ExpressLRS_currTlmDenom);
 930     TelemetrySender.ConfirmCurrentPayload(telemetryConfirmValue);
 931
 932     // No channels packets to the FC or PWM pins if no model match
 933     if (connectionHasModelMatch)
 934     {
 935         if (ExpressLRS_currAirRate_Modparams->numOfSends == 1)
 936         {
 937             crsfRCFrameAvailable();
 938             // teamrace is only checked for servos because the teamrace model select logic only runs
 939             // when new frames are available, and will decide later if the frame will be forwarded
 940             if (teamraceHasModelMatch)
 941                 servoNewChannelsAvailable();
 942         }
 943         else if (!LQCalcDVDA.currentIsSet())
 944         {
 945             LQCalcDVDA.add();
 946         }
 947         #if defined(DEBUG_RCVR_LINKSTATS)
 948         debugRcvrLinkstatsPending = true;
 949         #endif
 950     }
 951 }
 952
 953 void ICACHE_RAM_ATTR OnELRSBindMSP(uint8_t* newUid4)
 954 {
 955     // Binding over MSP only contains 4 bytes due to packet size limitations, clear out any leading bytes
 956     UID[0] = 0;
 957     UID[1] = 0;
 958     for (unsigned i = 0; i < 4; i++)
 959     {
 960         UID[i + 2] = newUid4[i];
 961     }
 962
 963     DBGLN("New UID = %u, %u, %u, %u, %u, %u", UID[0], UID[1], UID[2], UID[3], UID[4], UID[5]);
 964
 965     // Set new UID in eeprom
 966     // EEPROM commit will happen on the main thread in ExitBindingMode()
 967     config.SetUID(UID);
 968 }
 969
 970 static void ICACHE_RAM_ATTR ProcessRfPacket_MSP(OTA_Packet_s const * const otaPktPtr)
 971 {
 972     uint8_t packageIndex;
 973     uint8_t const * payload;
 974     uint8_t dataLen;
 975     if (OtaIsFullRes)
 976     {
 977         packageIndex = otaPktPtr->full.msp_ul.packageIndex;
 978         payload = otaPktPtr->full.msp_ul.payload;
 979         dataLen = sizeof(otaPktPtr->full.msp_ul.payload);
 980         if (config.GetSerialProtocol() == PROTOCOL_MAVLINK)
 981         {
 982             TelemetrySender.ConfirmCurrentPayload(otaPktPtr->full.msp_ul.tlmFlag);
 983         }
 984         else
 985         {
 986             packageIndex &= ELRS8_TELEMETRY_MAX_PACKAGES;
 987         }
 988     }
 989     else
 990     {
 991         packageIndex = otaPktPtr->std.msp_ul.packageIndex;
 992         payload = otaPktPtr->std.msp_ul.payload;
 993         dataLen = sizeof(otaPktPtr->std.msp_ul.payload);
 994         if (config.GetSerialProtocol() == PROTOCOL_MAVLINK)
 995         {
 996             TelemetrySender.ConfirmCurrentPayload(otaPktPtr->std.msp_ul.tlmFlag);
 997         }
 998         else
 999         {
1000             packageIndex &= ELRS4_TELEMETRY_MAX_PACKAGES;
1001         }
1002     }
1003
1004     // Always examine MSP packets for bind information if in bind mode
1005     // [1] is the package index, first packet of the MSP
1006     if (InBindingMode && packageIndex == 1 && payload[0] == MSP_ELRS_BIND)
1007     {
1008         OnELRSBindMSP((uint8_t *)&payload[1]);
1009         return;
1010     }
1011
1012     // Must be fully connected to process MSP, prevents processing MSP
1013     // during sync, where packets can be received before connection
1014     if (connectionState != connected)
1015         return;
1016
1017     bool currentMspConfirmValue = MspReceiver.GetCurrentConfirm();
1018     MspReceiver.ReceiveData(packageIndex, payload, dataLen);
1019     if (currentMspConfirmValue != MspReceiver.GetCurrentConfirm())
1020     {
1021         NextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
1022     }
1023 }
1024
1025 static void ICACHE_RAM_ATTR updateSwitchModePendingFromOta(uint8_t newSwitchMode)
1026 {
1027     if (OtaSwitchModeCurrent == newSwitchMode)
1028     {
1029         // Cancel any switch if pending
1030         SwitchModePending = 0;
1031         return;
1032     }
1033
1034     // One is added to the mode because SwitchModePending==0 means no switch pending
1035     // and that's also a valid switch mode. The 1 is removed when this is handled.
1036     // A negative SwitchModePending means not to switch yet
1037     int8_t newSwitchModePending = -(int8_t)newSwitchMode - 1;
1038
1039     // Switch mode can be changed while disconnected
1040     // OR there are two sync packets with the same new switch mode,
1041     // as a "confirm". No RC packets are processed until
1042     if (connectionState == disconnected ||
1043         SwitchModePending == newSwitchModePending)
1044     {
1045         // Add one to the mode because SwitchModePending==0 means no switch pending
1046         // and that's also a valid switch mode. The 1 is removed when this is handled
1047         SwitchModePending = newSwitchMode + 1;
1048     }
1049     else
1050     {
1051         // Save the negative version of the new switch mode to compare
1052         // against on the next SYNC packet, but do not switch yet
1053         SwitchModePending = newSwitchModePending;
1054     }
1055 }
1056
1057 static bool ICACHE_RAM_ATTR ProcessRfPacket_SYNC(uint32_t const now, OTA_Sync_s const * const otaSync)
1058 {
1059     // Verify the first two of three bytes of the binding ID, which should always match
1060     if (otaSync->UID3 != UID[3] || otaSync->UID4 != UID[4])
1061         return false;
1062
1063     // The third byte will be XORed with inverse of the ModelId if ModelMatch is on
1064     // Only require the first 18 bits of the UID to match to establish a connection
1065     // but the last 6 bits must modelmatch before sending any data to the FC
1066     if ((otaSync->UID5 & ~MODELMATCH_MASK) != (UID[5] & ~MODELMATCH_MASK))
1067         return false;
1068
1069     LastSyncPacket = now;
1070 #if defined(DEBUG_RX_SCOREBOARD)
1071     DBGW('s');
1072 #endif
1073
1074     // Will change the packet air rate in loop() if this changes
1075     ExpressLRS_nextAirRateIndex = otaSync->rateIndex;
1076     updateSwitchModePendingFromOta(otaSync->switchEncMode);
1077
1078     // Update TLM ratio, should never be TLM_RATIO_STD/DISARMED, the TX calculates the correct value for the RX
1079     expresslrs_tlm_ratio_e TLMrateIn = (expresslrs_tlm_ratio_e)(otaSync->newTlmRatio + (uint8_t)TLM_RATIO_NO_TLM);
1080     uint8_t TlmDenom = TLMratioEnumToValue(TLMrateIn);
1081     if (ExpressLRS_currTlmDenom != TlmDenom)
1082     {
1083         DBGLN("New TLMrate 1:%u", TlmDenom);
1084         ExpressLRS_currTlmDenom = TlmDenom;
1085         telemBurstValid = false;
1086     }
1087
1088     // modelId = 0xff indicates modelMatch is disabled, the XOR does nothing in that case
1089     uint8_t modelXor = (~config.GetModelId()) & MODELMATCH_MASK;
1090     bool modelMatched = otaSync->UID5 == (UID[5] ^ modelXor);
1091     DBGVLN("MM %u=%u %d", otaSync->UID5, UID[5], modelMatched);
1092
1093     if (connectionState == disconnected
1094         || OtaNonce != otaSync->nonce
1095         || FHSSgetCurrIndex() != otaSync->fhssIndex
1096         || connectionHasModelMatch != modelMatched)
1097     {
1098         //DBGLN("\r\n%ux%ux%u", OtaNonce, otaPktPtr->sync.nonce, otaPktPtr->sync.fhssIndex);
1099         FHSSsetCurrIndex(otaSync->fhssIndex);
1100         OtaNonce = otaSync->nonce;
1101         TentativeConnection(now);
1102         // connectionHasModelMatch must come after TentativeConnection, which resets it
1103         connectionHasModelMatch = modelMatched;
1104         return true;
1105     }
1106
1107     return false;
1108 }
1109
1110 bool ICACHE_RAM_ATTR ProcessRFPacket(SX12xxDriverCommon::rx_status const status)
1111 {
1112     if (status != SX12xxDriverCommon::SX12XX_RX_OK)
1113     {
1114         DBGVLN("HW CRC error");
1115         #if defined(DEBUG_RX_SCOREBOARD)
1116             lastPacketCrcError = true;
1117         #endif
1118         return false;
1119     }
1120     uint32_t const beginProcessing = micros();
1121
1122     OTA_Packet_s * const otaPktPtr = (OTA_Packet_s * const)Radio.RXdataBuffer;
1123     if (!OtaValidatePacketCrc(otaPktPtr))
1124     {
1125         DBGVLN("CRC error");
1126         #if defined(DEBUG_RX_SCOREBOARD)
1127             lastPacketCrcError = true;
1128         #endif
1129         return false;
1130     }
1131
1132     PFDloop.extEvent(beginProcessing + PACKET_TO_TOCK_SLACK);
1133
1134     doStartTimer = false;
1135     unsigned long now = millis();
1136
1137     LastValidPacket = now;
1138
1139     switch (otaPktPtr->std.type)
1140     {
1141     case PACKET_TYPE_RCDATA: //Standard RC Data Packet
1142         ProcessRfPacket_RC(otaPktPtr);
1143         break;
1144     case PACKET_TYPE_MSPDATA:
1145         ProcessRfPacket_MSP(otaPktPtr);
1146         break;
1147     case PACKET_TYPE_SYNC: //sync packet from master
1148         doStartTimer = ProcessRfPacket_SYNC(now,
1149             OtaIsFullRes ? &otaPktPtr->full.sync.sync : &otaPktPtr->std.sync)
1150             && !InBindingMode;
1151         break;
1152     case PACKET_TYPE_TLM:
1153         if (firmwareOptions.is_airport)
1154         {
1155             OtaUnpackAirportData(otaPktPtr, &apOutputBuffer);
1156         }
1157         break;
1158     default:
1159         break;
1160     }
1161
1162     // Store the LQ/RSSI/Antenna
1163     Radio.GetLastPacketStats();
1164     getRFlinkInfo();
1165
1166     if (Radio.FrequencyErrorAvailable())
1167     {
1168         int32_t tempFreqCorrection = HandleFreqCorr(Radio.GetFrequencyErrorbool());      // Adjusts FreqCorrection for RX freq offset
1169     #if defined(RADIO_SX127X)
1170         // Teamp900 also needs to adjust its demood PPM
1171         Radio.SetPPMoffsetReg(tempFreqCorrection);
1172     #endif /* RADIO_SX127X */
1173     }
1174
1175     // Received a packet, that's the definition of LQ
1176     LQCalc.add();
1177     // Extend sync duration since we've received a packet at this rate
1178     // but do not extend it indefinitely
1179     RFmodeCycleMultiplier = RFmodeCycleMultiplierSlow;
1180
1181 #if defined(DEBUG_RX_SCOREBOARD)
1182     if (otaPktPtr->std.type != PACKET_TYPE_SYNC) DBGW(connectionHasModelMatch ? 'R' : 'r');
1183 #endif
1184
1185     return true;
1186 }
1187
1188 bool ICACHE_RAM_ATTR RXdoneISR(SX12xxDriverCommon::rx_status const status)
1189 {
1190     if (LQCalc.currentIsSet() && connectionState == connected)
1191     {
1192         return false; // Already received a packet, do not run ProcessRFPacket() again.
1193     }
1194
1195     if (ProcessRFPacket(status))
1196     {
1197         didFHSS = HandleFHSS();
1198
1199         if (doStartTimer)
1200         {
1201             doStartTimer = false;
1202             hwTimer::resume(); // will throw an interrupt immediately
1203         }
1204
1205         return true;
1206     }
1207     return false;
1208 }
1209
1210 void ICACHE_RAM_ATTR TXdoneISR()
1211 {
1212     Radio.RXnb();
1213 #if defined(Regulatory_Domain_EU_CE_2400)
1214     SetClearChannelAssessmentTime();
1215 #endif
1216 #if defined(DEBUG_RX_SCOREBOARD)
1217     DBGW('T');
1218 #endif
1219 }
1220
1221 void UpdateModelMatch(uint8_t model)
1222 {
1223     DBGLN("Set ModelId=%u", model);
1224     config.SetModelId(model);
1225 }
1226
1227 void SendMSPFrameToFC(uint8_t *mspData)
1228 {
1229     serialIO->queueMSPFrameTransmission(mspData);
1230 }
1231
1232 /**
1233  * Process the assembled MSP packet in MspData[]
1234  **/
1235 void MspReceiveComplete()
1236 {
1237     switch (MspData[0])
1238     {
1239     case MSP_ELRS_SET_RX_WIFI_MODE: //0x0E
1240 #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
1241         // The MSP packet needs to be ACKed so the TX doesn't
1242         // keep sending it, so defer the switch to wifi
1243         deferExecutionMillis(500, []() {
1244             setWifiUpdateMode();
1245         });
1246 #endif
1247         break;
1248     case MSP_ELRS_MAVLINK_TLM: // 0xFD
1249         // raw mavlink data
1250         mavlinkOutputBuffer.atomicPushBytes(&MspData[2], MspData[1]);
1251         break;
1252     default:
1253         //handle received CRSF package
1254         crsf_ext_header_t *receivedHeader = (crsf_ext_header_t *) MspData;
1255         switch (receivedHeader->type)
1256         {
1257         case CRSF_FRAMETYPE_MSP_WRITE: //encapsulated MSP payload
1258             if (MspData[7] == MSP_SET_RX_CONFIG && MspData[8] == MSP_ELRS_MODEL_ID)
1259             {
1260                 UpdateModelMatch(MspData[9]);
1261                 break;
1262             }
1263             else if (MspData[7] == MSP_SET_VTX_CONFIG)
1264             {
1265                 if (OPT_HAS_VTX_SPI) {
1266                     vtxSPIFrequency = getFreqByIdx(MspData[8]);
1267                     if (MspData[6] >= 4) // If packet has 4 bytes it also contains power idx and pitmode.
1268                     {
1269                         vtxSPIPowerIdx = MspData[10];
1270                         vtxSPIPitmode = MspData[11];
1271                     }
1272                     devicesTriggerEvent();
1273                     break;
1274 #if defined(PLATFORM_ESP32)
1275                 } else if (config.GetSerial1Protocol() == PROTOCOL_SERIAL1_TRAMP || config.GetSerial1Protocol() == PROTOCOL_SERIAL1_SMARTAUDIO) {
1276                     serial1IO->queueMSPFrameTransmission(MspData);
1277                     break;
1278 #endif
1279                 }
1280             }
1281             // FALLTHROUGH
1282         default:
1283             if ((receivedHeader->dest_addr == CRSF_ADDRESS_BROADCAST || receivedHeader->dest_addr == CRSF_ADDRESS_CRSF_RECEIVER))
1284             {
1285                 luaParamUpdateReq(
1286                     MspData[CRSF_TELEMETRY_TYPE_INDEX],
1287                     MspData[CRSF_TELEMETRY_FIELD_ID_INDEX],
1288                     MspData[CRSF_TELEMETRY_FIELD_CHUNK_INDEX]
1289                 );
1290             }
1291             break;
1292         }
1293         // No MSP data to the FC if no model match
1294         if (connectionHasModelMatch && teamraceHasModelMatch &&
1295             (receivedHeader->dest_addr == CRSF_ADDRESS_BROADCAST || receivedHeader->dest_addr == CRSF_ADDRESS_FLIGHT_CONTROLLER))
1296         {
1297             serialIO->queueMSPFrameTransmission(MspData);
1298         }
1299     }
1300
1301     MspReceiver.Unlock();
1302 }
1303
1304 static void setupSerial()
1305 {
1306     bool sbusSerialOutput = false;
1307         bool sumdSerialOutput = false;
1308     bool mavlinkSerialOutput = false;
1309
1310 #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
1311     bool hottTlmSerial = false;
1312 #endif
1313
1314     if (OPT_CRSF_RCVR_NO_SERIAL)
1315     {
1316         // For PWM receivers with no serial pins defined, only turn on the Serial port if logging is on
1317         #if defined(DEBUG_LOG) || defined(DEBUG_RCVR_LINKSTATS)
1318         #if defined(PLATFORM_ESP32_S3) && !defined(ESP32_S3_USB_JTAG_ENABLED)
1319         // Requires pull-down on GPIO3.  If GPIO3 has a pull-up (for JTAG) this doesn't work.
1320         USBSerial.begin(serialBaud);
1321         SerialLogger = &USBSerial;
1322         #else
1323         Serial.begin(serialBaud);
1324         SerialLogger = &Serial;
1325         #endif
1326         #else
1327         SerialLogger = new NullStream();
1328         #endif
1329         serialIO = new SerialNOOP();
1330         return;
1331     }
1332     if (config.GetSerialProtocol() == PROTOCOL_CRSF || config.GetSerialProtocol() == PROTOCOL_INVERTED_CRSF)
1333     {
1334         serialBaud = firmwareOptions.uart_baud;
1335     }
1336     else if (config.GetSerialProtocol() == PROTOCOL_SBUS || config.GetSerialProtocol() == PROTOCOL_INVERTED_SBUS || config.GetSerialProtocol() == PROTOCOL_DJI_RS_PRO)
1337     {
1338         sbusSerialOutput = true;
1339         serialBaud = 100000;
1340     }
1341     else if (config.GetSerialProtocol() == PROTOCOL_SUMD)
1342     {
1343         sumdSerialOutput = true;
1344         serialBaud = 115200;
1345     }
1346     else if (config.GetSerialProtocol() == PROTOCOL_MAVLINK)
1347     {
1348         mavlinkSerialOutput = true;
1349         serialBaud = 460800;
1350     }
1351 #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
1352     else if (config.GetSerialProtocol() == PROTOCOL_HOTT_TLM)
1353     {
1354         hottTlmSerial = true;
1355         serialBaud = 19200;
1356     }
1357 #endif
1358     bool invert = config.GetSerialProtocol() == PROTOCOL_SBUS || config.GetSerialProtocol() == PROTOCOL_INVERTED_CRSF || config.GetSerialProtocol() == PROTOCOL_DJI_RS_PRO;
1359
1360 #ifdef PLATFORM_STM32
1361 #if defined(TARGET_R9SLIMPLUS_RX)
1362     SERIAL_PROTOCOL_RX.setRx(GPIO_PIN_RCSIGNAL_RX);
1363     SERIAL_PROTOCOL_RX.begin(serialBaud);
1364
1365     SERIAL_PROTOCOL_TX.setTx(GPIO_PIN_RCSIGNAL_TX);
1366 #else
1367 #if defined(GPIO_PIN_RCSIGNAL_RX_SBUS) && defined(GPIO_PIN_RCSIGNAL_TX_SBUS)
1368     if (invert)
1369     {
1370         SERIAL_PROTOCOL_TX.setTx(GPIO_PIN_RCSIGNAL_TX_SBUS);
1371         SERIAL_PROTOCOL_TX.setRx(GPIO_PIN_RCSIGNAL_RX_SBUS);
1372     }
1373     else
1374 #endif
1375     {
1376         SERIAL_PROTOCOL_TX.setTx(GPIO_PIN_RCSIGNAL_TX);
1377         SERIAL_PROTOCOL_TX.setRx(GPIO_PIN_RCSIGNAL_RX);
1378     }
1379 #endif /* TARGET_R9SLIMPLUS_RX */
1380 #if defined(TARGET_RX_GHOST_ATTO_V1)
1381     // USART1 is used for RX (half duplex)
1382     SERIAL_PROTOCOL_RX.setHalfDuplex();
1383     SERIAL_PROTOCOL_RX.setTx(GPIO_PIN_RCSIGNAL_RX);
1384     SERIAL_PROTOCOL_RX.begin(serialBaud);
1385     SERIAL_PROTOCOL_RX.enableHalfDuplexRx();
1386
1387     // USART2 is used for TX (half duplex)
1388     // Note: these must be set before begin()
1389     SERIAL_PROTOCOL_TX.setHalfDuplex();
1390     SERIAL_PROTOCOL_TX.setRx((PinName)NC);
1391     SERIAL_PROTOCOL_TX.setTx(GPIO_PIN_RCSIGNAL_TX);
1392 #endif /* TARGET_RX_GHOST_ATTO_V1 */
1393     SERIAL_PROTOCOL_TX.begin(serialBaud, sbusSerialOutput ? SERIAL_8E2 : SERIAL_8N1);
1394 #endif /* PLATFORM_STM32 */
1395 #if defined(TARGET_RX_GHOST_ATTO_V1) || defined(TARGET_RX_FM30_MINI)
1396     if (invert)
1397     {
1398         LL_GPIO_SetPinPull(GPIOA, GPIO_PIN_2, LL_GPIO_PULL_DOWN);
1399         USART2->CR1 &= ~USART_CR1_UE;
1400         USART2->CR2 |= USART_CR2_TXINV;
1401         USART2->CR1 |= USART_CR1_UE;
1402     }
1403 #endif
1404
1405 #if defined(TARGET_RX_FM30_MINI) || defined(TARGET_DIY_900_RX_STM32)
1406     Serial.setRx(GPIO_PIN_DEBUG_RX);
1407     Serial.setTx(GPIO_PIN_DEBUG_TX);
1408     Serial.begin(serialBaud); // Same baud as CRSF for simplicity
1409 #endif
1410
1411 #if defined(PLATFORM_ESP8266)
1412     SerialConfig config = SERIAL_8N1;
1413
1414     if(sbusSerialOutput)
1415     {
1416         config = SERIAL_8E2;
1417     }
1418     else if(hottTlmSerial)
1419     {
1420         config = SERIAL_8N2;
1421     }
1422
1423     SerialMode mode = (sbusSerialOutput || sumdSerialOutput)  ? SERIAL_TX_ONLY : SERIAL_FULL;
1424     Serial.begin(serialBaud, config, mode, -1, invert);
1425 #elif defined(PLATFORM_ESP32)
1426     uint32_t config = SERIAL_8N1;
1427
1428     if(sbusSerialOutput)
1429     {
1430         config = SERIAL_8E2;
1431     }
1432     else if(hottTlmSerial)
1433     {
1434         config = SERIAL_8N2;
1435     }
1436
1437     // ARDUINO_CORE_INVERT_FIX PT2
1438     #if defined(ARDUINO_CORE_INVERT_FIX)
1439     if(invert == false)
1440     {
1441         uart_set_line_inverse(0, UART_SIGNAL_INV_DISABLE);
1442     }
1443     #endif
1444     // ARDUINO_CORE_INVERT_FIX PT2 end
1445
1446     Serial.begin(serialBaud, config, GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX, invert);
1447 #endif
1448
1449     if (firmwareOptions.is_airport)
1450     {
1451         serialIO = new SerialAirPort(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
1452     }
1453     else if (sbusSerialOutput)
1454     {
1455         serialIO = new SerialSBUS(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
1456     }
1457     else if (sumdSerialOutput)
1458     {
1459         serialIO = new SerialSUMD(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
1460     }
1461     else if (mavlinkSerialOutput)
1462     {
1463         serialIO = new SerialMavlink(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
1464     }
1465     #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
1466     else if (hottTlmSerial)
1467     {
1468         serialIO = new SerialHoTT_TLM(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
1469     }
1470     #endif
1471     else
1472     {
1473         serialIO = new SerialCRSF(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
1474     }
1475
1476 #if defined(DEBUG_ENABLED)
1477 #if defined(PLATFORM_ESP32_S3) || defined(PLATFORM_ESP32_C3)
1478     USBSerial.begin(460800);
1479     SerialLogger = &USBSerial;
1480 #else
1481     SerialLogger = &Serial;
1482 #endif
1483 #else
1484     SerialLogger = new NullStream();
1485 #endif
1486 }
1487
1488 #if defined(PLATFORM_ESP32)
1489 static void serial1Shutdown()
1490 {
1491     if(serial1IO != nullptr)
1492     {
1493         Serial1.end();
1494         delete serial1IO;
1495         serial1IO = nullptr;
1496     }
1497 }
1498
1499 static void setupSerial1()
1500 {
1501     //
1502     // init secondary serial and protocol
1503     //
1504     int8_t serial1RXpin = GPIO_PIN_SERIAL1_RX;
1505
1506     if (serial1RXpin == UNDEF_PIN)
1507     {
1508         for (uint8_t ch = 0; ch < GPIO_PIN_PWM_OUTPUTS_COUNT; ch++)
1509         {
1510             if (config.GetPwmChannel(ch)->val.mode == somSerial1RX)
1511                 serial1RXpin = GPIO_PIN_PWM_OUTPUTS[ch];
1512         }
1513     }
1514
1515     int8_t serial1TXpin = GPIO_PIN_SERIAL1_TX;
1516
1517     if (serial1TXpin == UNDEF_PIN)
1518     {
1519         for (uint8_t ch = 0; ch < GPIO_PIN_PWM_OUTPUTS_COUNT; ch++)
1520         {
1521             if (config.GetPwmChannel(ch)->val.mode == somSerial1TX)
1522                 serial1TXpin = GPIO_PIN_PWM_OUTPUTS[ch];
1523         }
1524     }
1525
1526     switch(config.GetSerial1Protocol())
1527     {
1528         case PROTOCOL_SERIAL1_OFF:
1529             break;
1530         case PROTOCOL_SERIAL1_CRSF:
1531             Serial1.begin(firmwareOptions.uart_baud, SERIAL_8N1, serial1RXpin, serial1TXpin, false);
1532             serial1IO = new SerialCRSF(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX);
1533             break;
1534         case PROTOCOL_SERIAL1_INVERTED_CRSF:
1535             Serial1.begin(firmwareOptions.uart_baud, SERIAL_8N1, serial1RXpin, serial1TXpin, true);
1536             serial1IO = new SerialCRSF(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX);
1537             break;
1538         case PROTOCOL_SERIAL1_SBUS:
1539         case PROTOCOL_SERIAL1_DJI_RS_PRO:
1540             Serial1.begin(100000, SERIAL_8E2, UNDEF_PIN, serial1TXpin, true);
1541             serial1IO = new SerialSBUS(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX);
1542             break;
1543         case PROTOCOL_SERIAL1_INVERTED_SBUS:
1544             Serial1.begin(100000, SERIAL_8E2, UNDEF_PIN, serial1TXpin, false);
1545             serial1IO = new SerialSBUS(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX);
1546             break;
1547         case PROTOCOL_SERIAL1_SUMD:
1548             Serial1.begin(115200, SERIAL_8N1, UNDEF_PIN, serial1TXpin, false);
1549             serial1IO = new SerialSUMD(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX);
1550             break;
1551         case PROTOCOL_SERIAL1_HOTT_TLM:
1552             Serial1.begin(19200, SERIAL_8N2, serial1RXpin, serial1TXpin, false);
1553             serial1IO = new SerialHoTT_TLM(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX, serial1TXpin);
1554             break;
1555         case PROTOCOL_SERIAL1_TRAMP:
1556             Serial1.begin(9600, SERIAL_8N1, UNDEF_PIN, serial1TXpin, false);
1557             serial1IO = new SerialTramp(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX, serial1TXpin);
1558             break;
1559         case PROTOCOL_SERIAL1_SMARTAUDIO:
1560             Serial1.begin(4800, SERIAL_8N2, UNDEF_PIN, serial1TXpin, false);
1561             serial1IO = new SerialSmartAudio(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX, serial1TXpin);
1562             break;
1563     }
1564 }
1565
1566 void reconfigureSerial1()
1567 {
1568     serial1Shutdown();
1569     setupSerial1();
1570 }
1571 #else
1572     void setupSerial1() {};
1573     void reconfigureSerial1() {};
1574 #endif
1575
1576 static void serialShutdown()
1577 {
1578     SerialLogger = new NullStream();
1579 #ifdef PLATFORM_STM32
1580 #if defined(TARGET_R9SLIMPLUS_RX) || defined(TARGET_RX_GHOST_ATTO_V1)
1581     SERIAL_PROTOCOL_RX.end();
1582 #endif
1583     SERIAL_PROTOCOL_TX.end();
1584 #else
1585     if(serialIO != nullptr)
1586     {
1587         Serial.end();
1588     }
1589 #endif
1590     if(serialIO != nullptr)
1591     {
1592         delete serialIO;
1593         serialIO = nullptr;
1594     }
1595 }
1596
1597 void reconfigureSerial()
1598 {
1599     serialShutdown();
1600     setupSerial();
1601 }
1602
1603 static void setupConfigAndPocCheck()
1604 {
1605     eeprom.Begin();
1606     config.SetStorageProvider(&eeprom); // Pass pointer to the Config class for access to storage
1607     config.Load();
1608
1609     // If bound, track number of plug/unplug cycles to go to binding mode in eeprom
1610     if (config.GetIsBound() && config.GetPowerOnCounter() < 3)
1611     {
1612         config.SetPowerOnCounter(config.GetPowerOnCounter() + 1);
1613         config.Commit();
1614     }
1615
1616     // Set a deferred function to clear the power on counter if the RX has been running for more than 2s
1617     deferExecutionMillis(2000, []() {
1618         if (connectionState != connected && config.GetPowerOnCounter() != 0)
1619         {
1620             config.SetPowerOnCounter(0);
1621             config.Commit();
1622         }
1623     });
1624 }
1625
1626 static void setupTarget()
1627 {
1628     if (GPIO_PIN_ANT_CTRL != UNDEF_PIN)
1629     {
1630         pinMode(GPIO_PIN_ANT_CTRL, OUTPUT);
1631         digitalWrite(GPIO_PIN_ANT_CTRL, LOW);
1632         if (GPIO_PIN_ANT_CTRL_COMPL != UNDEF_PIN)
1633         {
1634             pinMode(GPIO_PIN_ANT_CTRL_COMPL, OUTPUT);
1635             digitalWrite(GPIO_PIN_ANT_CTRL_COMPL, HIGH);
1636         }
1637     }
1638
1639 #if defined(TARGET_RX_FM30_MINI)
1640     pinMode(GPIO_PIN_UART1TX_INVERT, OUTPUT);
1641     digitalWrite(GPIO_PIN_UART1TX_INVERT, LOW);
1642 #endif
1643
1644     setupTargetCommon();
1645 }
1646
1647 static void setupBindingFromConfig()
1648 {
1649     // VolatileBind's only function is to prevent loading the stored UID into RAM
1650     // which makes the RX boot into bind mode every time
1651     if (config.GetIsBound())
1652     {
1653         memcpy(UID, config.GetUID(), UID_LEN);
1654     }
1655
1656     DBGLN("UID=(%d, %d, %d, %d, %d, %d) ModelId=%u",
1657         UID[0], UID[1], UID[2], UID[3], UID[4], UID[5], config.GetModelId());
1658
1659     OtaUpdateCrcInitFromUid();
1660 }
1661
1662 static void setupRadio()
1663 {
1664     Radio.currFreq = FHSSgetInitialFreq();
1665 #if defined(RADIO_SX127X)
1666     //Radio.currSyncWord = UID[3];
1667 #endif
1668     bool init_success = Radio.Begin(FHSSgetMinimumFreq(), FHSSgetMaximumFreq());
1669     POWERMGNT::init();
1670     if (!init_success)
1671     {
1672         DBGLN("Failed to detect RF chipset!!!");
1673         connectionState = radioFailed;
1674         return;
1675     }
1676
1677     DynamicPower_UpdateRx(true);
1678
1679 #if defined(Regulatory_Domain_EU_CE_2400)
1680     LBTEnabled = (config.GetPower() > PWR_10mW);
1681 #endif
1682
1683     Radio.RXdoneCallback = &RXdoneISR;
1684     Radio.TXdoneCallback = &TXdoneISR;
1685
1686     scanIndex = config.GetRateInitialIdx();
1687     SetRFLinkRate(scanIndex, false);
1688     // Start slow on the selected rate to give it the best chance
1689     // to connect before beginning rate cycling
1690     RFmodeCycleMultiplier = RFmodeCycleMultiplierSlow / 2;
1691 }
1692
1693 static void updateTelemetryBurst()
1694 {
1695     if (telemBurstValid)
1696         return;
1697     telemBurstValid = true;
1698
1699     uint16_t hz = 1000000 / ExpressLRS_currAirRate_Modparams->interval;
1700     telemetryBurstMax = TLMBurstMaxForRateRatio(hz, ExpressLRS_currTlmDenom);
1701
1702     // Notify the sender to adjust its expected throughput
1703     TelemetrySender.UpdateTelemetryRate(hz, ExpressLRS_currTlmDenom, telemetryBurstMax);
1704 }
1705
1706 /* If not connected will rotate through the RF modes looking for sync
1707  * and blink LED
1708  */
1709 static void cycleRfMode(unsigned long now)
1710 {
1711     if (connectionState == connected || connectionState == wifiUpdate || InBindingMode)
1712         return;
1713
1714     // Actually cycle the RF mode if not LOCK_ON_FIRST_CONNECTION
1715     if (LockRFmode == false && (now - RFmodeLastCycled) > (cycleInterval * RFmodeCycleMultiplier))
1716     {
1717         RFmodeLastCycled = now;
1718         LastSyncPacket = now;           // reset this variable
1719         SendLinkStatstoFCForcedSends = 2;
1720         SetRFLinkRate(scanIndex % RATE_MAX, false); // switch between rates
1721         LQCalc.reset100();
1722         LQCalcDVDA.reset100();
1723         // Display the current air rate to the user as an indicator something is happening
1724         scanIndex++;
1725         Radio.RXnb();
1726         DBGLN("%u", ExpressLRS_currAirRate_Modparams->interval);
1727
1728         // Switch to FAST_SYNC if not already in it (won't be if was just connected)
1729         RFmodeCycleMultiplier = 1;
1730     } // if time to switch RF mode
1731 }
1732
1733 static void EnterBindingMode()
1734 {
1735     if (InBindingMode)
1736     {
1737         DBGLN("Already in binding mode");
1738         return;
1739     }
1740
1741     // Binding uses a CRCInit=0, 50Hz, and InvertIQ
1742     OtaCrcInitializer = 0;
1743     InBindingMode = true;
1744     // Any method of entering bind resets a loan
1745     // Model can be reloaned immediately by binding now
1746     config.ReturnLoan();
1747     config.Commit();
1748
1749     // Start attempting to bind
1750     // Lock the RF rate and freq while binding
1751     SetRFLinkRate(enumRatetoIndex(RATE_BINDING), true);
1752
1753     // If the Radio Params (including InvertIQ) parameter changed, need to restart RX to take effect
1754     Radio.RXnb();
1755
1756     DBGLN("Entered binding mode at freq = %d", Radio.currFreq);
1757     devicesTriggerEvent();
1758 }
1759
1760 static void ExitBindingMode()
1761 {
1762     if (!InBindingMode)
1763     {
1764         DBGLN("Not in binding mode");
1765         return;
1766     }
1767
1768     MspReceiver.ResetState();
1769
1770     // Prevent any new packets from coming in
1771     Radio.SetTxIdleMode();
1772     // Write the values to eeprom
1773     config.Commit();
1774
1775     OtaUpdateCrcInitFromUid();
1776     FHSSrandomiseFHSSsequence(uidMacSeedGet());
1777
1778     #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
1779     webserverPreventAutoStart = true;
1780     #endif
1781
1782     // Force RF cycling to start at the beginning immediately
1783     scanIndex = RATE_MAX;
1784     RFmodeLastCycled = 0;
1785     LockRFmode = false;
1786     LostConnection(false);
1787
1788     // Do this last as LostConnection() will wait for a tock that never comes
1789     // if we're in binding mode
1790     InBindingMode = false;
1791     DBGLN("Exiting binding mode");
1792     devicesTriggerEvent();
1793 }
1794
1795 static void updateBindingMode(unsigned long now)
1796 {
1797     // Exit binding mode if the config has been modified, indicating UID has been set
1798     if (InBindingMode && config.IsModified())
1799     {
1800         ExitBindingMode();
1801     }
1802
1803 #if defined(RADIO_LR1121)
1804     // Change frequency domains every 500ms.  This will allow single LR1121 receivers to receive bind packets from SX12XX Tx modules.
1805     else if (InBindingMode && (now - BindingRateChangeTime) > BindingRateChangeCyclePeriod)
1806     {
1807         BindingRateChangeTime = now;
1808         if (ExpressLRS_currAirRate_Modparams->index == RATE_DUALBAND_BINDING)
1809         {
1810             SetRFLinkRate(enumRatetoIndex(RATE_BINDING), true);
1811         }
1812         else
1813         {
1814             SetRFLinkRate(RATE_DUALBAND_BINDING, true);
1815         }
1816
1817         Radio.RXnb();
1818     }
1819 #endif
1820
1821     // If the power on counter is >=3, enter binding, the counter will be reset after 2s
1822     else if (!InBindingMode && config.GetPowerOnCounter() >= 3)
1823     {
1824 #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
1825         // Never enter wifi if forced to binding mode
1826         webserverPreventAutoStart = true;
1827 #endif
1828         DBGLN("Power on counter >=3, enter binding mode");
1829         EnterBindingMode();
1830     }
1831
1832     // If the eeprom is indicating that we're not bound, enter binding
1833     else if (!UID_IS_BOUND(UID) && !InBindingMode)
1834     {
1835         DBGLN("RX has not been bound, enter binding mode");
1836         EnterBindingMode();
1837     }
1838
1839     else if (BindingModeRequest)
1840     {
1841         DBGLN("Connected request to enter binding mode");
1842         BindingModeRequest = false;
1843         if (connectionState == connected)
1844         {
1845             LostConnection(false);
1846             // Skip entering bind mode if on loan. This comes from an OTA request
1847             // and the model is assumed to be inaccessible, do not want the receiver
1848             // sitting in a field ready to be bound to anyone within 10km
1849             if (config.IsOnLoan())
1850             {
1851                 DBGLN("Model was on loan, becoming inert");
1852                 config.ReturnLoan();
1853                 config.Commit(); // prevents CheckConfigChangePending() re-enabling radio
1854                 Radio.End();
1855                 // Enter a completely invalid state for a receiver, to prevent wifi or radio enabling
1856                 connectionState = noCrossfire;
1857                 return;
1858             }
1859             // if the InitRate config item was changed by LostConnection
1860             // save the config before entering bind, as the modified config
1861             // will immediately boot it out of bind mode
1862             config.Commit();
1863         }
1864         EnterBindingMode();
1865     }
1866 }
1867
1868 void EnterBindingModeSafely()
1869 {
1870     // Will not enter Binding mode if in the process of a passthrough update
1871     // or currently binding
1872     if (connectionState == serialUpdate || InBindingMode)
1873         return;
1874
1875 #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
1876     // Never enter wifi mode after requesting to enter binding mode
1877     webserverPreventAutoStart = true;
1878
1879     // If the radio and everything is shut down, better to reboot and boot to binding mode
1880     if (connectionState == wifiUpdate || connectionState == bleJoystick)
1881     {
1882         // Force 3-plug binding mode
1883         config.SetPowerOnCounter(3);
1884         config.Commit();
1885         ESP.restart();
1886         // Unreachable
1887     }
1888 #endif
1889
1890     // If connected, handle that in updateBindingMode()
1891     if (connectionState == connected)
1892     {
1893         BindingModeRequest = true;
1894         return;
1895     }
1896
1897     EnterBindingMode();
1898 }
1899
1900 static void checkSendLinkStatsToFc(uint32_t now)
1901 {
1902     if (now - SendLinkStatstoFCintervalLastSent > SEND_LINK_STATS_TO_FC_INTERVAL)
1903     {
1904         if (connectionState == disconnected)
1905         {
1906             getRFlinkInfo();
1907         }
1908
1909         if ((connectionState != disconnected && connectionHasModelMatch && teamraceHasModelMatch) ||
1910             SendLinkStatstoFCForcedSends)
1911         {
1912             serialIO->queueLinkStatisticsPacket();
1913             SendLinkStatstoFCintervalLastSent = now;
1914             if (SendLinkStatstoFCForcedSends)
1915                 --SendLinkStatstoFCForcedSends;
1916         }
1917     }
1918 }
1919
1920 static void debugRcvrLinkstats()
1921 {
1922 #if defined(DEBUG_RCVR_LINKSTATS)
1923     if (debugRcvrLinkstatsPending)
1924     {
1925         debugRcvrLinkstatsPending = false;
1926
1927         // Copy the data out of the ISR-updating bits ASAP
1928         // While YOLOing (const void *) away the volatile
1929         crsfLinkStatistics_t ls = *(crsfLinkStatistics_t *)((const void *)&CRSF::LinkStatistics);
1930         uint32_t packetCounter = debugRcvrLinkstatsPacketId;
1931         uint8_t fhss = debugRcvrLinkstatsFhssIdx;
1932         // actually the previous packet's offset since the update happens in tick, and this will
1933         // fire right after packet reception (a little before tock)
1934         int32_t pfd = PfdPrevRawOffset;
1935
1936         // Use serial instead of DBG() because do not necessarily want all the debug in our logs
1937         char buf[50];
1938         snprintf(buf, sizeof(buf), "%u,%u,-%u,%u,%d,%u,%u,%d\r\n",
1939             packetCounter, ls.active_antenna,
1940             ls.active_antenna ? ls.uplink_RSSI_2 : ls.uplink_RSSI_1,
1941             ls.uplink_Link_quality, ls.uplink_SNR,
1942             ls.uplink_TX_Power, fhss, pfd);
1943         Serial.write(buf);
1944     }
1945 #endif
1946 }
1947
1948 static void debugRcvrSignalStats(uint32_t now)
1949 {
1950 #if defined(DEBUG_RCVR_SIGNAL_STATS)
1951     static uint32_t lastReport = 0;
1952
1953     // log column header:  cnt1, rssi1, snr1, snr1_max, telem1, fail1, cnt2, rssi2, snr2, snr2_max, telem2, fail2, or, both
1954     if(now - lastReport >= 1000 && connectionState == connected)
1955     {
1956         for (int i = 0 ; i < (isDualRadio()?2:1) ; i++)
1957         {
1958             DBG("%d\t%f\t%f\t%f\t%d\t%d\t",
1959                 Radio.rxSignalStats[i].irq_count,
1960                 (Radio.rxSignalStats[i].irq_count==0) ? 0 : double(Radio.rxSignalStats[i].rssi_sum)/Radio.rxSignalStats[i].irq_count,
1961                 (Radio.rxSignalStats[i].irq_count==0) ? 0 : double(Radio.rxSignalStats[i].snr_sum)/Radio.rxSignalStats[i].irq_count/RADIO_SNR_SCALE,
1962                 float(Radio.rxSignalStats[i].snr_max)/RADIO_SNR_SCALE,
1963                 Radio.rxSignalStats[i].telem_count,
1964                 Radio.rxSignalStats[i].fail_count);
1965
1966                 Radio.rxSignalStats[i].irq_count = 0;
1967                 Radio.rxSignalStats[i].snr_sum = 0;
1968                 Radio.rxSignalStats[i].rssi_sum = 0;
1969                 Radio.rxSignalStats[i].snr_max = INT8_MIN;
1970                 Radio.rxSignalStats[i].telem_count = 0;
1971                 Radio.rxSignalStats[i].fail_count = 0;
1972         }
1973         if (isDualRadio())
1974         {
1975             DBGLN("%d\t%d", Radio.irq_count_or, Radio.irq_count_both);
1976         }
1977         else
1978         {
1979             DBGLN("");
1980         }
1981         Radio.irq_count_or = 0;
1982         Radio.irq_count_both = 0;
1983
1984         lastReport = now;
1985     }
1986 #endif
1987 }
1988
1989 static void updateSwitchMode()
1990 {
1991     // Negative value means waiting for confirm of the new switch mode while connected
1992     if (SwitchModePending <= 0)
1993         return;
1994
1995     OtaUpdateSerializers((OtaSwitchMode_e)(SwitchModePending - 1), ExpressLRS_currAirRate_Modparams->PayloadLength);
1996     SwitchModePending = 0;
1997 }
1998
1999 static void CheckConfigChangePending()
2000 {
2001     if (config.IsModified() && !InBindingMode && connectionState < NO_CONFIG_SAVE_STATES)
2002     {
2003         LostConnection(false);
2004         config.Commit();
2005         devicesTriggerEvent();
2006 #if defined(Regulatory_Domain_EU_CE_2400)
2007         LBTEnabled = (config.GetPower() > PWR_10mW);
2008 #endif
2009         Radio.RXnb();
2010     }
2011 }
2012
2013 #if defined(PLATFORM_ESP8266)
2014 // Called from core's user_rf_pre_init() function (which is called by SDK) before setup()
2015 RF_PRE_INIT()
2016 {
2017     // Set whether the chip will do RF calibration or not when power up.
2018     // I believe the Arduino core fakes this (byte 114 of phy_init_data.bin)
2019     // to be 1, but the TX power calibration can pull over 300mA which can
2020     // lock up receivers built with a underspeced LDO (such as the EP2 "SDG")
2021     // Option 2 is just VDD33 measurement
2022     #if defined(RF_CAL_MODE)
2023     system_phy_set_powerup_option(RF_CAL_MODE);
2024     #else
2025     system_phy_set_powerup_option(2);
2026     #endif
2027 }
2028 #endif
2029
2030 void resetConfigAndReboot()
2031 {
2032     config.SetDefaults(true);
2033 #if defined(PLATFORM_STM32)
2034     HAL_NVIC_SystemReset();
2035 #else
2036     // Prevent WDT from rebooting too early if
2037     // all this flash write is taking too long
2038     yield();
2039     // Remove options.json and hardware.json
2040     SPIFFS.format();
2041     yield();
2042     SPIFFS.begin();
2043     options_SetTrueDefaults();
2044
2045     ESP.restart();
2046 #endif
2047 }
2048
2049 void setup()
2050 {
2051     #if defined(TARGET_UNIFIED_RX)
2052     hardwareConfigured = options_init();
2053     if (!hardwareConfigured)
2054     {
2055         // In the failure case we set the logging to the null logger so nothing crashes
2056         // if it decides to log something
2057         SerialLogger = new NullStream();
2058
2059         // Register the WiFi with the framework
2060         static device_affinity_t wifi_device[] = {
2061             {&WIFI_device, 1}
2062         };
2063         devicesRegister(wifi_device, ARRAY_SIZE(wifi_device));
2064         devicesInit();
2065
2066         connectionState = hardwareUndefined;
2067     }
2068     #else
2069     hardwareConfigured = options_init();
2070     #endif
2071
2072     if (hardwareConfigured)
2073     {
2074         // default to CRSF protocol and the compiled baud rate
2075         serialBaud = firmwareOptions.uart_baud;
2076
2077         // pre-initialise serial must be done before anything as some libs write
2078         // to the serial port and they'll block if the buffer fills
2079         #if defined(DEBUG_LOG)
2080         Serial.begin(serialBaud);
2081         SerialLogger = &Serial;
2082         #else
2083         SerialLogger = new NullStream();
2084         #endif
2085
2086         // External EEPROM needs I2C setup so it can load config
2087         // but configurable I2C pins for PWM RX needs config loaded first
2088 #if (defined(TARGET_USE_EEPROM) && defined(USE_I2C))
2089         setupTarget();
2090 #endif
2091         // Init EEPROM and load config, checking powerup count
2092         setupConfigAndPocCheck();
2093 #if !(defined(TARGET_USE_EEPROM) && defined(USE_I2C))
2094         setupTarget();
2095 #endif
2096
2097         #if defined(OPT_HAS_SERVO_OUTPUT)
2098         // If serial is not already defined, then see if there is serial pin configured in the PWM configuration
2099         if (GPIO_PIN_RCSIGNAL_RX == UNDEF_PIN && GPIO_PIN_RCSIGNAL_TX == UNDEF_PIN)
2100         {
2101             for (int i = 0 ; i < GPIO_PIN_PWM_OUTPUTS_COUNT ; i++)
2102             {
2103                 eServoOutputMode pinMode = (eServoOutputMode)config.GetPwmChannel(i)->val.mode;
2104                 if (pinMode == somSerial)
2105                 {
2106                     pwmSerialDefined = true;
2107                     break;
2108                 }
2109             }
2110         }
2111         #endif
2112         setupSerial();
2113         setupSerial1();
2114
2115         devicesRegister(ui_devices, ARRAY_SIZE(ui_devices));
2116         devicesInit();
2117
2118         setupBindingFromConfig();
2119
2120         FHSSrandomiseFHSSsequence(uidMacSeedGet());
2121
2122         setupRadio();
2123
2124         if (connectionState != radioFailed)
2125         {
2126             // RFnoiseFloor = MeasureNoiseFloor(); //TODO move MeasureNoiseFloor to driver libs
2127             // DBGLN("RF noise floor: %d dBm", RFnoiseFloor);
2128
2129             MspReceiver.SetDataToReceive(MspData, ELRS_MSP_BUFFER);
2130             Radio.RXnb();
2131             hwTimer::init(HWtimerCallbackTick, HWtimerCallbackTock);
2132         }
2133     }
2134
2135 #if defined(HAS_BUTTON)
2136     registerButtonFunction(ACTION_BIND, EnterBindingModeSafely);
2137     registerButtonFunction(ACTION_RESET_REBOOT, resetConfigAndReboot);
2138 #endif
2139
2140     devicesStart();
2141
2142     // setup() eats up some of this time, which can cause the first mode connection to fail.
2143     // Resetting the time here give the first mode a better chance of connection.
2144     RFmodeLastCycled = millis();
2145 }
2146
2147 #if defined(PLATFORM_ESP32_C3)
2148 void main_loop()
2149 #else
2150 void loop()
2151 #endif
2152 {
2153     unsigned long now = millis();
2154
2155     if (MspReceiver.HasFinishedData())
2156     {
2157         MspReceiveComplete();
2158     }
2159
2160     devicesUpdate(now);
2161
2162 #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
2163     // If the reboot time is set and the current time is past the reboot time then reboot.
2164     if (rebootTime != 0 && now > rebootTime) {
2165         ESP.restart();
2166     }
2167     #endif
2168
2169     CheckConfigChangePending();
2170     executeDeferredFunction(micros());
2171
2172     // Clear the power-on-count
2173     if ((connectionState == connected || connectionState == tentative) && config.GetPowerOnCounter() != 0)
2174     {
2175         config.SetPowerOnCounter(0);
2176     }
2177
2178     if (connectionState > MODE_STATES)
2179     {
2180         return;
2181     }
2182
2183     if ((connectionState != disconnected) && (ExpressLRS_currAirRate_Modparams->index != ExpressLRS_nextAirRateIndex)){ // forced change
2184         DBGLN("Req air rate change %u->%u", ExpressLRS_currAirRate_Modparams->index, ExpressLRS_nextAirRateIndex);
2185         LostConnection(true);
2186         LastSyncPacket = now;           // reset this variable to stop rf mode switching and add extra time
2187         RFmodeLastCycled = now;         // reset this variable to stop rf mode switching and add extra time
2188         SendLinkStatstoFCintervalLastSent = 0;
2189         SendLinkStatstoFCForcedSends = 2;
2190     }
2191
2192     if (connectionState == tentative && (now - LastSyncPacket > ExpressLRS_currAirRate_RFperfParams->RxLockTimeoutMs))
2193     {
2194         DBGLN("Bad sync, aborting");
2195         LostConnection(true);
2196         RFmodeLastCycled = now;
2197         LastSyncPacket = now;
2198     }
2199
2200     cycleRfMode(now);
2201
2202     uint32_t localLastValidPacket = LastValidPacket; // Required to prevent race condition due to LastValidPacket getting updated from ISR
2203     if ((connectionState == connected) && ((int32_t)ExpressLRS_currAirRate_RFperfParams->DisconnectTimeoutMs < (int32_t)(now - localLastValidPacket))) // check if we lost conn.
2204     {
2205         LostConnection(true);
2206     }
2207
2208     if ((connectionState == tentative) && (abs(LPF_OffsetDx.value()) <= 10) && (LPF_Offset.value() < 100) && (LQCalc.getLQRaw() > minLqForChaos())) //detects when we are connected
2209     {
2210         GotConnection(now);
2211     }
2212
2213     checkSendLinkStatsToFc(now);
2214
2215     if ((RXtimerState == tim_tentative) && ((now - GotConnectionMillis) > ConsiderConnGoodMillis) && (abs(LPF_OffsetDx.value()) <= 5))
2216     {
2217         RXtimerState = tim_locked;
2218         DBGLN("Timer locked");
2219     }
2220
2221     uint8_t *nextPayload = 0;
2222     uint8_t nextPlayloadSize = 0;
2223     if (!TelemetrySender.IsActive() && telemetry.GetNextPayload(&nextPlayloadSize, &nextPayload))
2224     {
2225         TelemetrySender.SetDataToTransmit(nextPayload, nextPlayloadSize);
2226     }
2227
2228     uint16_t count = mavlinkInputBuffer.size();
2229     if (count > 0 && !TelemetrySender.IsActive())
2230     {
2231         count = std::min(count, (uint16_t)CRSF_PAYLOAD_SIZE_MAX); // Constrain to CRSF max payload size to match SS
2232         // First 2 bytes conform to crsf_header_s format
2233         mavlinkSSBuffer[0] = CRSF_ADDRESS_USB; // device_addr - used on TX to differentiate between std tlm and mavlink
2234         mavlinkSSBuffer[1] = count;
2235         // Following n bytes are just raw mavlink
2236         mavlinkInputBuffer.popBytes(mavlinkSSBuffer + CRSF_FRAME_NOT_COUNTED_BYTES, count);
2237         nextPayload = mavlinkSSBuffer;
2238         nextPlayloadSize = count + CRSF_FRAME_NOT_COUNTED_BYTES;
2239         TelemetrySender.SetDataToTransmit(nextPayload, nextPlayloadSize);
2240     }
2241
2242     updateTelemetryBurst();
2243     updateBindingMode(now);
2244     updateSwitchMode();
2245     checkGeminiMode();
2246     DynamicPower_UpdateRx(false);
2247     debugRcvrLinkstats();
2248     debugRcvrSignalStats(now);
2249 }
2250
2251 #if defined(PLATFORM_ESP32_C3)
2252 [[noreturn]] void loop()
2253 {
2254     // We have to do this dodgy hack because on the C3 the Arduino main loop calls
2255     // a yield function (vTaskDelay) every 2 seconds, which causes us to lose connection!
2256     extern bool loopTaskWDTEnabled;
2257     for (;;) {
2258         if (loopTaskWDTEnabled) {
2259             esp_task_wdt_reset();
2260         }
2261         main_loop();
2262     }
2263 }
2264 #endif
2265
2266 struct bootloader {
2267     uint32_t key;
2268     uint32_t reset_type;
2269 };
2270
2271 void reset_into_bootloader(void)
2272 {
2273     SERIAL_PROTOCOL_TX.println((const char *)&target_name[4]);
2274     SERIAL_PROTOCOL_TX.flush();
2275 #if defined(PLATFORM_STM32)
2276     delay(100);
2277     DBGLN("Jumping to Bootloader...");
2278     delay(100);
2279
2280     /** Write command for firmware update.
2281      *
2282      * Bootloader checks this memory area (if newer enough) and
2283      * perpare itself for fw update. Otherwise it skips the check
2284      * and starts ELRS firmware immediately
2285      */
2286     extern __IO uint32_t _bootloader_data;
2287     volatile struct bootloader * blinfo = ((struct bootloader*)&_bootloader_data) + 0;
2288     blinfo->key = 0x454c5253; // ELRS
2289     blinfo->reset_type = 0xACDC;
2290
2291     HAL_NVIC_SystemReset();
2292 #elif defined(PLATFORM_ESP8266)
2293     delay(100);
2294     ESP.rebootIntoUartDownloadMode();
2295 #elif defined(PLATFORM_ESP32)
2296     delay(100);
2297     connectionState = serialUpdate;
2298 #endif
2299 }