src/lib/SX1280Driver/SX1280.cpp

   1 #include "SX1280_Regs.h"
   2 #include "SX1280_hal.h"
   3 #include "SX1280.h"
   4 #include "logging.h"
   5 #include "RFAMP_hal.h"
   6 #include <math.h>
   7
   8 SX1280Hal hal;
   9 SX1280Driver *SX1280Driver::instance = NULL;
  10
  11 RFAMP_hal RFAMP;
  12
  13 //DEBUG_SX1280_OTA_TIMING
  14
  15 /* Steps for startup
  16
  17 1. If not in STDBY_RC mode, then go to this mode by sending the command:
  18 SetStandby(STDBY_RC)
  19
  20 2. Define the LoRa® packet type by sending the command:
  21 SetPacketType(PACKET_TYPE_LORA)
  22
  23 3. Define the RF frequency by sending the command:
  24 SetRfFrequency(rfFrequency)
  25 The LSB of rfFrequency is equal to the PLL step i.e. 52e6/2^18 Hz. SetRfFrequency() defines the Tx frequency.
  26
  27 4. Indicate the addresses where the packet handler will read (txBaseAddress in Tx) or write (rxBaseAddress in Rx) the first
  28 byte of the data payload by sending the command:
  29 SetBufferBaseAddress(txBaseAddress, rxBaseAddress)
  30 Note:
  31 txBaseAddress and rxBaseAddress are offset relative to the beginning of the data memory map.
  32
  33 5. Define the modulation parameter signal BW SF CR
  34 */
  35
  36 #if defined(DEBUG_SX1280_OTA_TIMING)
  37 static uint32_t beginTX;
  38 static uint32_t endTX;
  39 #endif
  40
  41 /*
  42  * Period Base from table 11-24, page 79 datasheet rev 3.2
  43  * SX1280_RADIO_TICK_SIZE_0015_US = 15625 nanos
  44  * SX1280_RADIO_TICK_SIZE_0062_US = 62500 nanos
  45  * SX1280_RADIO_TICK_SIZE_1000_US = 1000000 nanos
  46  * SX1280_RADIO_TICK_SIZE_4000_US = 4000000 nanos
  47  */
  48 #define RX_TIMEOUT_PERIOD_BASE SX1280_RADIO_TICK_SIZE_0015_US
  49 #define RX_TIMEOUT_PERIOD_BASE_NANOS 15625
  50
  51 #ifdef USE_HARDWARE_DCDC
  52     #ifndef OPT_USE_HARDWARE_DCDC
  53         #define OPT_USE_HARDWARE_DCDC true
  54     #endif
  55 #else
  56     #define OPT_USE_HARDWARE_DCDC false
  57 #endif
  58
  59 SX1280Driver::SX1280Driver(): SX12xxDriverCommon()
  60 {
  61     instance = this;
  62     timeout = 0xffff;
  63     currOpmode = SX1280_MODE_SLEEP;
  64     lastSuccessfulPacketRadio = SX12XX_Radio_1;
  65     fallBackMode = SX1280_MODE_STDBY_RC;
  66 }
  67
  68 void SX1280Driver::End()
  69 {
  70     if (currOpmode != SX1280_MODE_SLEEP)
  71     {
  72         SetMode(SX1280_MODE_SLEEP, SX12XX_Radio_All);
  73     }
  74     hal.end();
  75     RFAMP.TXRXdisable();
  76     RemoveCallbacks();
  77     currFreq = (uint32_t)((double)2400000000 / (double)FREQ_STEP);
  78     PayloadLength = 8; // Dummy default value which is overwritten during setup.
  79 }
  80
  81 bool SX1280Driver::Begin()
  82 {
  83     hal.init();
  84     hal.IsrCallback_1 = &SX1280Driver::IsrCallback_1;
  85     hal.IsrCallback_2 = &SX1280Driver::IsrCallback_2;
  86
  87     hal.reset();
  88     DBGLN("SX1280 Begin");
  89
  90     RFAMP.init();
  91
  92     SetMode(SX1280_MODE_STDBY_RC, SX12XX_Radio_All); // Put in STDBY_RC mode.  Must be SX1280_MODE_STDBY_RC for SX1280_RADIO_SET_REGULATORMODE to be set.
  93
  94     uint16_t firmwareRev = (((hal.ReadRegister(REG_LR_FIRMWARE_VERSION_MSB, SX12XX_Radio_1)) << 8) | (hal.ReadRegister(REG_LR_FIRMWARE_VERSION_MSB + 1, SX12XX_Radio_1)));
  95     DBGLN("Read Vers sx1280 #1: %d", firmwareRev);
  96     if ((firmwareRev == 0) || (firmwareRev == 65535))
  97     {
  98         // SPI communication failed, just return without configuration
  99         return false;
 100     }
 101
 102     hal.WriteRegister(0x0891, (hal.ReadRegister(0x0891, SX12XX_Radio_1) | 0xC0), SX12XX_Radio_1);   //default is low power mode, switch to high sensitivity instead
 103
 104     if (GPIO_PIN_NSS_2 != UNDEF_PIN)
 105     {
 106         firmwareRev = (((hal.ReadRegister(REG_LR_FIRMWARE_VERSION_MSB, SX12XX_Radio_2)) << 8) | (hal.ReadRegister(REG_LR_FIRMWARE_VERSION_MSB + 1, SX12XX_Radio_2)));
 107         DBGLN("Read Vers sx1280 #2: %d", firmwareRev);
 108         if ((firmwareRev == 0) || (firmwareRev == 65535))
 109         {
 110             // SPI communication failed, just return without configuration
 111             return false;
 112         }
 113
 114         hal.WriteRegister(0x0891, (hal.ReadRegister(0x0891, SX12XX_Radio_2) | 0xC0), SX12XX_Radio_2);   //default is low power mode, switch to high sensitivity instead
 115     }
 116
 117 #if defined(TARGET_RX)
 118     fallBackMode = SX1280_MODE_FS;
 119     hal.WriteCommand(SX1280_RADIO_SET_AUTOFS, 0x01, SX12XX_Radio_All); //Enable auto FS
 120 #else
 121 /*
 122 Do not enable for dual radio TX.
 123 When SX1280_RADIO_SET_AUTOFS is set and tlm received by only 1 of the 2 radios,  that radio will go into FS mode and the other
 124 into Standby mode.  After the following SPI command for tx mode, busy will go high for differing periods of time because 1 is
 125 transitioning from FS mode and the other from Standby mode. This causes the tx done dio of the 2 radios to occur at very different times.
 126 */
 127     if (GPIO_PIN_NSS_2 == UNDEF_PIN)
 128     {
 129         fallBackMode = SX1280_MODE_FS;
 130         hal.WriteCommand(SX1280_RADIO_SET_AUTOFS, 0x01, SX12XX_Radio_All); //Enable auto FS
 131     }
 132 #endif
 133
 134     // Force the next power update, and the lowest power
 135     pwrCurrent = PWRPENDING_NONE;
 136     SetOutputPower(SX1280_POWER_MIN);
 137     CommitOutputPower();
 138 #if defined(USE_HARDWARE_DCDC)
 139     if (OPT_USE_HARDWARE_DCDC)
 140     {
 141         hal.WriteCommand(SX1280_RADIO_SET_REGULATORMODE, SX1280_USE_DCDC, SX12XX_Radio_All);        // Enable DCDC converter instead of LDO
 142     }
 143 #endif
 144
 145     return true;
 146 }
 147
 148 void SX1280Driver::startCWTest(uint32_t freq, SX12XX_Radio_Number_t radioNumber)
 149 {
 150     uint8_t buffer;         // we just need a buffer for the write command
 151     SetFrequencyHz(freq, radioNumber);
 152     CommitOutputPower();
 153     RFAMP.TXenable(radioNumber);
 154     hal.WriteCommand(SX1280_RADIO_SET_TXCONTINUOUSWAVE, &buffer, 0, radioNumber);
 155 }
 156
 157 void SX1280Driver::Config(uint8_t bw, uint8_t sf, uint8_t cr, uint32_t regfreq,
 158                           uint8_t PreambleLength, bool InvertIQ, uint8_t _PayloadLength, uint32_t interval,
 159                           uint32_t flrcSyncWord, uint16_t flrcCrcSeed, uint8_t flrc)
 160 {
 161     uint8_t const mode = (flrc) ? SX1280_PACKET_TYPE_FLRC : SX1280_PACKET_TYPE_LORA;
 162
 163     PayloadLength = _PayloadLength;
 164     IQinverted = InvertIQ;
 165     packet_mode = mode;
 166     SetMode(SX1280_MODE_STDBY_RC, SX12XX_Radio_All);
 167     hal.WriteCommand(SX1280_RADIO_SET_PACKETTYPE, mode, SX12XX_Radio_All, 20);
 168     if (mode == SX1280_PACKET_TYPE_FLRC)
 169     {
 170         DBG("Config FLRC ");
 171         ConfigModParamsFLRC(bw, cr, sf);
 172         SetPacketParamsFLRC(SX1280_FLRC_PACKET_FIXED_LENGTH, PreambleLength, _PayloadLength, flrcSyncWord, flrcCrcSeed, cr);
 173     }
 174     else
 175     {
 176         DBG("Config LoRa ");
 177         ConfigModParamsLoRa(bw, sf, cr);
 178 #if defined(DEBUG_FREQ_CORRECTION)
 179         SX1280_RadioLoRaPacketLengthsModes_t packetLengthType = SX1280_LORA_PACKET_VARIABLE_LENGTH;
 180 #else
 181         SX1280_RadioLoRaPacketLengthsModes_t packetLengthType = SX1280_LORA_PACKET_FIXED_LENGTH;
 182 #endif
 183         SetPacketParamsLoRa(PreambleLength, packetLengthType, _PayloadLength, InvertIQ);
 184     }
 185     SetFrequencyReg(regfreq);
 186     SetRxTimeoutUs(interval);
 187
 188     uint16_t dio1Mask = SX1280_IRQ_TX_DONE | SX1280_IRQ_RX_DONE;
 189     uint16_t irqMask  = SX1280_IRQ_TX_DONE | SX1280_IRQ_RX_DONE | SX1280_IRQ_SYNCWORD_VALID | SX1280_IRQ_SYNCWORD_ERROR | SX1280_IRQ_CRC_ERROR;
 190     SetDioIrqParams(irqMask, dio1Mask);
 191 }
 192
 193 void SX1280Driver::SetRxTimeoutUs(uint32_t interval)
 194 {
 195     if (interval)
 196     {
 197         timeout = interval * 1000 / RX_TIMEOUT_PERIOD_BASE_NANOS; // number of periods for the SX1280 to timeout
 198     }
 199     else
 200     {
 201         timeout = 0xFFFF;   // no timeout, continuous mode
 202     }
 203 }
 204
 205 /***
 206  * @brief: Schedule an output power change after the next transmit
 207  ***/
 208 void SX1280Driver::SetOutputPower(int8_t power)
 209 {
 210     uint8_t pwrNew = constrain(power, SX1280_POWER_MIN, SX1280_POWER_MAX) + (-SX1280_POWER_MIN);
 211
 212     if ((pwrPending == PWRPENDING_NONE && pwrCurrent != pwrNew) || pwrPending != pwrNew)
 213     {
 214         pwrPending = pwrNew;
 215         DBGLN("SetPower: %u", pwrPending);
 216     }
 217 }
 218
 219 void ICACHE_RAM_ATTR SX1280Driver::CommitOutputPower()
 220 {
 221     if (pwrPending == PWRPENDING_NONE)
 222         return;
 223
 224     pwrCurrent = pwrPending;
 225     pwrPending = PWRPENDING_NONE;
 226     uint8_t buf[2] = { pwrCurrent, (uint8_t)SX1280_RADIO_RAMP_04_US };
 227     hal.WriteCommand(SX1280_RADIO_SET_TXPARAMS, buf, sizeof(buf), SX12XX_Radio_All);
 228 }
 229
 230 void SX1280Driver::SetMode(SX1280_RadioOperatingModes_t OPmode, SX12XX_Radio_Number_t radioNumber)
 231 {
 232     /*
 233     Comment out since it is difficult to keep track of dual radios.
 234     When checking SPI it is also useful to see every possible SPI transaction to make sure it fits when required.
 235     */
 236     // if (OPmode == currOpmode)
 237     // {
 238     //    return;
 239     // }
 240
 241     WORD_ALIGNED_ATTR uint8_t buf[3];
 242     switch (OPmode)
 243     {
 244
 245     case SX1280_MODE_SLEEP:
 246         hal.WriteCommand(SX1280_RADIO_SET_SLEEP, (uint8_t)0x01, radioNumber);
 247         break;
 248
 249     case SX1280_MODE_CALIBRATION:
 250         break;
 251
 252     case SX1280_MODE_STDBY_RC:
 253         hal.WriteCommand(SX1280_RADIO_SET_STANDBY, SX1280_STDBY_RC, radioNumber, 1500);
 254         break;
 255
 256     // The DC-DC supply regulation is automatically powered in STDBY_XOSC mode.
 257     case SX1280_MODE_STDBY_XOSC:
 258         hal.WriteCommand(SX1280_RADIO_SET_STANDBY, SX1280_STDBY_XOSC, radioNumber, 50);
 259         break;
 260
 261     case SX1280_MODE_FS:
 262         hal.WriteCommand(SX1280_RADIO_SET_FS, (uint8_t)0x00, radioNumber, 70);
 263         break;
 264
 265     case SX1280_MODE_RX:
 266         buf[0] = RX_TIMEOUT_PERIOD_BASE;
 267         buf[1] = timeout >> 8;
 268         buf[2] = timeout & 0xFF;
 269         hal.WriteCommand(SX1280_RADIO_SET_RX, buf, sizeof(buf), radioNumber, 100);
 270         break;
 271
 272     case SX1280_MODE_RX_CONT:
 273         buf[0] = RX_TIMEOUT_PERIOD_BASE;
 274         buf[1] = 0xFFFF >> 8;
 275         buf[2] = 0xFFFF & 0xFF;
 276         hal.WriteCommand(SX1280_RADIO_SET_RX, buf, sizeof(buf), radioNumber, 100);
 277         break;
 278
 279     case SX1280_MODE_TX:
 280         //uses timeout Time-out duration = periodBase * periodBaseCount
 281         buf[0] = RX_TIMEOUT_PERIOD_BASE;
 282         buf[1] = 0xFF; // no timeout set for now
 283         buf[2] = 0xFF; // TODO dynamic timeout based on expected onairtime
 284         hal.WriteCommand(SX1280_RADIO_SET_TX, buf, sizeof(buf), radioNumber, 100);
 285         break;
 286
 287     case SX1280_MODE_CAD:
 288         break;
 289
 290     default:
 291         break;
 292     }
 293
 294     currOpmode = OPmode;
 295 }
 296
 297 void SX1280Driver::ConfigModParamsLoRa(uint8_t bw, uint8_t sf, uint8_t cr)
 298 {
 299     // Care must therefore be taken to ensure that modulation parameters are set using the command
 300     // SetModulationParam() only after defining the packet type SetPacketType() to be used
 301
 302     WORD_ALIGNED_ATTR uint8_t rfparams[3] = {sf, bw, cr};
 303
 304     hal.WriteCommand(SX1280_RADIO_SET_MODULATIONPARAMS, rfparams, sizeof(rfparams), SX12XX_Radio_All, 25);
 305
 306     switch (sf)
 307     {
 308     case SX1280_LORA_SF5:
 309     case SX1280_LORA_SF6:
 310         hal.WriteRegister(SX1280_REG_SF_ADDITIONAL_CONFIG, 0x1E, SX12XX_Radio_All); // for SF5 or SF6
 311         break;
 312     case SX1280_LORA_SF7:
 313     case SX1280_LORA_SF8:
 314         hal.WriteRegister(SX1280_REG_SF_ADDITIONAL_CONFIG, 0x37, SX12XX_Radio_All); // for SF7 or SF8
 315         break;
 316     default:
 317         hal.WriteRegister(SX1280_REG_SF_ADDITIONAL_CONFIG, 0x32, SX12XX_Radio_All); // for SF9, SF10, SF11, SF12
 318     }
 319     // Datasheet in LoRa Operation says "After SetModulationParams command:
 320     // In all cases 0x1 must be written to the Frequency Error Compensation mode register 0x093C"
 321     // However, this causes CRC errors for SF9 when using a high deviation TX (145kHz) and not using Explicit Header mode.
 322     // The default register value (0x1b) seems most compatible, so don't mess with it
 323     // InvertIQ=0 0x00=No reception 0x01=Poor reception w/o Explicit Header 0x02=OK 0x03=OK
 324     // InvertIQ=1 0x00, 0x01, 0x02, and 0x03=Poor reception w/o Explicit Header
 325     // hal.WriteRegister(SX1280_REG_FREQ_ERR_CORRECTION, 0x03, SX12XX_Radio_All);
 326 }
 327
 328 void SX1280Driver::SetPacketParamsLoRa(uint8_t PreambleLength, SX1280_RadioLoRaPacketLengthsModes_t HeaderType,
 329                                        uint8_t PayloadLength, uint8_t InvertIQ)
 330 {
 331     uint8_t buf[7];
 332
 333     buf[0] = PreambleLength;
 334     buf[1] = HeaderType;
 335     buf[2] = PayloadLength;
 336     buf[3] = SX1280_LORA_CRC_OFF;
 337     buf[4] = InvertIQ ? SX1280_LORA_IQ_INVERTED : SX1280_LORA_IQ_NORMAL;
 338     buf[5] = 0x00;
 339     buf[6] = 0x00;
 340
 341     hal.WriteCommand(SX1280_RADIO_SET_PACKETPARAMS, buf, sizeof(buf), SX12XX_Radio_All, 20);
 342
 343     // FEI only triggers in Lora mode when the header is present :(
 344     modeSupportsFei = HeaderType == SX1280_LORA_PACKET_VARIABLE_LENGTH;
 345 }
 346
 347 void SX1280Driver::ConfigModParamsFLRC(uint8_t bw, uint8_t cr, uint8_t bt)
 348 {
 349     WORD_ALIGNED_ATTR uint8_t rfparams[3] = {bw, cr, bt};
 350     hal.WriteCommand(SX1280_RADIO_SET_MODULATIONPARAMS, rfparams, sizeof(rfparams), SX12XX_Radio_All, 110);
 351 }
 352
 353 void SX1280Driver::SetPacketParamsFLRC(uint8_t HeaderType,
 354                                        uint8_t PreambleLength,
 355                                        uint8_t PayloadLength,
 356                                        uint32_t syncWord,
 357                                        uint16_t crcSeed,
 358                                        uint8_t cr)
 359 {
 360     if (PreambleLength < 8)
 361         PreambleLength = 8;
 362     PreambleLength = ((PreambleLength / 4) - 1) << 4;
 363
 364     uint8_t buf[7];
 365     buf[0] = PreambleLength;                    // AGCPreambleLength
 366     buf[1] = SX1280_FLRC_SYNC_WORD_LEN_P32S;    // SyncWordLength
 367     buf[2] = SX1280_FLRC_RX_MATCH_SYNC_WORD_1;  // SyncWordMatch
 368     buf[3] = HeaderType;                        // PacketType
 369     buf[4] = PayloadLength;                     // PayloadLength
 370     buf[5] = SX1280_FLRC_CRC_3_BYTE;            // CrcLength
 371     buf[6] = 0x08;                              // Must be whitening disabled
 372     hal.WriteCommand(SX1280_RADIO_SET_PACKETPARAMS, buf, sizeof(buf), SX12XX_Radio_All, 30);
 373
 374     // CRC seed (use dedicated cipher)
 375     buf[0] = (uint8_t)(crcSeed >> 8);
 376     buf[1] = (uint8_t)crcSeed;
 377     hal.WriteRegister(SX1280_REG_FLRC_CRC_SEED, buf, 2, SX12XX_Radio_All);
 378
 379     // Set SyncWord1
 380     buf[0] = (uint8_t)(syncWord >> 24);
 381     buf[1] = (uint8_t)(syncWord >> 16);
 382     buf[2] = (uint8_t)(syncWord >> 8);
 383     buf[3] = (uint8_t)syncWord;
 384
 385     // DS_SX1280-1_V3.2.pdf - 16.4 FLRC Modem: Increased PER in FLRC Packets with Synch Word
 386     if (((cr == SX1280_FLRC_CR_1_2) || (cr == SX1280_FLRC_CR_3_4)) &&
 387         ((buf[0] == 0x8C && buf[1] == 0x38) || (buf[0] == 0x63 && buf[1] == 0x0E)))
 388     {
 389         uint8_t temp = buf[0];
 390         buf[0] = buf[1];
 391         buf[1] = temp;
 392         // For SX1280_FLRC_CR_3_4 the datasheet also says
 393         // "In addition to this the two LSB values XX XX must not be in the range 0x0000 to 0x3EFF"
 394         if (cr == SX1280_FLRC_CR_3_4 && buf[3] <= 0x3e)
 395             buf[3] |= 0x80; // 0x80 or 0x40 would work
 396     }
 397
 398     hal.WriteRegister(SX1280_REG_FLRC_SYNC_WORD, buf, 4, SX12XX_Radio_All);
 399
 400     // FEI only works in Lora and Ranging mode
 401     modeSupportsFei = false;
 402 }
 403
 404 void ICACHE_RAM_ATTR SX1280Driver::SetFrequencyHz(uint32_t freq, SX12XX_Radio_Number_t radioNumber)
 405 {
 406     uint32_t regfreq = (uint32_t)((double)freq / (double)FREQ_STEP);
 407
 408     SetFrequencyReg(regfreq, radioNumber);
 409 }
 410
 411 void ICACHE_RAM_ATTR SX1280Driver::SetFrequencyReg(uint32_t regfreq, SX12XX_Radio_Number_t radioNumber)
 412 {
 413     WORD_ALIGNED_ATTR uint8_t buf[3] = {0};
 414
 415     buf[0] = (uint8_t)((regfreq >> 16) & 0xFF);
 416     buf[1] = (uint8_t)((regfreq >> 8) & 0xFF);
 417     buf[2] = (uint8_t)(regfreq & 0xFF);
 418
 419     hal.WriteCommand(SX1280_RADIO_SET_RFFREQUENCY, buf, sizeof(buf), radioNumber);
 420
 421     currFreq = regfreq;
 422 }
 423
 424 void SX1280Driver::SetFIFOaddr(uint8_t txBaseAddr, uint8_t rxBaseAddr)
 425 {
 426     uint8_t buf[2];
 427
 428     buf[0] = txBaseAddr;
 429     buf[1] = rxBaseAddr;
 430     hal.WriteCommand(SX1280_RADIO_SET_BUFFERBASEADDRESS, buf, sizeof(buf), SX12XX_Radio_All);
 431 }
 432
 433 void SX1280Driver::SetDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask)
 434 {
 435     uint8_t buf[8];
 436
 437     buf[0] = (uint8_t)((irqMask >> 8) & 0x00FF);
 438     buf[1] = (uint8_t)(irqMask & 0x00FF);
 439     buf[2] = (uint8_t)((dio1Mask >> 8) & 0x00FF);
 440     buf[3] = (uint8_t)(dio1Mask & 0x00FF);
 441     buf[4] = (uint8_t)((dio2Mask >> 8) & 0x00FF);
 442     buf[5] = (uint8_t)(dio2Mask & 0x00FF);
 443     buf[6] = (uint8_t)((dio3Mask >> 8) & 0x00FF);
 444     buf[7] = (uint8_t)(dio3Mask & 0x00FF);
 445
 446     hal.WriteCommand(SX1280_RADIO_SET_DIOIRQPARAMS, buf, sizeof(buf), SX12XX_Radio_All);
 447 }
 448
 449 uint16_t ICACHE_RAM_ATTR SX1280Driver::GetIrqStatus(SX12XX_Radio_Number_t radioNumber)
 450 {
 451     uint8_t status[2];
 452
 453     hal.ReadCommand(SX1280_RADIO_GET_IRQSTATUS, status, 2, radioNumber);
 454     return status[0] << 8 | status[1];
 455 }
 456
 457 void ICACHE_RAM_ATTR SX1280Driver::ClearIrqStatus(uint16_t irqMask, SX12XX_Radio_Number_t radioNumber)
 458 {
 459     uint8_t buf[2];
 460
 461     buf[0] = (uint8_t)(((uint16_t)irqMask >> 8) & 0x00FF);
 462     buf[1] = (uint8_t)((uint16_t)irqMask & 0x00FF);
 463
 464     hal.WriteCommand(SX1280_RADIO_CLR_IRQSTATUS, buf, sizeof(buf), radioNumber);
 465 }
 466
 467 void ICACHE_RAM_ATTR SX1280Driver::TXnbISR()
 468 {
 469     currOpmode = SX1280_MODE_FS; // radio goes to FS after TX
 470 #ifdef DEBUG_SX1280_OTA_TIMING
 471     endTX = micros();
 472     DBGLN("TOA: %d", endTX - beginTX);
 473 #endif
 474     CommitOutputPower();
 475     TXdoneCallback();
 476 }
 477
 478 void ICACHE_RAM_ATTR SX1280Driver::TXnb(uint8_t * data, uint8_t size, SX12XX_Radio_Number_t radioNumber)
 479 {
 480     transmittingRadio = radioNumber;
 481
 482     //catch TX timeout
 483     if (currOpmode == SX1280_MODE_TX)
 484     {
 485         DBGLN("Timeout!");
 486         SetMode(fallBackMode, SX12XX_Radio_All);
 487         ClearIrqStatus(SX1280_IRQ_RADIO_ALL, SX12XX_Radio_All);
 488         TXnbISR();
 489         return;
 490     }
 491
 492     if (radioNumber == SX12XX_Radio_NONE)
 493     {
 494         instance->SetMode(fallBackMode, SX12XX_Radio_All);
 495         return;
 496     }
 497
 498 #if defined(DEBUG_RCVR_SIGNAL_STATS)
 499     if (radioNumber == SX12XX_Radio_All || radioNumber == SX12XX_Radio_1)
 500     {
 501         instance->rxSignalStats[0].telem_count++;
 502     }
 503     if (radioNumber == SX12XX_Radio_All || radioNumber == SX12XX_Radio_2)
 504     {
 505         instance->rxSignalStats[1].telem_count++;
 506     }
 507 #endif
 508
 509     // Normal diversity mode
 510     if (GPIO_PIN_NSS_2 != UNDEF_PIN && radioNumber != SX12XX_Radio_All)
 511     {
 512         // Make sure the unused radio is in FS mode and will not receive the tx packet.
 513         if (radioNumber == SX12XX_Radio_1)
 514         {
 515             instance->SetMode(fallBackMode, SX12XX_Radio_2);
 516         }
 517         else
 518         {
 519             instance->SetMode(fallBackMode, SX12XX_Radio_1);
 520         }
 521     }
 522
 523     RFAMP.TXenable(radioNumber); // do first to allow PA stablise
 524     hal.WriteBuffer(0x00, data, size, radioNumber); //todo fix offset to equal fifo addr
 525     instance->SetMode(SX1280_MODE_TX, radioNumber);
 526
 527 #ifdef DEBUG_SX1280_OTA_TIMING
 528     beginTX = micros();
 529 #endif
 530 }
 531
 532 bool ICACHE_RAM_ATTR SX1280Driver::RXnbISR(uint16_t irqStatus, SX12XX_Radio_Number_t radioNumber)
 533 {
 534     // In continuous receive mode, the device stays in Rx mode
 535     if (timeout != 0xFFFF)
 536     {
 537         // From table 11-28, pg 81 datasheet rev 3.2
 538         // upon successsful receipt, when the timer is active or in single mode, it returns to STDBY_RC
 539         // but because we have AUTO_FS enabled we automatically transition to state SX1280_MODE_FS
 540         currOpmode = SX1280_MODE_FS;
 541     }
 542
 543     rx_status fail = SX12XX_RX_OK;
 544     // The SYNCWORD_VALID bit isn't set on LoRa, it has no synch (sic) word, and CRC is only on for FLRC
 545     if (packet_mode == SX1280_PACKET_TYPE_FLRC)
 546     {
 547         fail = ((irqStatus & SX1280_IRQ_CRC_ERROR) ? SX12XX_RX_CRC_FAIL : SX12XX_RX_OK) |
 548                ((irqStatus & SX1280_IRQ_SYNCWORD_VALID) ? SX12XX_RX_OK : SX12XX_RX_SYNCWORD_ERROR) |
 549                ((irqStatus & SX1280_IRQ_SYNCWORD_ERROR) ? SX12XX_RX_SYNCWORD_ERROR : SX12XX_RX_OK);
 550     }
 551     if (fail == SX12XX_RX_OK)
 552     {
 553         uint8_t const FIFOaddr = GetRxBufferAddr(radioNumber);
 554         hal.ReadBuffer(FIFOaddr, RXdataBuffer, PayloadLength, radioNumber);
 555     }
 556
 557     return RXdoneCallback(fail);
 558 }
 559
 560 void ICACHE_RAM_ATTR SX1280Driver::RXnb(SX1280_RadioOperatingModes_t rxMode)
 561 {
 562     RFAMP.RXenable();
 563     SetMode(rxMode, SX12XX_Radio_All);
 564 }
 565
 566 uint8_t ICACHE_RAM_ATTR SX1280Driver::GetRxBufferAddr(SX12XX_Radio_Number_t radioNumber)
 567 {
 568     WORD_ALIGNED_ATTR uint8_t status[2] = {0};
 569     hal.ReadCommand(SX1280_RADIO_GET_RXBUFFERSTATUS, status, 2, radioNumber);
 570     return status[1];
 571 }
 572
 573 void ICACHE_RAM_ATTR SX1280Driver::GetStatus(SX12XX_Radio_Number_t radioNumber)
 574 {
 575     uint8_t status = 0;
 576     hal.ReadCommand(SX1280_RADIO_GET_STATUS, (uint8_t *)&status, 1, radioNumber);
 577     DBGLN("Status: %x, %x, %x", (0b11100000 & status) >> 5, (0b00011100 & status) >> 2, 0b00000001 & status);
 578 }
 579
 580 bool ICACHE_RAM_ATTR SX1280Driver::GetFrequencyErrorbool()
 581 {
 582     // Only need the highest bit of the 20-bit FEI to determine the direction
 583     uint8_t feiMsb = hal.ReadRegister(SX1280_REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB, lastSuccessfulPacketRadio);
 584     // fei & (1 << 19) and flip sign if IQinverted
 585     if (feiMsb & 0x08)
 586         return IQinverted;
 587     else
 588         return !IQinverted;
 589 }
 590
 591 int8_t ICACHE_RAM_ATTR SX1280Driver::GetRssiInst(SX12XX_Radio_Number_t radioNumber)
 592 {
 593     uint8_t status = 0;
 594
 595     hal.ReadCommand(SX1280_RADIO_GET_RSSIINST, (uint8_t *)&status, 1, radioNumber);
 596     return -(int8_t)(status / 2);
 597 }
 598
 599 void ICACHE_RAM_ATTR SX1280Driver::GetLastPacketStats()
 600 {
 601     SX12XX_Radio_Number_t radio[2] = {SX12XX_Radio_1, SX12XX_Radio_2};
 602     bool gotRadio[2] = {false, false}; // one-radio default.
 603     uint8_t processingRadioIdx = (instance->processingPacketRadio == SX12XX_Radio_1) ? 0 : 1;
 604     uint8_t secondRadioIdx = !processingRadioIdx;
 605
 606     // processingRadio always passed the sanity check here
 607     gotRadio[processingRadioIdx] = true;
 608
 609     // if it's a dual radio, and if it's the first IRQ
 610     // (don't need this if it's the second IRQ, because we know the first IRQ is already failed)
 611     if (instance->isFirstRxIrq && GPIO_PIN_NSS_2 != UNDEF_PIN)
 612     {
 613         bool isSecondRadioGotData = false;
 614
 615         uint16_t secondIrqStatus = instance->GetIrqStatus(radio[secondRadioIdx]);
 616         if(secondIrqStatus&SX1280_IRQ_RX_DONE)
 617         {
 618             rx_status second_rx_fail = SX12XX_RX_OK;
 619             if (packet_mode == SX1280_PACKET_TYPE_FLRC)
 620             {
 621                second_rx_fail = ((secondIrqStatus & SX1280_IRQ_CRC_ERROR) ? SX12XX_RX_CRC_FAIL : SX12XX_RX_OK) |
 622                                 ((secondIrqStatus & SX1280_IRQ_SYNCWORD_VALID) ? SX12XX_RX_OK : SX12XX_RX_SYNCWORD_ERROR) |
 623                                 ((secondIrqStatus & SX1280_IRQ_SYNCWORD_ERROR) ? SX12XX_RX_SYNCWORD_ERROR : SX12XX_RX_OK);
 624             }
 625             if (second_rx_fail == SX12XX_RX_OK)
 626             {
 627                 uint8_t const FIFOaddr = GetRxBufferAddr(radio[secondRadioIdx]);
 628                 WORD_ALIGNED_ATTR uint8_t RXdataBuffer_second[RXBuffSize];
 629                 hal.ReadBuffer (FIFOaddr, RXdataBuffer_second, PayloadLength, radio[secondRadioIdx]);
 630
 631                 // if the second packet is same to the first, it's valid
 632                 if(memcmp(RXdataBuffer, RXdataBuffer_second, PayloadLength) == 0)
 633                 {
 634                     isSecondRadioGotData = true;
 635                 }
 636             }
 637         }
 638
 639         // second radio received the same packet to the processing radio
 640         gotRadio[secondRadioIdx] = isSecondRadioGotData;
 641         #if defined(DEBUG_RCVR_SIGNAL_STATS)
 642         if(!isSecondRadioGotData)
 643         {
 644             instance->rxSignalStats[secondRadioIdx].fail_count++;
 645         }
 646         #endif
 647     }
 648
 649     uint8_t status[2];
 650     int8_t rssi[2];
 651     int8_t snr[2];
 652
 653     for(uint8_t i=0;i<2;i++)
 654     {
 655         if (gotRadio[i])
 656         {
 657             hal.ReadCommand(SX1280_RADIO_GET_PACKETSTATUS, status, 2, radio[i]);
 658
 659             if (packet_mode == SX1280_PACKET_TYPE_FLRC)
 660             {
 661                 // No SNR in FLRC mode
 662                 rssi[i] = -(int8_t)(status[1] / 2);
 663                 snr[i] = 0;
 664             }
 665             else
 666             {
 667                 // LoRa mode has both RSSI and SNR
 668                 rssi[i] = -(int8_t)(status[0] / 2);
 669                 snr[i] = (int8_t)status[1];
 670
 671                 // https://www.mouser.com/datasheet/2/761/DS_SX1280-1_V2.2-1511144.pdf p84
 672                 // need to subtract SNR from RSSI when SNR <= 0;
 673                 int8_t negOffset = (snr[i] < 0) ? (snr[i] / RADIO_SNR_SCALE) : 0;
 674                 rssi[i] += negOffset;
 675             }
 676
 677             // If radio # is 0, update LastPacketRSSI, otherwise LastPacketRSSI2
 678             (i == 0) ? LastPacketRSSI = rssi[i] : LastPacketRSSI2 = rssi[i];
 679             // Update whatever SNRs we have
 680             LastPacketSNRRaw = snr[i];
 681         }
 682     }
 683
 684     // by default, set the last successful packet radio to be the current processing radio (which got a successful packet)
 685     instance->lastSuccessfulPacketRadio = instance->processingPacketRadio;
 686
 687     // when both radio got the packet, use the better RSSI one
 688     if(gotRadio[0] && gotRadio[1])
 689     {
 690         LastPacketSNRRaw = instance->fuzzy_snr(snr[0], snr[1], instance->FuzzySNRThreshold);
 691         // Update the last successful packet radio to be the one with better signal strength
 692         instance->lastSuccessfulPacketRadio = (rssi[0]>rssi[1])? radio[0]: radio[1];
 693     }
 694
 695 #if defined(DEBUG_RCVR_SIGNAL_STATS)
 696     // stat updates
 697     for (uint8_t i = 0; i < 2; i++)
 698     {
 699         if (gotRadio[i])
 700         {
 701             instance->rxSignalStats[i].irq_count++;
 702             instance->rxSignalStats[i].rssi_sum += rssi[i];
 703             instance->rxSignalStats[i].snr_sum += snr[i];
 704             if (snr[i] > instance->rxSignalStats[i].snr_max)
 705             {
 706                 instance->rxSignalStats[i].snr_max = snr[i];
 707             }
 708             LastPacketSNRRaw = snr[i];
 709         }
 710     }
 711     if(gotRadio[0] || gotRadio[1])
 712     {
 713         instance->irq_count_or++;
 714     }
 715     if(gotRadio[0] && gotRadio[1])
 716     {
 717         instance->irq_count_both++;
 718     }
 719 #endif
 720 }
 721
 722 void ICACHE_RAM_ATTR SX1280Driver::IsrCallback_1()
 723 {
 724     instance->IsrCallback(SX12XX_Radio_1);
 725 }
 726
 727 void ICACHE_RAM_ATTR SX1280Driver::IsrCallback_2()
 728 {
 729     instance->IsrCallback(SX12XX_Radio_2);
 730 }
 731
 732 void ICACHE_RAM_ATTR SX1280Driver::IsrCallback(SX12XX_Radio_Number_t radioNumber)
 733 {
 734     instance->processingPacketRadio = radioNumber;
 735     SX12XX_Radio_Number_t irqClearRadio = radioNumber;
 736
 737     uint16_t irqStatus = instance->GetIrqStatus(radioNumber);
 738     if (irqStatus & SX1280_IRQ_TX_DONE)
 739     {
 740         RFAMP.TXRXdisable();
 741         instance->TXnbISR();
 742         irqClearRadio = SX12XX_Radio_All;
 743     }
 744     else if (irqStatus & SX1280_IRQ_RX_DONE)
 745     {
 746         if (instance->RXnbISR(irqStatus, radioNumber))
 747         {
 748             irqClearRadio = SX12XX_Radio_All; // Packet received so clear all radios and dont spend extra time retrieving data.
 749         }
 750 #if defined(DEBUG_RCVR_SIGNAL_STATS)
 751         else
 752         {
 753             instance->rxSignalStats[(radioNumber == SX12XX_Radio_1) ? 0 : 1].fail_count++;
 754         }
 755 #endif
 756         instance->isFirstRxIrq = false;   // RX isr is already fired in this period. (reset to true in tock)
 757     }
 758     else if (irqStatus == SX1280_IRQ_RADIO_NONE)
 759     {
 760         return;
 761     }
 762     instance->ClearIrqStatus(SX1280_IRQ_RADIO_ALL, irqClearRadio);
 763 }