src/src/tx_main.cpp

   1 #include "rxtx_common.h"
   2
   3 #include "CRSFHandset.h"
   4 #include "dynpower.h"
   5 #include "lua.h"
   6 #include "msp.h"
   7 #include "msptypes.h"
   8 #include "telemetry_protocol.h"
   9 #include "stubborn_receiver.h"
  10 #include "stubborn_sender.h"
  11
  12 #include "devHandset.h"
  13 #include "devLED.h"
  14 #include "devScreen.h"
  15 #include "devBuzzer.h"
  16 #include "devBLE.h"
  17 #include "devLUA.h"
  18 #include "devWIFI.h"
  19 #include "devButton.h"
  20 #include "devVTX.h"
  21 #include "devGsensor.h"
  22 #include "devThermal.h"
  23 #include "devPDET.h"
  24 #include "devBackpack.h"
  25
  26 //// CONSTANTS ////
  27 #define MSP_PACKET_SEND_INTERVAL 10LU
  28
  29 /// define some libs to use ///
  30 MSP msp;
  31 ELRS_EEPROM eeprom;
  32 TxConfig config;
  33 Stream *TxBackpack;
  34 Stream *TxUSB;
  35
  36 // Variables / constants for Airport //
  37 FIFO<AP_MAX_BUF_LEN> apInputBuffer;
  38 FIFO<AP_MAX_BUF_LEN> apOutputBuffer;
  39
  40 #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
  41 unsigned long rebootTime = 0;
  42 extern bool webserverPreventAutoStart;
  43 #endif
  44 //// MSP Data Handling ///////
  45 bool NextPacketIsMspData = false;  // if true the next packet will contain the msp data
  46 char backpackVersion[32] = "";
  47
  48 ////////////SYNC PACKET/////////
  49 /// sync packet spamming on mode change vars ///
  50 #define syncSpamAResidualTimeMS 500 // we spam some more after rate change to help link get up to speed
  51 #define syncSpamAmount 3
  52 volatile uint8_t syncSpamCounter = 0;
  53 uint32_t rfModeLastChangedMS = 0;
  54 uint32_t SyncPacketLastSent = 0;
  55 ////////////////////////////////////////////////
  56
  57 volatile uint32_t LastTLMpacketRecvMillis = 0;
  58 uint32_t TLMpacketReported = 0;
  59 static bool commitInProgress = false;
  60
  61 LQCALC<25> LQCalc;
  62
  63 volatile bool busyTransmitting;
  64 static volatile bool ModelUpdatePending;
  65
  66 uint8_t MSPDataPackage[5];
  67 static uint8_t BindingSendCount;
  68 bool RxWiFiReadyToSend = false;
  69
  70 bool headTrackingEnabled = false;
  71 #if !defined(CRITICAL_FLASH)
  72 static uint16_t ptrChannelData[3] = {CRSF_CHANNEL_VALUE_MID, CRSF_CHANNEL_VALUE_MID, CRSF_CHANNEL_VALUE_MID};
  73 static uint32_t lastPTRValidTimeMs;
  74 #endif
  75
  76 static TxTlmRcvPhase_e TelemetryRcvPhase = ttrpTransmitting;
  77 StubbornReceiver TelemetryReceiver;
  78 StubbornSender MspSender;
  79 uint8_t CRSFinBuffer[CRSF_MAX_PACKET_LEN+1];
  80
  81 device_affinity_t ui_devices[] = {
  82   {&Handset_device, 1},
  83 #ifdef HAS_LED
  84   {&LED_device, 0},
  85 #endif
  86 #ifdef HAS_RGB
  87   {&RGB_device, 0},
  88 #endif
  89   {&LUA_device, 1},
  90 #if defined(USE_TX_BACKPACK)
  91   {&Backpack_device, 0},
  92 #endif
  93 #ifdef HAS_BLE
  94   {&BLE_device, 0},
  95 #endif
  96 #ifdef HAS_BUZZER
  97   {&Buzzer_device, 0},
  98 #endif
  99 #ifdef HAS_WIFI
 100   {&WIFI_device, 0},
 101 #endif
 102 #ifdef HAS_BUTTON
 103   {&Button_device, 0},
 104 #endif
 105 #ifdef HAS_SCREEN
 106   {&Screen_device, 0},
 107 #endif
 108 #ifdef HAS_GSENSOR
 109   {&Gsensor_device, 0},
 110 #endif
 111 #if defined(HAS_THERMAL) || defined(HAS_FAN)
 112   {&Thermal_device, 0},
 113 #endif
 114 #if defined(GPIO_PIN_PA_PDET)
 115   {&PDET_device, 0},
 116 #endif
 117   {&VTX_device, 0}
 118 };
 119
 120 #if defined(GPIO_PIN_ANT_CTRL)
 121     static bool diversityAntennaState = LOW;
 122 #endif
 123
 124 #ifdef TARGET_TX_GHOST
 125 extern "C"
 126 /**
 127   * @brief This function handles external line 2 interrupt request.
 128   * @param  None
 129   * @retval None
 130   */
 131 void EXTI2_TSC_IRQHandler()
 132 {
 133   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
 134 }
 135 #endif
 136
 137 void switchDiversityAntennas()
 138 {
 139   if (GPIO_PIN_ANT_CTRL != UNDEF_PIN)
 140   {
 141     diversityAntennaState = !diversityAntennaState;
 142     digitalWrite(GPIO_PIN_ANT_CTRL, diversityAntennaState);
 143   }
 144   if (GPIO_PIN_ANT_CTRL_COMPL != UNDEF_PIN)
 145   {
 146     digitalWrite(GPIO_PIN_ANT_CTRL_COMPL, !diversityAntennaState);
 147   }
 148 }
 149
 150 void ICACHE_RAM_ATTR LinkStatsFromOta(OTA_LinkStats_s * const ls)
 151 {
 152   int8_t snrScaled = ls->SNR;
 153   DynamicPower_TelemetryUpdate(snrScaled);
 154
 155   // Antenna is the high bit in the RSSI_1 value
 156   // RSSI received is signed, inverted polarity (positive value = -dBm)
 157   // OpenTX's value is signed and will display +dBm and -dBm properly
 158   CRSF::LinkStatistics.uplink_RSSI_1 = -(ls->uplink_RSSI_1);
 159   CRSF::LinkStatistics.uplink_RSSI_2 = -(ls->uplink_RSSI_2);
 160   CRSF::LinkStatistics.uplink_Link_quality = ls->lq;
 161 #if defined(DEBUG_FREQ_CORRECTION)
 162   // Don't descale the FreqCorrection value being send in SNR
 163   CRSF::LinkStatistics.uplink_SNR = snrScaled;
 164 #else
 165   CRSF::LinkStatistics.uplink_SNR = SNR_DESCALE(snrScaled);
 166 #endif
 167   CRSF::LinkStatistics.active_antenna = ls->antenna;
 168   connectionHasModelMatch = ls->modelMatch;
 169   // -- downlink_SNR / downlink_RSSI is updated for any packet received, not just Linkstats
 170   // -- uplink_TX_Power is updated when sending to the handset, so it updates when missing telemetry
 171   // -- rf_mode is updated when we change rates
 172   // -- downlink_Link_quality is updated before the LQ period is incremented
 173   MspSender.ConfirmCurrentPayload(ls->mspConfirm);
 174 }
 175
 176 bool ICACHE_RAM_ATTR ProcessTLMpacket(SX12xxDriverCommon::rx_status const status)
 177 {
 178   if (status != SX12xxDriverCommon::SX12XX_RX_OK)
 179   {
 180     DBGLN("TLM HW CRC error");
 181     return false;
 182   }
 183
 184   OTA_Packet_s * const otaPktPtr = (OTA_Packet_s * const)Radio.RXdataBuffer;
 185   if (!OtaValidatePacketCrc(otaPktPtr))
 186   {
 187     DBGLN("TLM crc error");
 188     return false;
 189   }
 190
 191   if (otaPktPtr->std.type != PACKET_TYPE_TLM)
 192   {
 193     DBGLN("TLM type error %d", otaPktPtr->std.type);
 194     return false;
 195   }
 196
 197   LastTLMpacketRecvMillis = millis();
 198   LQCalc.add();
 199
 200   Radio.GetLastPacketStats();
 201   CRSF::LinkStatistics.downlink_SNR = SNR_DESCALE(Radio.LastPacketSNRRaw);
 202   CRSF::LinkStatistics.downlink_RSSI = Radio.LastPacketRSSI;
 203
 204   // Full res mode
 205   if (OtaIsFullRes)
 206   {
 207     OTA_Packet8_s * const ota8 = (OTA_Packet8_s * const)otaPktPtr;
 208     uint8_t *telemPtr;
 209     uint8_t dataLen;
 210     if (ota8->tlm_dl.containsLinkStats)
 211     {
 212       LinkStatsFromOta(&ota8->tlm_dl.ul_link_stats.stats);
 213       telemPtr = ota8->tlm_dl.ul_link_stats.payload;
 214       dataLen = sizeof(ota8->tlm_dl.ul_link_stats.payload);
 215     }
 216     else
 217     {
 218       if (firmwareOptions.is_airport)
 219       {
 220         OtaUnpackAirportData(otaPktPtr, &apOutputBuffer);
 221         return true;
 222       }
 223       telemPtr = ota8->tlm_dl.payload;
 224       dataLen = sizeof(ota8->tlm_dl.payload);
 225     }
 226     //DBGLN("pi=%u len=%u", ota8->tlm_dl.packageIndex, dataLen);
 227     TelemetryReceiver.ReceiveData(ota8->tlm_dl.packageIndex, telemPtr, dataLen);
 228   }
 229   // Std res mode
 230   else
 231   {
 232     switch (otaPktPtr->std.tlm_dl.type)
 233     {
 234       case ELRS_TELEMETRY_TYPE_LINK:
 235         LinkStatsFromOta(&otaPktPtr->std.tlm_dl.ul_link_stats.stats);
 236         break;
 237
 238       case ELRS_TELEMETRY_TYPE_DATA:
 239         if (firmwareOptions.is_airport)
 240         {
 241           OtaUnpackAirportData(otaPktPtr, &apOutputBuffer);
 242           return true;
 243         }
 244         TelemetryReceiver.ReceiveData(otaPktPtr->std.tlm_dl.packageIndex,
 245           otaPktPtr->std.tlm_dl.payload,
 246           sizeof(otaPktPtr->std.tlm_dl.payload));
 247         break;
 248     }
 249   }
 250   return true;
 251 }
 252
 253 expresslrs_tlm_ratio_e ICACHE_RAM_ATTR UpdateTlmRatioEffective()
 254 {
 255   expresslrs_tlm_ratio_e ratioConfigured = (expresslrs_tlm_ratio_e)config.GetTlm();
 256   // default is suggested rate for TLM_RATIO_STD/TLM_RATIO_DISARMED
 257   expresslrs_tlm_ratio_e retVal = ExpressLRS_currAirRate_Modparams->TLMinterval;
 258   bool updateTelemDenom = true;
 259
 260   // TLM ratio is boosted for one sync cycle when the MspSender goes active
 261   if (MspSender.IsActive())
 262   {
 263     retVal = TLM_RATIO_1_2;
 264   }
 265   // If Armed, telemetry is disabled, otherwise use STD
 266   else if (ratioConfigured == TLM_RATIO_DISARMED)
 267   {
 268     if (handset->IsArmed())
 269     {
 270       retVal = TLM_RATIO_NO_TLM;
 271       // Avoid updating ExpressLRS_currTlmDenom until connectionState == disconnected
 272       if (connectionState == connected)
 273         updateTelemDenom = false;
 274     }
 275   }
 276   else if (ratioConfigured != TLM_RATIO_STD)
 277   {
 278     retVal = ratioConfigured;
 279   }
 280
 281   if (updateTelemDenom)
 282   {
 283     uint8_t newTlmDenom = TLMratioEnumToValue(retVal);
 284     // Delay going into disconnected state when the TLM ratio increases
 285     if (connectionState == connected && ExpressLRS_currTlmDenom > newTlmDenom)
 286       LastTLMpacketRecvMillis = SyncPacketLastSent;
 287     ExpressLRS_currTlmDenom = newTlmDenom;
 288   }
 289
 290   return retVal;
 291 }
 292
 293 void ICACHE_RAM_ATTR GenerateSyncPacketData(OTA_Sync_s * const syncPtr)
 294 {
 295   const uint8_t SwitchEncMode = config.GetSwitchMode();
 296   const uint8_t Index = (syncSpamCounter) ? config.GetRate() : ExpressLRS_currAirRate_Modparams->index;
 297
 298   if (syncSpamCounter)
 299     --syncSpamCounter;
 300   SyncPacketLastSent = millis();
 301
 302   expresslrs_tlm_ratio_e newTlmRatio = UpdateTlmRatioEffective();
 303
 304   syncPtr->fhssIndex = FHSSgetCurrIndex();
 305   syncPtr->nonce = OtaNonce;
 306   syncPtr->rateIndex = Index;
 307   syncPtr->newTlmRatio = newTlmRatio - TLM_RATIO_NO_TLM;
 308   syncPtr->switchEncMode = SwitchEncMode;
 309   syncPtr->UID3 = UID[3];
 310   syncPtr->UID4 = UID[4];
 311   syncPtr->UID5 = UID[5];
 312
 313   // For model match, the last byte of the binding ID is XORed with the inverse of the modelId
 314   if (!InBindingMode && config.GetModelMatch())
 315   {
 316     syncPtr->UID5 ^= (~CRSFHandset::getModelID()) & MODELMATCH_MASK;
 317   }
 318 }
 319
 320 uint8_t adjustPacketRateForBaud(uint8_t rateIndex)
 321 {
 322   #if defined(RADIO_SX128X)
 323     if (CRSFHandset::GetCurrentBaudRate() == 115200 && GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX) // Packet rate limited to 150Hz if we are on 115k baud on external module
 324     {
 325       rateIndex = get_elrs_HandsetRate_max(rateIndex, 6666);
 326     }
 327     else if (CRSFHandset::GetCurrentBaudRate() == 115200) // Packet rate limited to 250Hz if we are on 115k baud (on internal module)
 328     {
 329       rateIndex = get_elrs_HandsetRate_max(rateIndex, 4000);
 330     }
 331     else if (CRSFHandset::GetCurrentBaudRate() == 400000 && GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX) // Packet rate limited to 333Hz if we are on 400k baud on external module
 332     {
 333       rateIndex = get_elrs_HandsetRate_max(rateIndex, 3003);
 334     }
 335     else if (CRSFHandset::GetCurrentBaudRate() == 400000) // Packet rate limited to 500Hz if we are on 400k baud
 336     {
 337       rateIndex = get_elrs_HandsetRate_max(rateIndex, 2000);
 338     }
 339   #endif
 340   return rateIndex;
 341 }
 342
 343 void SetRFLinkRate(uint8_t index) // Set speed of RF link
 344 {
 345   expresslrs_mod_settings_s *const ModParams = get_elrs_airRateConfig(index);
 346   expresslrs_rf_pref_params_s *const RFperf = get_elrs_RFperfParams(index);
 347   // Binding always uses invertIQ
 348   bool invertIQ = InBindingMode || (UID[5] & 0x01);
 349   OtaSwitchMode_e newSwitchMode = (OtaSwitchMode_e)config.GetSwitchMode();
 350
 351   if ((ModParams == ExpressLRS_currAirRate_Modparams)
 352     && (RFperf == ExpressLRS_currAirRate_RFperfParams)
 353     && (invertIQ == Radio.IQinverted)
 354     && (OtaSwitchModeCurrent == newSwitchMode))
 355     return;
 356
 357   DBGLN("set rate %u", index);
 358   uint32_t interval = ModParams->interval;
 359 #if defined(DEBUG_FREQ_CORRECTION) && defined(RADIO_SX128X)
 360   interval = interval * 12 / 10; // increase the packet interval by 20% to allow adding packet header
 361 #endif
 362   hwTimer::updateInterval(interval);
 363
 364   FHSSusePrimaryFreqBand = !(ModParams->radio_type == RADIO_TYPE_LR1121_LORA_2G4);
 365   FHSSuseDualBand = ModParams->radio_type == RADIO_TYPE_LR1121_LORA_DUAL;
 366
 367   Radio.Config(ModParams->bw, ModParams->sf, ModParams->cr, FHSSgetInitialFreq(),
 368                ModParams->PreambleLen, invertIQ, ModParams->PayloadLength, ModParams->interval
 369 #if defined(RADIO_SX128X)
 370                , uidMacSeedGet(), OtaCrcInitializer, (ModParams->radio_type == RADIO_TYPE_SX128x_FLRC)
 371 #endif
 372                );
 373
 374 #if defined(RADIO_LR1121)
 375   if (FHSSuseDualBand)
 376   {
 377     Radio.Config(ModParams->bw2, ModParams->sf2, ModParams->cr2, FHSSgetInitialGeminiFreq(),
 378                 ModParams->PreambleLen2, invertIQ, ModParams->PayloadLength, ModParams->interval, SX12XX_Radio_2);
 379   }
 380 #endif
 381
 382   Radio.FuzzySNRThreshold = (RFperf->DynpowerSnrThreshUp == DYNPOWER_SNR_THRESH_NONE) ? 0 : (RFperf->DynpowerSnrThreshUp - RFperf->DynpowerSnrThreshDn);
 383
 384   if ((isDualRadio() && config.GetAntennaMode() == TX_RADIO_MODE_GEMINI) || FHSSuseDualBand) // Gemini mode
 385   {
 386     Radio.SetFrequencyReg(FHSSgetInitialGeminiFreq(), SX12XX_Radio_2);
 387   }
 388
 389   OtaUpdateSerializers(newSwitchMode, ModParams->PayloadLength);
 390   MspSender.setMaxPackageIndex(ELRS_MSP_MAX_PACKAGES);
 391   TelemetryReceiver.setMaxPackageIndex(OtaIsFullRes ? ELRS8_TELEMETRY_MAX_PACKAGES : ELRS4_TELEMETRY_MAX_PACKAGES);
 392
 393   ExpressLRS_currAirRate_Modparams = ModParams;
 394   ExpressLRS_currAirRate_RFperfParams = RFperf;
 395   CRSF::LinkStatistics.rf_Mode = ModParams->enum_rate;
 396
 397   handset->setPacketInterval(interval * ExpressLRS_currAirRate_Modparams->numOfSends);
 398   connectionState = disconnected;
 399   rfModeLastChangedMS = millis();
 400 }
 401
 402 void ICACHE_RAM_ATTR HandleFHSS()
 403 {
 404   uint8_t modresult = (OtaNonce + 1) % ExpressLRS_currAirRate_Modparams->FHSShopInterval;
 405   // If the next packet should be on the next FHSS frequency, do the hop
 406   if (!InBindingMode && modresult == 0)
 407   {
 408     // Gemini mode
 409     // If using DualBand always set the correct frequency band to the radios.  The HighFreq/LowFreq Tx amp is set during config.
 410     if ((isDualRadio() && config.GetAntennaMode() == TX_RADIO_MODE_GEMINI) || FHSSuseDualBand)
 411     {
 412       Radio.SetFrequencyReg(FHSSgetNextFreq(), SX12XX_Radio_1);
 413       Radio.SetFrequencyReg(FHSSgetGeminiFreq(), SX12XX_Radio_2);
 414     }
 415     else
 416     {
 417       Radio.SetFrequencyReg(FHSSgetNextFreq());
 418     }
 419   }
 420 }
 421
 422 void ICACHE_RAM_ATTR HandlePrepareForTLM()
 423 {
 424   // If TLM enabled and next packet is going to be telemetry, start listening to have a large receive window (time-wise)
 425   if (ExpressLRS_currTlmDenom != 1 && ((OtaNonce + 1) % ExpressLRS_currTlmDenom) == 0)
 426   {
 427     Radio.RXnb();
 428     TelemetryRcvPhase = ttrpPreReceiveGap;
 429   }
 430 }
 431
 432 void injectBackpackPanTiltRollData(uint32_t const now)
 433 {
 434 #if !defined(CRITICAL_FLASH)
 435   // Do not override channels if the backpack is NOT communicating or PanTiltRoll is disabled
 436   if (config.GetPTREnableChannel() == HT_OFF || backpackVersion[0] == 0)
 437   {
 438     return;
 439   }
 440
 441   uint8_t ptrStartChannel = config.GetPTRStartChannel();
 442   bool enable = config.GetPTREnableChannel() == HT_ON;
 443   if (!enable)
 444   {
 445     uint8_t chan = CRSF_to_BIT(ChannelData[config.GetPTREnableChannel() / 2 + 3]);
 446     if (config.GetPTREnableChannel() % 2 == 0)
 447     {
 448       enable |= chan;
 449     }
 450     else
 451     {
 452       enable |= !chan;
 453     }
 454   }
 455   if (enable != headTrackingEnabled)
 456   {
 457     headTrackingEnabled = enable;
 458     HTEnableFlagReadyToSend = true;
 459   }
 460   // If enabled and this packet is less that 1 second old then use it
 461   if (enable && now - lastPTRValidTimeMs < 1000)
 462   {
 463     ChannelData[ptrStartChannel + 4] = ptrChannelData[0];
 464     ChannelData[ptrStartChannel + 5] = ptrChannelData[1];
 465     ChannelData[ptrStartChannel + 6] = ptrChannelData[2];
 466   }
 467   else
 468   {
 469     ChannelData[ptrStartChannel + 4] = CRSF_CHANNEL_VALUE_MID;
 470     ChannelData[ptrStartChannel + 5] = CRSF_CHANNEL_VALUE_MID;
 471     ChannelData[ptrStartChannel + 6] = CRSF_CHANNEL_VALUE_MID;
 472   }
 473 #endif
 474 }
 475
 476 void ICACHE_RAM_ATTR SendRCdataToRF()
 477 {
 478   uint32_t const now = millis();
 479   // ESP requires word aligned buffer
 480   WORD_ALIGNED_ATTR OTA_Packet_s otaPkt = {0};
 481   static uint8_t syncSlot;
 482
 483   const bool isTlmDisarmed = config.GetTlm() == TLM_RATIO_DISARMED;
 484   uint32_t SyncInterval = (connectionState == connected && !isTlmDisarmed) ? ExpressLRS_currAirRate_RFperfParams->SyncPktIntervalConnected : ExpressLRS_currAirRate_RFperfParams->SyncPktIntervalDisconnected;
 485   bool skipSync = InBindingMode ||
 486     // TLM_RATIO_DISARMED keeps sending sync packets even when armed until the RX stops sending telemetry and the TLM=Off has taken effect
 487     (isTlmDisarmed && handset->IsArmed() && (ExpressLRS_currTlmDenom == 1));
 488
 489   uint8_t NonceFHSSresult = OtaNonce % ExpressLRS_currAirRate_Modparams->FHSShopInterval;
 490   bool WithinSyncSpamResidualWindow = now - rfModeLastChangedMS < syncSpamAResidualTimeMS;
 491
 492   // Sync spam only happens on slot 1 and 2 and can't be disabled
 493   if ((syncSpamCounter || WithinSyncSpamResidualWindow) && (NonceFHSSresult == 1 || NonceFHSSresult == 2))
 494   {
 495     otaPkt.std.type = PACKET_TYPE_SYNC;
 496     GenerateSyncPacketData(OtaIsFullRes ? &otaPkt.full.sync.sync : &otaPkt.std.sync);
 497     syncSlot = 0; // reset the sync slot in case the new rate (after the syncspam) has a lower FHSShopInterval
 498   }
 499   // Regular sync rotates through 4x slots, twice on each slot, and telemetry pushes it to the next slot up
 500   // But only on the sync FHSS channel and with a timed delay between them
 501   else if ((!skipSync) && ((syncSlot / 2) <= NonceFHSSresult) && (now - SyncPacketLastSent > SyncInterval) && FHSSonSyncChannel())
 502   {
 503     otaPkt.std.type = PACKET_TYPE_SYNC;
 504     GenerateSyncPacketData(OtaIsFullRes ? &otaPkt.full.sync.sync : &otaPkt.std.sync);
 505     syncSlot = (syncSlot + 1) % (ExpressLRS_currAirRate_Modparams->FHSShopInterval * 2);
 506   }
 507   else
 508   {
 509     if (NextPacketIsMspData && MspSender.IsActive())
 510     {
 511       otaPkt.std.type = PACKET_TYPE_MSPDATA;
 512       if (OtaIsFullRes)
 513       {
 514         otaPkt.full.msp_ul.packageIndex = MspSender.GetCurrentPayload(
 515           otaPkt.full.msp_ul.payload,
 516           sizeof(otaPkt.full.msp_ul.payload));
 517       }
 518       else
 519       {
 520         otaPkt.std.msp_ul.packageIndex = MspSender.GetCurrentPayload(
 521           otaPkt.std.msp_ul.payload,
 522           sizeof(otaPkt.std.msp_ul.payload));
 523       }
 524
 525       // send channel data next so the channel messages also get sent during msp transmissions
 526       NextPacketIsMspData = false;
 527       // counter can be increased even for normal msp messages since it's reset if a real bind message should be sent
 528       BindingSendCount++;
 529       // If the telemetry ratio isn't already 1:2, send a sync packet to boost it
 530       // to add bandwidth for the reply
 531       if (ExpressLRS_currTlmDenom != 2)
 532         syncSpamCounter = 1;
 533     }
 534     else
 535     {
 536       // always enable msp after a channel package since the slot is only used if MspSender has data to send
 537       NextPacketIsMspData = true;
 538
 539       if (firmwareOptions.is_airport)
 540       {
 541         OtaPackAirportData(&otaPkt, &apInputBuffer);
 542       }
 543       else
 544       {
 545         injectBackpackPanTiltRollData(now);
 546         OtaPackChannelData(&otaPkt, ChannelData, TelemetryReceiver.GetCurrentConfirm(), ExpressLRS_currTlmDenom);
 547       }
 548     }
 549   }
 550
 551   ///// Next, Calculate the CRC and put it into the buffer /////
 552   OtaGeneratePacketCrc(&otaPkt);
 553
 554   SX12XX_Radio_Number_t transmittingRadio = Radio.GetLastSuccessfulPacketRadio();
 555
 556   if (isDualRadio())
 557   {
 558     switch (config.GetAntennaMode())
 559     {
 560     case TX_RADIO_MODE_GEMINI:
 561       transmittingRadio = SX12XX_Radio_All; // Gemini mode
 562       break;
 563     case TX_RADIO_MODE_ANT_1:
 564       transmittingRadio = SX12XX_Radio_1; // Single antenna tx and true diversity rx for tlm receiption.
 565       break;
 566     case TX_RADIO_MODE_ANT_2:
 567       transmittingRadio = SX12XX_Radio_2; // Single antenna tx and true diversity rx for tlm receiption.
 568       break;
 569     case TX_RADIO_MODE_SWITCH:
 570       if(OtaNonce%2==0)   transmittingRadio = SX12XX_Radio_1; // Single antenna tx and true diversity rx for tlm receiption.
 571       else   transmittingRadio = SX12XX_Radio_2; // Single antenna tx and true diversity rx for tlm receiption.
 572       break;
 573     default:
 574       break;
 575     }
 576   }
 577
 578 #if defined(Regulatory_Domain_EU_CE_2400)
 579   transmittingRadio &= ChannelIsClear(transmittingRadio);   // weed out the radio(s) if channel in use
 580
 581   if (transmittingRadio == SX12XX_Radio_NONE)
 582   {
 583     // no packet will be sent due to LBT
 584     // call TXdoneCallback() to prepare for TLM
 585     // and fall through to call TXnb() which will
 586     // set the transceiver the correct fallback mode
 587     Radio.TXdoneCallback();
 588   }
 589 #endif
 590
 591   Radio.TXnb((uint8_t*)&otaPkt, ExpressLRS_currAirRate_Modparams->PayloadLength, transmittingRadio);
 592 }
 593
 594 void ICACHE_RAM_ATTR nonceAdvance()
 595 {
 596   OtaNonce++;
 597   if ((OtaNonce + 1) % ExpressLRS_currAirRate_Modparams->FHSShopInterval == 0)
 598   {
 599     ++FHSSptr;
 600   }
 601 }
 602
 603 /*
 604  * Called as the TOCK timer ISR when there is a CRSF connection from the handset
 605  */
 606 void ICACHE_RAM_ATTR timerCallback()
 607 {
 608   /* If we are busy writing to EEPROM (committing config changes) then we just advance the nonces, i.e. no SPI traffic */
 609   if (commitInProgress)
 610   {
 611     nonceAdvance();
 612     return;
 613   }
 614
 615   // No packet has been sent due to LBT.  Call TXdoneCallback to prepare for TLM.
 616   if (Radio.GetLastTransmitRadio() == SX12XX_Radio_NONE)
 617   {
 618                 Radio.TXdoneCallback();
 619   }
 620
 621   // Sync OpenTX to this point
 622   if (!(OtaNonce % ExpressLRS_currAirRate_Modparams->numOfSends))
 623   {
 624     handset->JustSentRFpacket();
 625   }
 626
 627   // Do not transmit or advance FHSS/Nonce until in disconnected/connected state
 628   if (connectionState == awaitingModelId)
 629     return;
 630
 631   // Tx Antenna Diversity
 632   if ((OtaNonce % ExpressLRS_currAirRate_Modparams->numOfSends == 0 || // Swicth with new packet data
 633       OtaNonce % ExpressLRS_currAirRate_Modparams->numOfSends == ExpressLRS_currAirRate_Modparams->numOfSends / 2) && // Swicth in the middle of DVDA sends
 634       TelemetryRcvPhase == ttrpTransmitting) // Only switch when transmitting.  A diversity rx will send tlm back on the best antenna.  So dont switch away from it.
 635   {
 636     switchDiversityAntennas();
 637   }
 638
 639   // Nonce advances on every timer tick
 640   if (!InBindingMode)
 641     OtaNonce++;
 642
 643   // If HandleTLM has started Receive mode, TLM packet reception should begin shortly
 644   // Skip transmitting on this slot
 645   if (TelemetryRcvPhase == ttrpPreReceiveGap)
 646   {
 647     TelemetryRcvPhase = ttrpExpectingTelem;
 648 #if defined(Regulatory_Domain_EU_CE_2400)
 649     // Use downlink LQ for LBT success ratio instead for EU/CE reg domain
 650     CRSF::LinkStatistics.downlink_Link_quality = LBTSuccessCalc.getLQ();
 651 #else
 652     CRSF::LinkStatistics.downlink_Link_quality = LQCalc.getLQ();
 653 #endif
 654     LQCalc.inc();
 655     return;
 656   }
 657   else if (TelemetryRcvPhase == ttrpExpectingTelem && !LQCalc.currentIsSet())
 658   {
 659     // Indicate no telemetry packet received to the DP system
 660     DynamicPower_TelemetryUpdate(DYNPOWER_UPDATE_MISSED);
 661   }
 662   TelemetryRcvPhase = ttrpTransmitting;
 663
 664 #if defined(Regulatory_Domain_EU_CE_2400)
 665     BeginClearChannelAssessment(); // Get RSSI reading here, used also for next TX if in receiveMode.
 666 #endif
 667
 668   // Do not send a stale channels packet to the RX if one has not been received from the handset
 669   // *Do* send data if a packet has never been received from handset and the timer is running
 670   //     this is the case when bench testing and TXing without a handset
 671   uint32_t lastRcData = handset->GetRCdataLastRecv();
 672   if (!lastRcData || (micros() - lastRcData < 1000000))
 673   {
 674     busyTransmitting = true;
 675     SendRCdataToRF();
 676   }
 677 }
 678
 679 static void UARTdisconnected()
 680 {
 681   hwTimer::stop();
 682   connectionState = noCrossfire;
 683 }
 684
 685 static void UARTconnected()
 686 {
 687   #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
 688   webserverPreventAutoStart = true;
 689   #endif
 690   rfModeLastChangedMS = millis(); // force syncspam on first packets
 691
 692   auto index = adjustPacketRateForBaud(config.GetRate());
 693   config.SetRate(index);
 694   if (connectionState == noCrossfire || connectionState < MODE_STATES)
 695   {
 696     // When CRSF first connects, always go into a brief delay before
 697     // starting to transmit, to make sure a ModelID update isn't coming
 698     // right behind it
 699     connectionState = awaitingModelId;
 700   }
 701   // But start the timer to get OpenTX sync going and a ModelID update sent
 702   hwTimer::resume();
 703 }
 704
 705 void ResetPower()
 706 {
 707   // Dynamic Power starts at MinPower unless armed
 708   // (user may be turning up the power while flying and dropping the power may compromise the link)
 709   if (config.GetDynamicPower())
 710   {
 711     if (!handset->IsArmed())
 712     {
 713       // if dynamic power enabled and not armed then set to MinPower
 714       POWERMGNT::setPower(MinPower);
 715     }
 716     else if (POWERMGNT::currPower() < config.GetPower())
 717     {
 718       // if the new config is a higher power then set it, otherwise leave it alone
 719       POWERMGNT::setPower((PowerLevels_e)config.GetPower());
 720     }
 721   }
 722   else
 723   {
 724     POWERMGNT::setPower((PowerLevels_e)config.GetPower());
 725   }
 726   // TLM interval is set on the next SYNC packet
 727 #if defined(Regulatory_Domain_EU_CE_2400)
 728   LBTEnabled = (config.GetPower() > PWR_10mW);
 729 #endif
 730 }
 731
 732 static void ChangeRadioParams()
 733 {
 734   ModelUpdatePending = false;
 735   SetRFLinkRate(config.GetRate());
 736   ResetPower();
 737 }
 738
 739 void ModelUpdateReq()
 740 {
 741   // Force synspam with the current rate parameters in case already have a connection established
 742   if (config.SetModelId(CRSFHandset::getModelID()))
 743   {
 744     syncSpamCounter = syncSpamAmount;
 745     ModelUpdatePending = true;
 746   }
 747
 748   devicesTriggerEvent();
 749
 750   // Jump from awaitingModelId to transmitting to break the startup delay now
 751   // that the ModelID has been confirmed by the handset
 752   if (connectionState == awaitingModelId)
 753   {
 754     connectionState = disconnected;
 755   }
 756 }
 757
 758 static void ConfigChangeCommit()
 759 {
 760   // Adjust the air rate based on teh current baud rate
 761   auto index = adjustPacketRateForBaud(config.GetRate());
 762   config.SetRate(index);
 763
 764   // Write the uncommitted eeprom values (may block for a while)
 765   config.Commit();
 766   // Change params after the blocking finishes as a rate change will change the radio freq
 767   ChangeRadioParams();
 768   // Clear the commitInProgress flag so normal processing resumes
 769   commitInProgress = false;
 770   // UpdateFolderNames is expensive so it is called directly instead of in event() which gets called a lot
 771   luadevUpdateFolderNames();
 772   devicesTriggerEvent();
 773 }
 774
 775 static void CheckConfigChangePending()
 776 {
 777   if (config.IsModified() || ModelUpdatePending)
 778   {
 779     // Keep transmitting sync packets until the spam counter runs out
 780     if (syncSpamCounter > 0)
 781       return;
 782
 783 #if !defined(PLATFORM_STM32) || defined(TARGET_USE_EEPROM)
 784     while (busyTransmitting); // wait until no longer transmitting
 785 #else
 786     // The code expects to enter here shortly after the tock ISR has started sending the last
 787     // sync packet, before the tick ISR. Because the EEPROM write takes so long and disables
 788     // interrupts, FastForward the timer
 789     const uint32_t EEPROM_WRITE_DURATION = 30000; // us, a page write on F103C8 takes ~29.3ms
 790     const uint32_t cycleInterval = ExpressLRS_currAirRate_Modparams->interval;
 791     // Total time needs to be at least DURATION, rounded up to next cycle
 792     // adding one cycle that will be eaten by busywaiting for the transmit to end
 793     uint32_t pauseCycles = ((EEPROM_WRITE_DURATION + cycleInterval - 1) / cycleInterval) + 1;
 794     // Pause won't return until paused, and has just passed the tick ISR (but not fired)
 795     hwTimer::pause(pauseCycles * cycleInterval);
 796
 797     while (busyTransmitting); // wait until no longer transmitting
 798
 799     --pauseCycles; // the last cycle will actually be a transmit
 800     while (pauseCycles--)
 801       nonceAdvance();
 802 #endif
 803     // Set the commitInProgress flag to prevent any other RF SPI traffic during the commit from RX or scheduled TX
 804     commitInProgress = true;
 805     // If telemetry expected in the next interval, the radio was in RX mode
 806     // and will skip sending the next packet when the timer resumes.
 807     // Return to normal send mode because if the skipped packet happened
 808     // to be on the last slot of the FHSS the skip will prevent FHSS
 809     if (TelemetryRcvPhase != ttrpTransmitting)
 810     {
 811       Radio.SetTxIdleMode();
 812       TelemetryRcvPhase = ttrpTransmitting;
 813     }
 814     ConfigChangeCommit();
 815   }
 816 }
 817
 818 bool ICACHE_RAM_ATTR RXdoneISR(SX12xxDriverCommon::rx_status const status)
 819 {
 820   if (LQCalc.currentIsSet())
 821   {
 822     return false; // Already received tlm, do not run ProcessTLMpacket() again.
 823   }
 824
 825   bool packetSuccessful = ProcessTLMpacket(status);
 826   busyTransmitting = false;
 827   return packetSuccessful;
 828 }
 829
 830 void ICACHE_RAM_ATTR TXdoneISR()
 831 {
 832   if (!busyTransmitting)
 833   {
 834     return; // Already finished transmission and do not call HandleFHSS() a second time, which may hop the frequency!
 835   }
 836
 837   if (connectionState != awaitingModelId)
 838   {
 839     HandleFHSS();
 840     HandlePrepareForTLM();
 841 #if defined(Regulatory_Domain_EU_CE_2400)
 842     if (TelemetryRcvPhase != ttrpPreReceiveGap)
 843     {
 844       // Start RX for Listen Before Talk early because it takes about 100us
 845       // from RX enable to valid instant RSSI values are returned.
 846       // If rx was already started by TLM prepare above, this call will let RX
 847       // continue as normal.
 848       BeginClearChannelAssessment();
 849     }
 850 #endif // non-CE
 851   }
 852   busyTransmitting = false;
 853 }
 854
 855 static void UpdateConnectDisconnectStatus()
 856 {
 857   // Number of telemetry packets which can be lost in a row before going to disconnected state
 858   constexpr unsigned RX_LOSS_CNT = 5;
 859   // Must be at least 512ms and +2 to account for any rounding down and partial millis()
 860   const uint32_t msConnectionLostTimeout = std::max((uint32_t)512U,
 861     (uint32_t)ExpressLRS_currTlmDenom * ExpressLRS_currAirRate_Modparams->interval / (1000U / RX_LOSS_CNT)
 862     ) + 2U;
 863   // Capture the last before now so it will always be <= now
 864   const uint32_t lastTlmMillis = LastTLMpacketRecvMillis;
 865   const uint32_t now = millis();
 866   if (lastTlmMillis && ((now - lastTlmMillis) <= msConnectionLostTimeout))
 867   {
 868     if (connectionState != connected)
 869     {
 870       connectionState = connected;
 871       CRSFHandset::ForwardDevicePings = true;
 872       DBGLN("got downlink conn");
 873
 874       if (firmwareOptions.is_airport)
 875       {
 876         apInputBuffer.flush();
 877         apOutputBuffer.flush();
 878       }
 879     }
 880   }
 881   // If past RX_LOSS_CNT, or in awaitingModelId state for longer than DisconnectTimeoutMs, go to disconnected
 882   else if (connectionState == connected ||
 883     (now - rfModeLastChangedMS) > ExpressLRS_currAirRate_RFperfParams->DisconnectTimeoutMs)
 884   {
 885     connectionState = disconnected;
 886     connectionHasModelMatch = true;
 887     CRSFHandset::ForwardDevicePings = false;
 888   }
 889 }
 890
 891 void SetSyncSpam()
 892 {
 893   // Send sync spam if a UI device has requested to and the config has changed
 894   if (config.IsModified())
 895   {
 896     syncSpamCounter = syncSpamAmount;
 897   }
 898 }
 899
 900 static void SendRxWiFiOverMSP()
 901 {
 902   MSPDataPackage[0] = MSP_ELRS_SET_RX_WIFI_MODE;
 903   MspSender.SetDataToTransmit(MSPDataPackage, 1);
 904 }
 905
 906 static void CheckReadyToSend()
 907 {
 908   if (RxWiFiReadyToSend)
 909   {
 910     RxWiFiReadyToSend = false;
 911     if (!handset->IsArmed())
 912     {
 913       SendRxWiFiOverMSP();
 914     }
 915   }
 916 }
 917
 918 #if !defined(CRITICAL_FLASH)
 919 void OnPowerGetCalibration(mspPacket_t *packet)
 920 {
 921   uint8_t index = packet->readByte();
 922   UNUSED(index);
 923   int8_t values[PWR_COUNT] = {0};
 924   POWERMGNT::GetPowerCaliValues(values, PWR_COUNT);
 925   DBGLN("power get calibration value %d",  values[index]);
 926 }
 927
 928 void OnPowerSetCalibration(mspPacket_t *packet)
 929 {
 930   uint8_t index = packet->readByte();
 931   int8_t value = packet->readByte();
 932
 933   if((index < 0) || (index > PWR_COUNT))
 934   {
 935     DBGLN("calibration error index %d out of range", index);
 936     return;
 937   }
 938   hwTimer::stop();
 939   delay(20);
 940
 941   int8_t values[PWR_COUNT] = {0};
 942   POWERMGNT::GetPowerCaliValues(values, PWR_COUNT);
 943   values[index] = value;
 944   POWERMGNT::SetPowerCaliValues(values, PWR_COUNT);
 945   DBGLN("power calibration done %d, %d", index, value);
 946   hwTimer::resume();
 947 }
 948 #endif
 949
 950 void SendUIDOverMSP()
 951 {
 952   MSPDataPackage[0] = MSP_ELRS_BIND;
 953   memcpy(&MSPDataPackage[1], &UID[2], 4);
 954   BindingSendCount = 0;
 955   MspSender.ResetState();
 956   MspSender.SetDataToTransmit(MSPDataPackage, 5);
 957 }
 958
 959 static void EnterBindingMode()
 960 {
 961   if (InBindingMode)
 962       return;
 963
 964   // Disable the TX timer and wait for any TX to complete
 965   hwTimer::stop();
 966   while (busyTransmitting);
 967
 968   // Queue up sending the Master UID as MSP packets
 969   SendUIDOverMSP();
 970
 971   // Binding uses a CRCInit=0, 50Hz, and InvertIQ
 972   OtaCrcInitializer = 0;
 973   OtaNonce = 0; // Lock the OtaNonce to prevent syncspam packets
 974   InBindingMode = true; // Set binding mode before SetRFLinkRate() for correct IQ
 975
 976   // Start attempting to bind
 977   // Lock the RF rate and freq while binding
 978   SetRFLinkRate(enumRatetoIndex(RATE_BINDING));
 979
 980 #if defined(RADIO_LR1121)
 981   FHSSuseDualBand = true;
 982   expresslrs_mod_settings_s *const dualBandBindingModParams = get_elrs_airRateConfig(RATE_DUALBAND_BINDING); // 2.4GHz 50Hz
 983   Radio.Config(dualBandBindingModParams->bw2, dualBandBindingModParams->sf2, dualBandBindingModParams->cr2, FHSSgetInitialGeminiFreq(),
 984                dualBandBindingModParams->PreambleLen2, true, dualBandBindingModParams->PayloadLength, dualBandBindingModParams->interval, SX12XX_Radio_2);
 985 #endif
 986
 987   Radio.SetFrequencyReg(FHSSgetInitialFreq());
 988   if (isDualRadio() && config.GetAntennaMode() == TX_RADIO_MODE_GEMINI) // Gemini mode
 989   {
 990     Radio.SetFrequencyReg(FHSSgetInitialGeminiFreq(), SX12XX_Radio_2);
 991   }
 992   // Start transmitting again
 993   hwTimer::resume();
 994
 995   DBGLN("Entered binding mode at freq = %d", Radio.currFreq);
 996 }
 997
 998 static void ExitBindingMode()
 999 {
1000   if (!InBindingMode)
1001     return;
1002
1003   MspSender.ResetState();
1004
1005   // Reset CRCInit to UID-defined value
1006   OtaUpdateCrcInitFromUid();
1007   InBindingMode = false; // Clear binding mode before SetRFLinkRate() for correct IQ
1008
1009   SetRFLinkRate(config.GetRate()); //return to original rate
1010
1011   DBGLN("Exiting binding mode");
1012 }
1013
1014 void EnterBindingModeSafely()
1015 {
1016   // TX can always enter binding mode safely as the function handles stopping the transmitter
1017   EnterBindingMode();
1018 }
1019
1020
1021 void ProcessMSPPacket(uint32_t now, mspPacket_t *packet)
1022 {
1023 #if !defined(CRITICAL_FLASH)
1024   // Inspect packet for ELRS specific opcodes
1025   if (packet->function == MSP_ELRS_FUNC)
1026   {
1027     uint8_t opcode = packet->readByte();
1028
1029     CHECK_PACKET_PARSING();
1030
1031     switch (opcode)
1032     {
1033     case MSP_ELRS_POWER_CALI_GET:
1034       OnPowerGetCalibration(packet);
1035       break;
1036     case MSP_ELRS_POWER_CALI_SET:
1037       OnPowerSetCalibration(packet);
1038       break;
1039     default:
1040       break;
1041     }
1042   }
1043   else if (packet->function == MSP_SET_VTX_CONFIG)
1044   {
1045     if (packet->payload[0] < 48) // Standard 48 channel VTx table size e.g. A, B, E, F, R, L
1046     {
1047       config.SetVtxBand(packet->payload[0] / 8 + 1);
1048       config.SetVtxChannel(packet->payload[0] % 8);
1049     } else
1050     {
1051       return; // Packets containing frequency in MHz are not yet supported.
1052     }
1053
1054     VtxTriggerSend();
1055   }
1056   else if (packet->function == MSP_ELRS_BACKPACK_SET_PTR && packet->payloadSize == 6)
1057   {
1058     ptrChannelData[0] = packet->payload[0] + (packet->payload[1] << 8);
1059     ptrChannelData[1] = packet->payload[2] + (packet->payload[3] << 8);
1060     ptrChannelData[2] = packet->payload[4] + (packet->payload[5] << 8);
1061     lastPTRValidTimeMs = now;
1062   }
1063 #endif
1064   if (packet->function == MSP_ELRS_GET_BACKPACK_VERSION)
1065   {
1066     memset(backpackVersion, 0, sizeof(backpackVersion));
1067     memcpy(backpackVersion, packet->payload, min((size_t)packet->payloadSize, sizeof(backpackVersion)-1));
1068   }
1069 }
1070
1071 static void HandleUARTout()
1072 {
1073   if (firmwareOptions.is_airport)
1074   {
1075     auto size = apOutputBuffer.size();
1076     if (size)
1077     {
1078       uint8_t buf[size];
1079       apOutputBuffer.lock();
1080       apOutputBuffer.popBytes(buf, size);
1081       apOutputBuffer.unlock();
1082       TxUSB->write(buf, size);
1083     }
1084   }
1085 }
1086
1087 static void setupSerial()
1088 {  /*
1089    * Setup the logging/backpack serial port, and the USB serial port.
1090    * This is always done because we need a place to send data even if there is no backpack!
1091    */
1092   bool portConflict = false;
1093   int8_t rxPin = UNDEF_PIN;
1094   int8_t txPin = UNDEF_PIN;
1095
1096   if (firmwareOptions.is_airport)
1097   {
1098     #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
1099       // Airport enabled - set TxUSB port to pins 1 and 3
1100       rxPin = 3;
1101       txPin = 1;
1102
1103       if (GPIO_PIN_DEBUG_RX == rxPin && GPIO_PIN_DEBUG_TX == txPin)
1104       {
1105         // Avoid conflict between TxUSB and TxBackpack for UART0 (pins 1 and 3)
1106         // TxUSB takes priority over TxBackpack
1107         portConflict = true;
1108       }
1109     #else
1110       // For STM targets, assume GPIO_PIN_DEBUG defines point to USB
1111       rxPin = GPIO_PIN_DEBUG_RX;
1112       txPin = GPIO_PIN_DEBUG_TX;
1113     #endif
1114   }
1115
1116 // Setup TxBackpack
1117 #if defined(PLATFORM_ESP32)
1118   Stream *serialPort;
1119   if (GPIO_PIN_DEBUG_RX != UNDEF_PIN && GPIO_PIN_DEBUG_TX != UNDEF_PIN && !portConflict)
1120   {
1121     serialPort = new HardwareSerial(2);
1122     ((HardwareSerial *)serialPort)->begin(BACKPACK_LOGGING_BAUD, SERIAL_8N1, GPIO_PIN_DEBUG_RX, GPIO_PIN_DEBUG_TX);
1123   }
1124   else
1125   {
1126     serialPort = new NullStream();
1127   }
1128 #elif defined(PLATFORM_ESP8266)
1129   Stream *serialPort;
1130   if (GPIO_PIN_DEBUG_TX != UNDEF_PIN)
1131   {
1132     serialPort = new HardwareSerial(1);
1133     ((HardwareSerial*)serialPort)->begin(BACKPACK_LOGGING_BAUD, SERIAL_8N1, SERIAL_TX_ONLY, GPIO_PIN_DEBUG_TX);
1134   }
1135   else
1136   {
1137     serialPort = new NullStream();
1138   }
1139 #elif defined(TARGET_TX_FM30)
1140   #if defined(PIO_FRAMEWORK_ARDUINO_ENABLE_CDC)
1141     USBSerial *serialPort = &SerialUSB; // No way to disable creating SerialUSB global, so use it
1142     serialPort->begin();
1143   #else
1144     Stream *serialPort = new NullStream();
1145   #endif
1146 #elif (defined(GPIO_PIN_DEBUG_RX) && GPIO_PIN_DEBUG_RX != UNDEF_PIN) || (defined(GPIO_PIN_DEBUG_TX) && GPIO_PIN_DEBUG_TX != UNDEF_PIN)
1147   HardwareSerial *serialPort = new HardwareSerial(2);
1148   #if defined(GPIO_PIN_DEBUG_RX) && GPIO_PIN_DEBUG_RX != UNDEF_PIN
1149     serialPort->setRx(GPIO_PIN_DEBUG_RX);
1150   #endif
1151   #if defined(GPIO_PIN_DEBUG_TX) && GPIO_PIN_DEBUG_TX != UNDEF_PIN
1152     serialPort->setTx(GPIO_PIN_DEBUG_TX);
1153   #endif
1154   serialPort->begin(BACKPACK_LOGGING_BAUD);
1155 #else
1156   Stream *serialPort = new NullStream();
1157 #endif
1158   TxBackpack = serialPort;
1159
1160 #if defined(PLATFORM_ESP32_S3)
1161   Serial.begin(460800);
1162 #endif
1163
1164 // Setup TxUSB
1165 #if defined(PLATFORM_ESP32)
1166   if (rxPin != UNDEF_PIN && txPin != UNDEF_PIN)
1167   {
1168     TxUSB = new HardwareSerial(1);
1169     ((HardwareSerial *)TxUSB)->begin(firmwareOptions.uart_baud, SERIAL_8N1, rxPin, txPin);
1170   }
1171   else
1172   {
1173     TxUSB = new NullStream();
1174   }
1175 #else
1176   TxUSB = new NullStream();
1177   UNUSED(portConflict);
1178   UNUSED(rxPin);
1179   UNUSED(txPin);
1180 #endif
1181 }
1182
1183 /**
1184  * Target-specific initialization code called early in setup()
1185  * Setup GPIOs or other hardware, config not yet loaded
1186  ***/
1187 static void setupTarget()
1188 {
1189 #if defined(TARGET_TX_FM30)
1190   pinMode(GPIO_PIN_UART3RX_INVERT, OUTPUT); // RX3 inverter (from radio)
1191   digitalWrite(GPIO_PIN_UART3RX_INVERT, LOW); // RX3 not inverted
1192   pinMode(GPIO_PIN_BLUETOOTH_EN, OUTPUT); // Bluetooth enable (disabled)
1193   digitalWrite(GPIO_PIN_BLUETOOTH_EN, HIGH);
1194   pinMode(GPIO_PIN_UART1RX_INVERT, OUTPUT); // RX1 inverter (TX handled in CRSF)
1195   digitalWrite(GPIO_PIN_UART1RX_INVERT, HIGH);
1196   pinMode(GPIO_PIN_ANT_CTRL_FIXED, OUTPUT);
1197   digitalWrite(GPIO_PIN_ANT_CTRL_FIXED, LOW); // LEFT antenna
1198   HardwareSerial *uart2 = new HardwareSerial(USART2);
1199   uart2->begin(57600);
1200   CRSFHandset::PortSecondary = uart2;
1201 #endif
1202
1203 #if defined(TARGET_TX_FM30_MINI)
1204   pinMode(GPIO_PIN_UART1TX_INVERT, OUTPUT); // TX1 inverter used for debug
1205   digitalWrite(GPIO_PIN_UART1TX_INVERT, LOW);
1206 #endif
1207
1208   if (GPIO_PIN_ANT_CTRL != UNDEF_PIN)
1209   {
1210     pinMode(GPIO_PIN_ANT_CTRL, OUTPUT);
1211     digitalWrite(GPIO_PIN_ANT_CTRL, diversityAntennaState);
1212   }
1213   if (GPIO_PIN_ANT_CTRL_COMPL != UNDEF_PIN)
1214   {
1215     pinMode(GPIO_PIN_ANT_CTRL_COMPL, OUTPUT);
1216     digitalWrite(GPIO_PIN_ANT_CTRL_COMPL, !diversityAntennaState);
1217   }
1218
1219   setupTargetCommon();
1220   setupSerial();
1221 }
1222
1223 bool setupHardwareFromOptions()
1224 {
1225 #if defined(TARGET_UNIFIED_TX)
1226   if (!options_init())
1227   {
1228     // Register the WiFi with the framework
1229     static device_affinity_t wifi_device[] = {
1230         {&WIFI_device, 1}
1231     };
1232     devicesRegister(wifi_device, ARRAY_SIZE(wifi_device));
1233     devicesInit();
1234
1235     connectionState = hardwareUndefined;
1236     return false;
1237   }
1238 #else
1239   options_init();
1240 #endif
1241
1242   return true;
1243 }
1244
1245 static void setupBindingFromConfig()
1246 {
1247   if (firmwareOptions.hasUID)
1248   {
1249       memcpy(UID, firmwareOptions.uid, UID_LEN);
1250   }
1251   else
1252   {
1253 #ifdef PLATFORM_ESP32
1254     esp_read_mac(UID, ESP_MAC_WIFI_STA);
1255 #elif PLATFORM_STM32
1256     UID[0] = (uint8_t)HAL_GetUIDw0();
1257     UID[1] = (uint8_t)(HAL_GetUIDw0() >> 8);
1258     UID[2] = (uint8_t)HAL_GetUIDw1();
1259     UID[3] = (uint8_t)(HAL_GetUIDw1() >> 8);
1260     UID[4] = (uint8_t)HAL_GetUIDw2();
1261     UID[5] = (uint8_t)(HAL_GetUIDw2() >> 8);
1262 #endif
1263   }
1264
1265   DBGLN("UID=(%d, %d, %d, %d, %d, %d)",
1266     UID[0], UID[1], UID[2], UID[3], UID[4], UID[5]);
1267
1268   OtaUpdateCrcInitFromUid();
1269 }
1270
1271
1272 static void cyclePower()
1273 {
1274   // Only change power if we are running normally
1275   if (connectionState < MODE_STATES)
1276   {
1277     PowerLevels_e curr = POWERMGNT::currPower();
1278     if (curr == POWERMGNT::getMaxPower())
1279     {
1280       POWERMGNT::setPower(POWERMGNT::getMinPower());
1281     }
1282     else
1283     {
1284       POWERMGNT::incPower();
1285     }
1286   }
1287 }
1288
1289 void setup()
1290 {
1291   if (setupHardwareFromOptions())
1292   {
1293     setupTarget();
1294     // Register the devices with the framework
1295     devicesRegister(ui_devices, ARRAY_SIZE(ui_devices));
1296     // Initialise the devices
1297     devicesInit();
1298     DBGLN("Initialised devices");
1299
1300     setupBindingFromConfig();
1301     FHSSrandomiseFHSSsequence(uidMacSeedGet());
1302
1303     Radio.RXdoneCallback = &RXdoneISR;
1304     Radio.TXdoneCallback = &TXdoneISR;
1305
1306     handset->registerCallbacks(UARTconnected, firmwareOptions.is_airport ? nullptr : UARTdisconnected, ModelUpdateReq, EnterBindingModeSafely);
1307
1308     DBGLN("ExpressLRS TX Module Booted...");
1309
1310     eeprom.Begin(); // Init the eeprom
1311     config.SetStorageProvider(&eeprom); // Pass pointer to the Config class for access to storage
1312     config.Load(); // Load the stored values from eeprom
1313
1314     Radio.currFreq = FHSSgetInitialFreq(); //set frequency first or an error will occur!!!
1315     #if defined(RADIO_SX127X)
1316     //Radio.currSyncWord = UID[3];
1317     #endif
1318     bool init_success;
1319     #if defined(USE_BLE_JOYSTICK)
1320     init_success = true; // No radio is attached with a joystick only module.  So we are going to fake success so that crsf, hwTimer etc are initiated below.
1321     #else
1322     if (GPIO_PIN_SCK != UNDEF_PIN)
1323     {
1324       init_success = Radio.Begin();
1325     }
1326     else
1327     {
1328       // Assume BLE Joystick mode if no radio SCK pin
1329       init_success = true;
1330     }
1331     #endif
1332
1333     if (!init_success)
1334     {
1335       connectionState = radioFailed;
1336     }
1337     else
1338     {
1339       TelemetryReceiver.SetDataToReceive(CRSFinBuffer, sizeof(CRSFinBuffer));
1340
1341       POWERMGNT::init();
1342       DynamicPower_Init();
1343
1344       // Set the pkt rate, TLM ratio, and power from the stored eeprom values
1345       ChangeRadioParams();
1346
1347   #if defined(Regulatory_Domain_EU_CE_2400)
1348       BeginClearChannelAssessment();
1349   #endif
1350       hwTimer::init(nullptr, timerCallback);
1351       connectionState = noCrossfire;
1352     }
1353   }
1354
1355 #if defined(HAS_BUTTON)
1356   registerButtonFunction(ACTION_BIND, EnterBindingMode);
1357   registerButtonFunction(ACTION_INCREASE_POWER, cyclePower);
1358 #endif
1359
1360   devicesStart();
1361
1362   if (firmwareOptions.is_airport)
1363   {
1364     config.SetTlm(TLM_RATIO_1_2); // Force TLM ratio of 1:2 for balanced bi-dir link
1365     config.SetMotionMode(0); // Ensure motion detection is off
1366     UARTconnected();
1367   }
1368 }
1369
1370 void loop()
1371 {
1372   uint32_t now = millis();
1373
1374   HandleUARTout(); // Only used for non-CRSF output
1375
1376   #if defined(USE_BLE_JOYSTICK)
1377   if (connectionState != bleJoystick && connectionState != noCrossfire) // Wait until the correct crsf baud has been found
1378   {
1379       connectionState = bleJoystick;
1380   }
1381   #endif
1382
1383   if (connectionState < MODE_STATES)
1384   {
1385     UpdateConnectDisconnectStatus();
1386   }
1387
1388   // Update UI devices
1389   devicesUpdate(now);
1390
1391   // Not a device because it must be run on the loop core
1392   checkBackpackUpdate();
1393
1394   #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
1395     // If the reboot time is set and the current time is past the reboot time then reboot.
1396     if (rebootTime != 0 && now > rebootTime) {
1397       ESP.restart();
1398     }
1399   #endif
1400
1401   executeDeferredFunction(now);
1402
1403   if (firmwareOptions.is_airport && connectionState == connected)
1404   {
1405     auto size = std::min(AP_MAX_BUF_LEN - apInputBuffer.size(), TxUSB->available());
1406     if (size > 0)
1407     {
1408       uint8_t buf[size];
1409       TxUSB->readBytes(buf, size);
1410       apInputBuffer.lock();
1411       apInputBuffer.pushBytes(buf, size);
1412       apInputBuffer.unlock();
1413     }
1414   }
1415
1416   if (TxBackpack->available())
1417   {
1418     if (msp.processReceivedByte(TxBackpack->read()))
1419     {
1420       // Finished processing a complete packet
1421       ProcessMSPPacket(now, msp.getReceivedPacket());
1422       msp.markPacketReceived();
1423     }
1424   }
1425
1426   if (connectionState > MODE_STATES)
1427   {
1428     return;
1429   }
1430
1431   CheckReadyToSend();
1432   CheckConfigChangePending();
1433   DynamicPower_Update(now);
1434   VtxPitmodeSwitchUpdate();
1435
1436   /* Send TLM updates to handset if connected + reporting period
1437    * is elapsed. This keeps handset happy dispite of the telemetry ratio */
1438   if ((connectionState == connected) && (LastTLMpacketRecvMillis != 0) &&
1439       (now >= (uint32_t)(firmwareOptions.tlm_report_interval + TLMpacketReported))) {
1440     // 3 byte header + 1 byte CRC
1441     uint8_t linkStatisticsFrame[LinkStatisticsFrameLength + 4];
1442     CRSFHandset::makeLinkStatisticsPacket(linkStatisticsFrame);
1443     handset->sendTelemetryToTX(linkStatisticsFrame);
1444     crsfTelemToMSPOut(linkStatisticsFrame);
1445     TLMpacketReported = now;
1446   }
1447
1448   if (TelemetryReceiver.HasFinishedData())
1449   {
1450     handset->sendTelemetryToTX(CRSFinBuffer);
1451     crsfTelemToMSPOut(CRSFinBuffer);
1452     TelemetryReceiver.Unlock();
1453   }
1454
1455   // only send msp data when binding is not active
1456   static bool mspTransferActive = false;
1457   if (InBindingMode)
1458   {
1459     // exit bind mode if package after some repeats
1460     if (BindingSendCount > 6) {
1461       ExitBindingMode();
1462     }
1463   }
1464   else if (!MspSender.IsActive())
1465   {
1466     // sending is done and we need to update our flag
1467     if (mspTransferActive)
1468     {
1469       // unlock buffer for msp messages
1470       CRSF::UnlockMspMessage();
1471       mspTransferActive = false;
1472     }
1473     // we are not sending so look for next msp package
1474     else
1475     {
1476       uint8_t* mspData;
1477       uint8_t mspLen;
1478       CRSF::GetMspMessage(&mspData, &mspLen);
1479       // if we have a new msp package start sending
1480       if (mspData != nullptr)
1481       {
1482         MspSender.SetDataToTransmit(mspData, mspLen);
1483         mspTransferActive = true;
1484       }
1485     }
1486   }
1487 }