VTX SPI output calibration routine and initial PWM value config (Expr…

…essLRS#2515)

* Added VTX initial PWM values

* Added a VTX calibration routine

src/html/hardware.html

@@ -286,6 +286,8 @@ <h1><b>ExpressLRS</b></h1>
 					<tr><td></td><td>SPI MOSI pin<img class="icon-output"/></td><td><input size='3' id='vtx_mosi' name='vtx_mosi' type='text'/></td><td>MOSI pin on RTC6705 VTx (leave undefined if sharing Radio SPI bus)</td></tr>
 					<tr><td></td><td>25mW VPD interpolation values</td><td><input size='40' id='vtx_amp_vpd_25mW' name='vtx_amp_vpd_25mW' type='text' class='array'/></td><td>4 values for 5650, 5750, 5850, 5950 frequencies at 25mW</td></tr>
 					<tr><td></td><td>100mW VPD interpolation values</td><td><input size='40' id='vtx_amp_vpd_100mW' name='vtx_amp_vpd_100mW' type='text' class='array'/></td><td>4 values for 5650, 5750, 5850, 5950 frequencies at 100mW</td></tr>
+					<tr><td></td><td>25mW PWM interpolation values</td><td><input size='40' id='vtx_amp_pwm_25mW' name='vtx_amp_pwm_25mW' type='text' class='array'/></td><td>4 values for 5650, 5750, 5850, 5950 frequencies at 25mW</td></tr>
+					<tr><td></td><td>100mW PWM interpolation values</td><td><input size='40' id='vtx_amp_pwm_100mW' name='vtx_amp_pwm_100mW' type='text' class='array'/></td><td>4 values for 5650, 5750, 5850, 5950 frequencies at 100mW</td></tr>
 
 					<tr><td colspan='2'><b>I2C</td></tr>
 					<tr><td></td><td>SCL pin<img class="icon-output"/></td><td><input size='3' id='i2c_scl' name='i2c_scl' type='text'/></td><td>I2C clock pin used to communicate with I2C devices</td></tr>

src/include/hardware.h

@@ -137,6 +137,8 @@ typedef enum {
     HARDWARE_vtx_sck,
     HARDWARE_vtx_amp_vpd_25mW,
     HARDWARE_vtx_amp_vpd_100mW,
+    HARDWARE_vtx_amp_pwm_25mW,
+    HARDWARE_vtx_amp_pwm_100mW,
 
     HARDWARE_LAST
 } nameType;

src/include/target/Unified_ESP32_TX.h

@@ -202,6 +202,8 @@ GPIO_PIN_RF_AMP_PWM
 GPIO_PIN_RF_AMP_VPD
 GPIO_PIN_RF_AMP_VREF
 GPIO_PIN_SPI_VTX_NSS
-VPD_VALUES_100MW
 VPD_VALUES_25MW
+VPD_VALUES_100MW
+PWM_VALUES_25MW
+PWM_VALUES_100MW
 */

src/include/target/Unified_ESP_RX.h

@@ -141,4 +141,6 @@
 #define GPIO_PIN_SPI_VTX_SCK hardware_pin(HARDWARE_vtx_sck)
 #define VPD_VALUES_25MW hardware_u16_array(HARDWARE_vtx_amp_vpd_25mW)
 #define VPD_VALUES_100MW hardware_u16_array(HARDWARE_vtx_amp_vpd_100mW)
+#define PWM_VALUES_25MW hardware_u16_array(HARDWARE_vtx_amp_pwm_25mW)
+#define PWM_VALUES_100MW hardware_u16_array(HARDWARE_vtx_amp_pwm_100mW)
 #endif

src/lib/MSPVTX/devMSPVTX.cpp

@@ -219,7 +219,7 @@ void mspVtxProcessPacket(uint8_t *packet)
                 power = 1;
             }
 
-            if (power >= NUM_POWER_LEVELS)
+            if (power > NUM_POWER_LEVELS)
             {
                 power = 3; // 25 mW
             }

src/lib/OPTIONS/hardware.cpp

@@ -129,6 +129,8 @@ static const struct {
     {HARDWARE_vtx_sck, "vtx_sck", INT},
     {HARDWARE_vtx_amp_vpd_25mW, "vtx_amp_vpd_25mW", ARRAY},
     {HARDWARE_vtx_amp_vpd_100mW, "vtx_amp_vpd_100mW", ARRAY},
+    {HARDWARE_vtx_amp_pwm_25mW, "vtx_amp_pwm_25mW", ARRAY},
+    {HARDWARE_vtx_amp_pwm_100mW, "vtx_amp_pwm_100mW", ARRAY},
 };
 
 typedef union {

src/lib/VTXSPI/devVTXSPI.cpp

@@ -65,9 +65,13 @@ static bool stopVtxMonitoring = false;
 #if defined(TARGET_UNIFIED_RX)
 const uint16_t *VpdSetPointArray25mW = nullptr;
 const uint16_t *VpdSetPointArray100mW = nullptr;
+const uint16_t *PwmArray25mW = nullptr;
+const uint16_t *PwmArray100mW = nullptr;
 #else
 uint16_t VpdSetPointArray25mW[] = VPD_VALUES_25MW;
 uint16_t VpdSetPointArray100mW[] = VPD_VALUES_100MW;
+uint16_t PwmArray25mW[] = PWM_VALUES_25MW;
+uint16_t PwmArray100mW[] = PWM_VALUES_100MW;
 #endif
 
 uint16_t VpdFreqArray[] = {5650, 5750, 5850, 5950};
@@ -203,29 +207,60 @@ static uint16_t LinearInterpVpdSetPointArray(const uint16_t VpdSetPointArray[])
     return newVpd;
 }
 
