src/lib/VTXSPI/devVTXSPI.cpp

   1 #if defined(HAS_VTX_SPI)
   2
   3 #include "devVTXSPI.h"
   4 #include "targets.h"
   5 #include "common.h"
   6 #include "helpers.h"
   7 #include "hwTimer.h"
   8 #include "logging.h"
   9 #include <SPI.h>
  10 #include "PWM.h"
  11
  12 #define SYNTHESIZER_REGISTER_A                  0x00
  13 #define SYNTHESIZER_REGISTER_B                  0x01
  14 #define SYNTHESIZER_REGISTER_C                  0x02
  15 #define RF_VCO_DFC_CONTROL_REGISTER             0x03
  16 #define VCO_CONTROL_REGISTER                    0x04
  17 #define VCO_CONTROL_REGISTER_CONT               0x05
  18 #define AUDIO_MODULATOR_CONTROL_REGISTER        0x06
  19 #define PRE_DRIVER_AND_PA_CONTROL_REGISTER      0x07
  20 #define STATE_REGISTER                          0x0F
  21
  22 #define SYNTH_REG_A_DEFAULT                     0x0190
  23
  24 #define POWER_AMP_ON                            0b00000100111110111111
  25 #define POWER_AMP_OFF                           0x00
  26 // ESP32 DAC pins are 0-255
  27 #define MIN_DAC                                 1 // Testing required.
  28 #define MAX_DAC                                 250 // Absolute max is 255.  But above 250 does nothing.
  29 // PWM is 0-4095
  30 #define MIN_PWM                                 2000 // could be even higher than that, depends on HW.
  31 #define MAX_PWM                                 3700 // Absolute max is 4095. But above 3600 does nothing.
  32
  33 #define VPD_BUFFER                              5
  34
  35 #define READ_BIT                                0x00
  36 #define WRITE_BIT                               0x01
  37
  38 #define RTC6705_BOOT_DELAY                      350
  39 #define RTC6705_PLL_SETTLE_TIME_MS              500 // ms - after changing frequency turn amps back on after this time for clean switching
  40 #define VTX_POWER_INTERVAL_MS                   20
  41
  42 #define BUF_PACKET_SIZE                         4 // 25b packet in 4 bytes
  43
  44 #if defined(PLATFORM_ESP32)
  45 pwm_channel_t rfAmpPwmChannel = -1;
  46 #endif
  47
  48 uint16_t vtxSPIFrequency = 6000;
  49 static uint16_t vtxSPIFrequencyCurrent = 6000;
  50
  51 uint8_t vtxSPIPowerIdx = 0;
  52 static uint8_t vtxSPIPowerIdxCurrent = 0;
  53
  54 uint8_t vtxSPIPitmode = 1;
  55 static uint8_t vtxSPIPitmodeCurrent = 1;
  56
  57 bool vtxPowerAmpEnable = false;
  58 bool vtxPowerAmpEnableCurrent = false;
  59
  60 static uint8_t RfAmpVrefState = 0;
  61
  62 static uint16_t vtxSPIPWM = MAX_PWM;
  63 static uint16_t vtxPreviousSPIPWM = 0;
  64
  65 static uint16_t vtxMinPWM = MIN_PWM;
  66 static uint16_t vtxMaxPWM = MAX_PWM;
  67
  68 static uint16_t VpdSetPoint = 0;
  69 static uint16_t Vpd = 0;
  70
  71 static bool stopVtxMonitoring = false;
  72
  73 #define VPD_SETPOINT_0_MW                       VPD_BUFFER // to avoid overflow
  74 #define VPD_SETPOINT_YOLO_MW                    2250
  75 const uint16_t *VpdSetPointArray25mW = nullptr;
  76 const uint16_t *VpdSetPointArray100mW = nullptr;
  77 const uint16_t *PwmArray25mW = nullptr;
  78 const uint16_t *PwmArray100mW = nullptr;
  79
  80 uint16_t VpdFreqArray[] = {5650, 5750, 5850, 5950};
  81 uint8_t VpdSetPointCount =  ARRAY_SIZE(VpdFreqArray);
  82
  83 static SPIClass *vtxSPI;
  84
  85 static void rtc6705WriteRegister(uint32_t regData)
  86 {
  87     // When sharing the SPI Bus control of the NSS pin is done by us
  88     if (GPIO_PIN_SPI_VTX_SCK == GPIO_PIN_SCK)
  89     {
  90         vtxSPI->setBitOrder(LSBFIRST);
  91         digitalWrite(GPIO_PIN_SPI_VTX_NSS, LOW);
  92     }
  93
  94     // On some ESP32 MCUs there's a silicon bug which affects all 8n+1 bit and 1 bit transfers where the
  95     // last bit sent is corrupt.
  96     // See: https://github.com/ExpressLRS/ExpressLRS/pull/2406#issuecomment-1722573356
  97     //
  98     // To reproduce, use an ESP32S3 and 25 bit transfers, change from channel A4 to A1, then A1 to A4 (ok),
  99     // then A4 to A3, then A3 to A4 (fail)
 100     //
 101     // 12816, 9286833 appears on the scope when changing from A:1->A:4
 102     // 12816, 26064049 appears on the scope when changing from A:3->A:4
 103     //
 104     // 9286833  = 0_1000_1101_1011_0100_1011_0001
 105     // 26064049 = 1_1000_1101_1011_0100_1011_0001
 106     //
 107     // Since the RTC6705 just ignores the extra bits, we send 32 bits and the RTC6705 ignores the last 7 bits.
 108     vtxSPI->transfer32(regData);
 109
 110     if (GPIO_PIN_SPI_VTX_SCK == GPIO_PIN_SCK)
 111     {
 112         digitalWrite(GPIO_PIN_SPI_VTX_NSS, HIGH);
 113         vtxSPI->setBitOrder(MSBFIRST);
 114     }
 115 }
 116
 117 static void rtc6705ResetSynthRegA()
 118 {
 119   uint32_t regData = SYNTHESIZER_REGISTER_A | (WRITE_BIT << 4) | (SYNTH_REG_A_DEFAULT << 5);
 120   rtc6705WriteRegister(regData);
 121 }
 122
 123 static void rtc6705SetFrequency(uint32_t freq)
 124 {
 125     rtc6705ResetSynthRegA();
 126
 127     VTxOutputMinimum(); // Set power to zero for clear channel switching
 128
 129     uint32_t f = 25 * freq;
 130     uint32_t SYN_RF_N_REG = f / 64;
 131     uint32_t SYN_RF_A_REG = f % 64;
 132
 133     uint32_t regData = SYNTHESIZER_REGISTER_B | (WRITE_BIT << 4) | (SYN_RF_A_REG << 5) | (SYN_RF_N_REG << 12);
 134     rtc6705WriteRegister(regData);
 135 }
 136
 137 static void rtc6705PowerAmpOn()
 138 {
 139     uint32_t regData = PRE_DRIVER_AND_PA_CONTROL_REGISTER | (WRITE_BIT << 4) | (POWER_AMP_ON << 5);
 140     rtc6705WriteRegister(regData);
 141 }
 142
 143 static void RfAmpVrefOn()
 144 {
 145     if (!RfAmpVrefState) digitalWrite(GPIO_PIN_RF_AMP_VREF, HIGH);
 146
 147     RfAmpVrefState = 1;
 148 }
 149
 150 static void RfAmpVrefOff()
 151 {
 152     if (RfAmpVrefState) digitalWrite(GPIO_PIN_RF_AMP_VREF, LOW);
 153
 154     RfAmpVrefState = 0;
 155 }
 156
 157 static void setPWM()
 158 {
 159     if (vtxSPIPWM == vtxPreviousSPIPWM) {
 160         return;
 161     }
 162     vtxPreviousSPIPWM = vtxSPIPWM;
 163 #if defined(PLATFORM_ESP32_S3) || defined(PLATFORM_ESP32_C3)
 164     PWM.setDuty(rfAmpPwmChannel, vtxSPIPWM * 1000 / 4096);
 165 #elif defined(PLATFORM_ESP32)
 166     if (GPIO_PIN_RF_AMP_PWM == 25 || GPIO_PIN_RF_AMP_PWM == 26)
 167     {
 168         dacWrite(GPIO_PIN_RF_AMP_PWM, vtxSPIPWM >> 4);
 169     }
 170     else
 171     {
 172         PWM.setDuty(rfAmpPwmChannel, vtxSPIPWM * 1000 / 4096);
 173     }
 174 #else
 175     analogWrite(GPIO_PIN_RF_AMP_PWM, vtxSPIPWM);
 176 #endif
 177 }
 178
 179 void VTxOutputMinimum()
 180 {
 181     RfAmpVrefOff();
 182
 183     vtxSPIPWM = vtxMaxPWM;
 184     setPWM();
 185 }
 186
 187 static void VTxOutputIncrease()
 188 {
 189     if (vtxSPIPWM > vtxMinPWM) vtxSPIPWM -= 1;
 190     setPWM();
 191 }
 192
 193 static void VTxOutputDecrease()
 194 {
 195     if (vtxSPIPWM < vtxMaxPWM) vtxSPIPWM += 1;
 196     setPWM();
 197 }
 198
 199 static uint16_t LinearInterpVpdSetPointArray(const uint16_t VpdSetPointArray[])
 200 {
 201     uint16_t newVpd = 0;
 202
 203     if (vtxSPIFrequencyCurrent <= VpdFreqArray[0])
 204     {
 205         newVpd = VpdSetPointArray[0];
 206     }
 207     else if (vtxSPIFrequencyCurrent >= VpdFreqArray[VpdSetPointCount - 1])
 208     {
 209         newVpd = VpdSetPointArray[VpdSetPointCount - 1];
 210     }
 211     else
 212     {
 213         for (uint8_t i = 0; i < (VpdSetPointCount - 1); i++)
 214         {
 215             if (vtxSPIFrequencyCurrent < VpdFreqArray[i + 1])
 216             {
 217                 newVpd = VpdSetPointArray[i] + ((VpdSetPointArray[i + 1]-VpdSetPointArray[i])/(VpdFreqArray[i + 1]-VpdFreqArray[i])) * (vtxSPIFrequencyCurrent - VpdFreqArray[i]);
 218             }
 219         }
 220     }
 221
 222     return newVpd;
 223 }
 224
 225 static uint16_t LinearInterpSetPwm(const uint16_t PwmArray[])
 226 {
 227     uint16_t newPwm = 0;
 228
 229     if (vtxSPIFrequencyCurrent <= VpdFreqArray[0])
 230     {
 231         newPwm = PwmArray[0];
 232     }
 233     else if (vtxSPIFrequencyCurrent >= VpdFreqArray[VpdSetPointCount - 1])
 234     {
 235         newPwm = PwmArray[VpdSetPointCount - 1];
 236     }
 237     else
 238     {
 239         for (uint8_t i = 0; i < (VpdSetPointCount - 1); i++)
 240         {
 241             if (vtxSPIFrequencyCurrent < VpdFreqArray[i + 1])
 242             {
 243                 newPwm = PwmArray[i] + ((PwmArray[i + 1]-PwmArray[i])/(VpdFreqArray[i + 1]-VpdFreqArray[i])) * (vtxSPIFrequencyCurrent - VpdFreqArray[i]);
 244             }
 245         }
 246     }
 247
 248     return newPwm;
 249 }
 250
 251 static void SetVpdSetPoint()
 252 {
 253     switch (vtxSPIPowerIdx)
 254     {
 255     case 1: // 0 mW
 256         VpdSetPoint = VPD_SETPOINT_0_MW;
 257         vtxSPIPWM = vtxMaxPWM;
 258         break;
 259
 260     case 2: // RCE
 261     case 3: // 25 mW
 262         VpdSetPoint = LinearInterpVpdSetPointArray(VpdSetPointArray25mW);
 263         vtxSPIPWM = LinearInterpSetPwm(PwmArray25mW);
 264         break;
 265
 266     case 4: // 100 mW
 267         VpdSetPoint = LinearInterpVpdSetPointArray(VpdSetPointArray100mW);
 268         vtxSPIPWM = LinearInterpSetPwm(PwmArray100mW);
 269         break;
 270
 271     default: // YOLO mW
 272         VpdSetPoint = VPD_SETPOINT_YOLO_MW;
 273         vtxSPIPWM = vtxMinPWM;
 274         break;
 275     }
 276
 277     setPWM();
 278     DBGLN("VTX: Setting new VPD setpoint: %d, initial PWM: %d", VpdSetPoint, vtxSPIPWM);
 279 }
 280
 281 static void checkOutputPower()
 282 {
 283     if (vtxSPIPitmodeCurrent)
 284     {
 285         VTxOutputMinimum();
 286     }
 287     else
 288     {
 289         RfAmpVrefOn();
 290
 291         uint16_t VpdReading = analogRead(GPIO_PIN_RF_AMP_VPD); // WARNING - Max input 1.0V !!!!
 292
 293         Vpd = (8 * Vpd + 2 * VpdReading) / 10; // IIR filter
 294
 295         if (Vpd < (VpdSetPoint - VPD_BUFFER))
 296         {
 297             VTxOutputIncrease();
 298         }
 299         else if (Vpd > (VpdSetPoint + VPD_BUFFER))
 300         {
 301             VTxOutputDecrease();
 302         }
 303
 304         //DBGLN("VTX: VPD setpoint=%d, raw=%d, filtered=%d, PWM=%d", VpdSetPoint, VpdReading, Vpd, vtxSPIPWM);
 305     }
 306 }
 307
 308 #if defined(VTX_OUTPUT_CALIBRATION)
 309 int sampleCount = 0;
 310 int calibFreqIndex = 0;
 311 #define CALIB_SAMPLES 10
 312
 313 static int gatherOutputCalibrationData()
 314 {
 315     if (VpdSetPoint <= VPD_SETPOINT_YOLO_MW && calibFreqIndex < VpdSetPointCount)
 316     {
 317         sampleCount++;
 318         checkOutputPower();
 319         DBGLN("VTX Freq=%d, VPD setpoint=%d, VPD=%d, PWM=%d, sample=%d", VpdFreqArray[calibFreqIndex], VpdSetPoint, Vpd, vtxSPIPWM, sampleCount);
 320         if (sampleCount >= CALIB_SAMPLES)
 321         {
 322             VpdSetPoint += VPD_BUFFER;
 323             sampleCount = 0;
 324         }
 325
 326         if (VpdSetPoint > VPD_SETPOINT_YOLO_MW)
 327         {
 328             calibFreqIndex++;
 329             rtc6705SetFrequency(VpdFreqArray[calibFreqIndex]);
 330             VpdSetPoint = VPD_BUFFER;
 331             return RTC6705_PLL_SETTLE_TIME_MS;
 332         }
 333         return VTX_POWER_INTERVAL_MS;
 334     }
 335     return DURATION_NEVER;
 336 }
 337 #endif
 338
 339 void disableVTxSpi()
 340 {
 341     stopVtxMonitoring = true;
 342     VTxOutputMinimum();
 343 }
 344
 345
 346 static void initialize()
 347 {
 348     VpdSetPointArray25mW = VPD_VALUES_25MW;
 349     VpdSetPointArray100mW = VPD_VALUES_100MW;
 350     PwmArray25mW = PWM_VALUES_25MW;
 351     PwmArray100mW = PWM_VALUES_100MW;
 352
 353     if (GPIO_PIN_SPI_VTX_NSS != UNDEF_PIN)
 354     {
 355         if (GPIO_PIN_SPI_VTX_SCK != UNDEF_PIN && GPIO_PIN_SPI_VTX_SCK != GPIO_PIN_SCK)
 356         {
 357             vtxSPI = new SPIClass(HSPI);
 358             vtxSPI->begin(GPIO_PIN_SPI_VTX_SCK, GPIO_PIN_SPI_VTX_MISO, GPIO_PIN_SPI_VTX_MOSI, GPIO_PIN_SPI_VTX_NSS);
 359             vtxSPI->setHwCs(true);
 360             vtxSPI->setBitOrder(LSBFIRST);
 361         }
 362         else
 363         {
 364             vtxSPI = &SPI;
 365             pinMode(GPIO_PIN_SPI_VTX_NSS, OUTPUT);
 366             digitalWrite(GPIO_PIN_SPI_VTX_NSS, HIGH);
 367         }
 368
 369         pinMode(GPIO_PIN_RF_AMP_VREF, OUTPUT);
 370         digitalWrite(GPIO_PIN_RF_AMP_VREF, LOW);
 371
 372         #if defined(PLATFORM_ESP32_S3)
 373             rfAmpPwmChannel = PWM.allocate(GPIO_PIN_RF_AMP_PWM, 10000);
 374         #elif defined(PLATFORM_ESP32)
 375             // If using a DAC pin then adjust min/max and initial value
 376             if (GPIO_PIN_RF_AMP_PWM == 25 || GPIO_PIN_RF_AMP_PWM == 26)
 377             {
 378                 vtxMinPWM = MIN_DAC;
 379                 vtxMaxPWM = MAX_DAC;
 380                 vtxSPIPWM = vtxMaxPWM;
 381             }
 382             else
 383             {
 384                 rfAmpPwmChannel = PWM.allocate(GPIO_PIN_RF_AMP_PWM, 10000);
 385             }
 386         #else
 387             pinMode(GPIO_PIN_RF_AMP_PWM, OUTPUT);
 388             analogWriteFreq(10000); // 10kHz
 389             analogWriteResolution(12); // 0 - 4095
 390         #endif
 391         setPWM();
 392     }
 393 }
 394
 395 static int start()
 396 {
 397     if (GPIO_PIN_SPI_VTX_NSS == UNDEF_PIN)
 398     {
 399         return DURATION_NEVER;
 400     }
 401
 402 #if defined(VTX_OUTPUT_CALIBRATION)
 403     rtc6705SetFrequency(VpdFreqArray[calibFreqIndex]); // Set to the first calib frequency
 404     vtxSPIPitmodeCurrent = 0;
 405     VpdSetPoint = VPD_SETPOINT_0_MW;
 406     rtc6705PowerAmpOn();
 407     return RTC6705_PLL_SETTLE_TIME_MS;
 408 #endif
 409
 410     return RTC6705_BOOT_DELAY;
 411 }
 412
 413 static int timeout()
 414 {
 415     if ((GPIO_PIN_SPI_VTX_NSS == UNDEF_PIN) || stopVtxMonitoring)
 416     {
 417         return DURATION_NEVER;
 418     }
 419
 420     if (hwTimer::running && !hwTimer::isTick)
 421     {
 422         // Dont run spi and analog reads during rx hopping, wifi or updating
 423         return DURATION_IMMEDIATELY;
 424     }
 425
 426 #if defined(VTX_OUTPUT_CALIBRATION)
 427     return gatherOutputCalibrationData();
 428 #endif
 429
 430     if (vtxSPIFrequencyCurrent != vtxSPIFrequency)
 431     {
 432         rtc6705SetFrequency(vtxSPIFrequency);
 433         vtxSPIFrequencyCurrent = vtxSPIFrequency;
 434         vtxPowerAmpEnable = true;
 435
 436         DBGLN("VTX: Set frequency: %d", vtxSPIFrequency);
 437
 438         return RTC6705_PLL_SETTLE_TIME_MS;
 439     }
 440
 441     // Note: it's important that the PA is handled after the frequency.
 442     if (vtxPowerAmpEnableCurrent != vtxPowerAmpEnable)
 443     {
 444         DBGLN("VTX: Changing internal PA, old: %d, new: %d", vtxPowerAmpEnableCurrent, vtxPowerAmpEnable);
 445         if (vtxPowerAmpEnable)
 446         {
 447             rtc6705PowerAmpOn();
 448         }
 449         vtxPowerAmpEnableCurrent = vtxPowerAmpEnable;
 450
 451         return VTX_POWER_INTERVAL_MS;
 452     }
 453
 454     if (vtxSPIPowerIdxCurrent != vtxSPIPowerIdx)
 455     {
 456         DBGLN("VTX: Set power: %d", vtxSPIPowerIdx);
 457         SetVpdSetPoint();
 458         vtxSPIPowerIdxCurrent = vtxSPIPowerIdx;
 459     }
 460
 461     if (vtxSPIPitmodeCurrent != vtxSPIPitmode)
 462     {
 463         DBGLN("VTX: Set PIT mode: %d", vtxSPIPitmode);
 464         vtxSPIPitmodeCurrent = vtxSPIPitmode;
 465     }
 466
 467     checkOutputPower();
 468
 469     return VTX_POWER_INTERVAL_MS;
 470 }
 471
 472 device_t VTxSPI_device = {
 473     .initialize = initialize,
 474     .start = start,
 475     .event = nullptr,
 476     .timeout = timeout
 477 };
 478
 479 #endif