From 758b97b65278e1929bedcdd67d757657f5f2cc11 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Stefa=C5=84ski?= Date: Wed, 24 Jan 2024 19:17:02 +0100 Subject: [PATCH] VTX SPI output calibration routine and initial PWM value config (#2515) * Added VTX initial PWM values * Added a VTX calibration routine --- src/html/hardware.html | 2 + src/include/hardware.h | 2 + src/include/target/Unified_ESP32_TX.h | 4 +- src/include/target/Unified_ESP_RX.h | 2 + src/lib/MSPVTX/devMSPVTX.cpp | 2 +- src/lib/OPTIONS/hardware.cpp | 2 + src/lib/VTXSPI/devVTXSPI.cpp | 80 ++++++++++++++++++++++++++- 7 files changed, 91 insertions(+), 3 deletions(-) diff --git a/src/html/hardware.html b/src/html/hardware.html index 13b475557e..2b9f17538a 100644 --- a/src/html/hardware.html +++ b/src/html/hardware.html @@ -286,6 +286,8 @@

ExpressLRS

SPI MOSI pinMOSI pin on RTC6705 VTx (leave undefined if sharing Radio SPI bus) 25mW VPD interpolation values4 values for 5650, 5750, 5850, 5950 frequencies at 25mW 100mW VPD interpolation values4 values for 5650, 5750, 5850, 5950 frequencies at 100mW + 25mW PWM interpolation values4 values for 5650, 5750, 5850, 5950 frequencies at 25mW + 100mW PWM interpolation values4 values for 5650, 5750, 5850, 5950 frequencies at 100mW I2C SCL pinI2C clock pin used to communicate with I2C devices diff --git a/src/include/hardware.h b/src/include/hardware.h index 150d6f5fc1..0a32121345 100644 --- a/src/include/hardware.h +++ b/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; diff --git a/src/include/target/Unified_ESP32_TX.h b/src/include/target/Unified_ESP32_TX.h index 9d2a6283ad..ee1e1baac0 100644 --- a/src/include/target/Unified_ESP32_TX.h +++ b/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 */ diff --git a/src/include/target/Unified_ESP_RX.h b/src/include/target/Unified_ESP_RX.h index 03819cf0ae..87ea76c660 100644 --- a/src/include/target/Unified_ESP_RX.h +++ b/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 \ No newline at end of file diff --git a/src/lib/MSPVTX/devMSPVTX.cpp b/src/lib/MSPVTX/devMSPVTX.cpp index 5c091fc1cd..7771b880b0 100644 --- a/src/lib/MSPVTX/devMSPVTX.cpp +++ b/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 } diff --git a/src/lib/OPTIONS/hardware.cpp b/src/lib/OPTIONS/hardware.cpp index aafb4a2cae..137b6a7dc3 100644 --- a/src/lib/OPTIONS/hardware.cpp +++ b/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 { diff --git a/src/lib/VTXSPI/devVTXSPI.cpp b/src/lib/VTXSPI/devVTXSPI.cpp index fd7d4c4b51..888ea556e2 100644 --- a/src/lib/VTXSPI/devVTXSPI.cpp +++ b/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);