+static uint16_t LinearInterpSetPwm(const uint16_t PwmArray[])
+{
+    uint16_t newPwm = 0;
+
+    if (vtxSPIFrequencyCurrent <= VpdFreqArray[0])
+    {
+        newPwm = PwmArray[0];
+    }
+    else if (vtxSPIFrequencyCurrent >= VpdFreqArray[VpdSetPointCount - 1])
+    {
+        newPwm = PwmArray[VpdSetPointCount - 1];
+    }
+    else
+    {
+        for (uint8_t i = 0; i < (VpdSetPointCount - 1); i++)
+        {
+            if (vtxSPIFrequencyCurrent < VpdFreqArray[i + 1])
+            {
+                newPwm = PwmArray[i] + ((PwmArray[i + 1]-PwmArray[i])/(VpdFreqArray[i + 1]-VpdFreqArray[i])) * (vtxSPIFrequencyCurrent - VpdFreqArray[i]);
+            }
+        }
+    }
+
+    return newPwm;
+}
+
 static void SetVpdSetPoint()
 {
     switch (vtxSPIPowerIdx)
     {
     case 1: // 0 mW
         VpdSetPoint = VPD_SETPOINT_0_MW;
+        vtxSPIPWM = vtxMaxPWM;
         break;
 
     case 2: // RCE
     case 3: // 25 mW
         VpdSetPoint = LinearInterpVpdSetPointArray(VpdSetPointArray25mW);
+        vtxSPIPWM = LinearInterpSetPwm(PwmArray25mW);
         break;
 
     case 4: // 100 mW
         VpdSetPoint = LinearInterpVpdSetPointArray(VpdSetPointArray100mW);
+        vtxSPIPWM = LinearInterpSetPwm(PwmArray100mW);
         break;
 
     default: // YOLO mW
         VpdSetPoint = VPD_SETPOINT_YOLO_MW;
+        vtxSPIPWM = vtxMinPWM;
         break;
     }
 
-    DBGLN("Setting new VPD setpoint: %d", VpdSetPoint);
+    setPWM();
+    DBGLN("Setting new VPD setpoint: %d, initial PWM: %d", VpdSetPoint, vtxSPIPWM);
 }
 
 static void checkOutputPower()
@@ -255,6 +290,37 @@ static void checkOutputPower()
     }
 }
 
+#if defined(VTX_OUTPUT_CALIBRATION)
+int sampleCount = 0;
+int calibFreqIndex = 0;
+#define CALIB_SAMPLES 10
+
+static int gatherOutputCalibrationData()
+{
+    if (VpdSetPoint <= VPD_SETPOINT_YOLO_MW && calibFreqIndex < VpdSetPointCount)
+    {
+        sampleCount++;
+        checkOutputPower();
+        DBGLN("VTX Freq=%d, VPD setpoint=%d, VPD=%d, PWM=%d, sample=%d", VpdFreqArray[calibFreqIndex], VpdSetPoint, Vpd, vtxSPIPWM, sampleCount);
+        if (sampleCount >= CALIB_SAMPLES)
+        {
+            VpdSetPoint += VPD_BUFFER;
+            sampleCount = 0;
+        }
+
+        if (VpdSetPoint > VPD_SETPOINT_YOLO_MW)
+        {
+            calibFreqIndex++;
+            rtc6705SetFrequency(VpdFreqArray[calibFreqIndex]);
+            VpdSetPoint = VPD_BUFFER;
+            return RTC6705_PLL_SETTLE_TIME_MS;
+        }
+        return VTX_POWER_INTERVAL_MS;
+    }
+    return DURATION_NEVER;
+}
+#endif
+
 void disableVTxSpi()
 {
     stopVtxMonitoring = true;
@@ -320,6 +386,14 @@ static int start()
         return DURATION_NEVER;
     }
 
+#if defined(VTX_OUTPUT_CALIBRATION)
+    rtc6705SetFrequency(VpdFreqArray[calibFreqIndex]); // Set to the first calib frequency
+    vtxSPIPitmodeCurrent = 0;
+    VpdSetPoint = VPD_SETPOINT_0_MW;
+    rtc6705PowerAmpOn();
+    return RTC6705_PLL_SETTLE_TIME_MS;
+#endif
+
     rtc6705SetFrequency(5999); // Boot with VTx set away from standard frequencies.
 
     rtc6705PowerAmpOn();
@@ -355,6 +429,10 @@ static int timeout()
         return DURATION_IMMEDIATELY;
     }
 
+#if defined(VTX_OUTPUT_CALIBRATION)
+    return gatherOutputCalibrationData();
+#endif
+
     if (vtxSPIFrequencyCurrent != vtxSPIFrequency)
     {
         rtc6705SetFrequency(vtxSPIFrequency);