Refactored frequency table to common place, fixed lowband frequencies

ExpressLRS · Mar 6, 2023 · 7e73fca · 7e73fca
1 parent 343f456
commit 7e73fca
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 58 deletions.
diff --git a/src/lib/MSPVTX/devMSPVTX.cpp b/src/lib/MSPVTX/devMSPVTX.cpp
@@ -226,7 +226,7 @@ void mspVtxProcessPacket(uint8_t *packet)
             channel = ((vtxConfigPacket->band - 1) * 8) + (vtxConfigPacket->channel - 1);
             if (channel < FREQ_TABLE_SIZE)
             {
-                vtxSPIBandChannelIdx = channel;
+                vtxSPIFrequency = getFreqByIdx(channel);
             }
             break;
         }
@@ -274,7 +274,7 @@ void mspVtxProcessPacket(uint8_t *packet)
                             mspState = (eepromWriteRequired ? SEND_EEPROM_WRITE : MONITORING);
                             vtxSPIPitmode = pitMode;
                             vtxSPIPowerIdx = power;
-                            vtxSPIBandChannelIdx = channel;
+                            vtxSPIFrequency = getFreqByIdx(channel);
                         }
                         break;
                     }

diff --git a/src/lib/MSPVTX/freqTable.cpp b/src/lib/MSPVTX/freqTable.cpp
@@ -0,0 +1,26 @@
+#include "freqTable.h"
+
+uint8_t getFreqTableChannels(void)
+{
+    return CHANNEL_COUNT;
+}
+
+uint8_t getFreqTableBands(void)
+{
+    return FREQ_TABLE_SIZE / getFreqTableChannels();
+}
+
+uint16_t getFreqByIdx(uint8_t idx)
+{
+    return channelFreqTable[idx];
+}
+
+uint8_t channelFreqLabelByIdx(uint8_t idx)
+{
+    return channelFreqLabel[idx];
+}
+
+uint8_t getBandLetterByIdx(uint8_t idx)
+{
+    return bandLetter[idx];
+}
diff --git a/src/lib/MSPVTX/freqTable.h b/src/lib/MSPVTX/freqTable.h
@@ -1,3 +1,5 @@
+#pragma once
+
 #include "common.h"
 #include "targets.h"
 
@@ -22,44 +24,25 @@ const uint8_t channelFreqLabel[FREQ_TABLE_SIZE] = {
 
 const uint8_t bandLetter[6] = {'A', 'B', 'E', 'F', 'R', 'L'};
 
-uint16_t channelFreqTable[FREQ_TABLE_SIZE] = {
+const uint16_t channelFreqTable[FREQ_TABLE_SIZE] = {
     5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, // A
     5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, // B
     5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, // E
     5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, // F
     5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // R
-    5333, 5373, 5413, 5453, 5493, 5533, 5573, 5613  // L
+    5362, 5399, 5436, 5473, 5510, 5547, 5584, 5621  // L
 };
 
-uint8_t powerLevelsLut[NUM_POWER_LEVELS] = {1, RACE_MODE, 14, 20, 26};
+const uint8_t powerLevelsLut[NUM_POWER_LEVELS] = {1, RACE_MODE, 14, 20, 26};
 
-uint8_t powerLevelsLabel[NUM_POWER_LEVELS * POWER_LEVEL_LABEL_LENGTH] = {'0', ' ', ' ',
+const uint8_t powerLevelsLabel[NUM_POWER_LEVELS * POWER_LEVEL_LABEL_LENGTH] = {'0', ' ', ' ',
                                                                          'R', 'C', 'E',
                                                                          '2', '5', ' ',
                                                                          '1', '0', '0',
                                                                          '4', '0', '0'};
 
-uint8_t getFreqTableChannels(void)
-{
-    return CHANNEL_COUNT;
-}
-
-uint8_t getFreqTableBands(void)
-{
-    return FREQ_TABLE_SIZE / getFreqTableChannels();
-}
-
-uint16_t getFreqByIdx(uint8_t idx)
-{
-    return channelFreqTable[idx];
-}
-
-uint8_t channelFreqLabelByIdx(uint8_t idx)
-{
-    return channelFreqLabel[idx];
-}
-
-uint8_t getBandLetterByIdx(uint8_t idx)
-{
-    return bandLetter[idx];
-}
+uint8_t getFreqTableChannels(void);
+uint8_t getFreqTableBands(void);
+uint16_t getFreqByIdx(uint8_t idx);
+uint8_t channelFreqLabelByIdx(uint8_t idx);
+uint8_t getBandLetterByIdx(uint8_t idx);
diff --git a/src/lib/VTXSPI/devVTXSPI.cpp b/src/lib/VTXSPI/devVTXSPI.cpp
@@ -44,8 +44,8 @@
 constexpr uint8_t rfAmpPwmChannel = 0;
 #endif
 
-uint8_t vtxSPIBandChannelIdx = 255;
-static uint8_t vtxSPIBandChannelIdxCurrent = 255;
+uint16_t vtxSPIFrequency = 6000;
+static uint16_t vtxSPIFrequencyCurrent = 6000;
 uint8_t vtxSPIPowerIdx = 0;
 static uint8_t vtxSPIPowerIdxCurrent = 0;
 uint8_t vtxSPIPitmode = 1;
@@ -71,15 +71,6 @@ uint16_t VpdSetPointArray100mW[] = VPD_VALUES_100MW;
 uint16_t VpdFreqArray[] = {5650, 5750, 5850, 5950};
 uint8_t VpdSetPointCount =  ARRAY_SIZE(VpdFreqArray);
 
-static const uint16_t freqTable[48] = {
-    5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, // A
-    5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, // B
-    5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, // E
-    5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, // F
-    5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // R
-    5333, 5373, 5413, 5453, 5493, 5533, 5573, 5613  // L
-};
-
 static SPIClass *vtxSPI;
 
 static void rtc6705WriteRegister(uint32_t regData)
@@ -126,11 +117,6 @@ static void rtc6705SetFrequency(uint32_t freq)
     rtc6705WriteRegister(regData);
 }
 
-static void rtc6705SetFrequencyByIdx(uint8_t idx)
-{
-    rtc6705SetFrequency((uint32_t)freqTable[idx]);
-}
-
 static void rtc6705PowerAmpOn()
 {
     uint32_t regData = PRE_DRIVER_AND_PA_CONTROL_REGISTER | (WRITE_BIT << 4) | (POWER_AMP_ON << 5);
@@ -190,23 +176,22 @@ static void VTxOutputDecrease()
 static uint16_t LinearInterpVpdSetPointArray(const uint16_t VpdSetPointArray[])
 {
     uint16_t newVpd = 0;
-    uint16_t f = freqTable[vtxSPIBandChannelIdxCurrent];
 
-    if (f <= VpdFreqArray[0])
+    if (vtxSPIFrequencyCurrent <= VpdFreqArray[0])
     {
         newVpd = VpdSetPointArray[0];
     }
-    else if (f >= VpdFreqArray[VpdSetPointCount - 1])
+    else if (vtxSPIFrequencyCurrent >= VpdFreqArray[VpdSetPointCount - 1])
     {
         newVpd = VpdSetPointArray[VpdSetPointCount - 1];
     }
     else
     {
         for (uint8_t i = 0; i < (VpdSetPointCount - 1); i++)
         {
-            if (f < VpdFreqArray[i + 1])
+            if (vtxSPIFrequencyCurrent < VpdFreqArray[i + 1])
             {
-                newVpd = VpdSetPointArray[i] + ((VpdSetPointArray[i + 1]-VpdSetPointArray[i])/(VpdFreqArray[i + 1]-VpdFreqArray[i])) * (f - VpdFreqArray[i]);
+                newVpd = VpdSetPointArray[i] + ((VpdSetPointArray[i + 1]-VpdSetPointArray[i])/(VpdFreqArray[i + 1]-VpdFreqArray[i])) * (vtxSPIFrequencyCurrent - VpdFreqArray[i]);
             }
         }
     }
@@ -349,7 +334,7 @@ static int event()
         return DURATION_NEVER;
     }
 
-    if (vtxSPIBandChannelIdxCurrent != vtxSPIBandChannelIdx || vtxSPIPowerIdxCurrent != vtxSPIPowerIdx)
+    if (vtxSPIFrequencyCurrent != vtxSPIFrequency || vtxSPIPowerIdxCurrent != vtxSPIPowerIdx)
     {
         return DURATION_IMMEDIATELY;
     }
@@ -370,12 +355,12 @@ static int timeout()
         return DURATION_IMMEDIATELY;
     }
 
-    if (vtxSPIBandChannelIdxCurrent != vtxSPIBandChannelIdx)
+    if (vtxSPIFrequencyCurrent != vtxSPIFrequency)
     {
-        rtc6705SetFrequencyByIdx(vtxSPIBandChannelIdx);
-        vtxSPIBandChannelIdxCurrent = vtxSPIBandChannelIdx;
+        rtc6705SetFrequency(vtxSPIFrequency);
+        vtxSPIFrequencyCurrent = vtxSPIFrequency;
 
-        DBGLN("Set VTX channel: %d", vtxSPIBandChannelIdx);
+        DBGLN("Set VTX frequency: %d", vtxSPIFrequency);
 
         return RTC6705_PLL_SETTLE_TIME_MS;
     }

diff --git a/src/lib/VTXSPI/devVTXSPI.h b/src/lib/VTXSPI/devVTXSPI.h
@@ -4,7 +4,7 @@
 
 extern device_t VTxSPI_device;
 
-extern uint8_t vtxSPIBandChannelIdx;
+extern uint16_t vtxSPIFrequency;
 extern uint8_t vtxSPIPowerIdx;
 extern uint8_t vtxSPIPitmode;
 

diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp
@@ -13,6 +13,7 @@
 #include "PFD.h"
 #include "options.h"
 #include "MeanAccumulator.h"
+#include "freqTable.h"
 
 #include "devCRSF.h"
 #include "devLED.h"
@@ -1039,7 +1040,7 @@ void MspReceiveComplete()
     {
         if (OPT_HAS_VTX_SPI && MspData[7] == MSP_SET_VTX_CONFIG)
         {
-            vtxSPIBandChannelIdx = MspData[8];
+            vtxSPIFrequency = getFreqByIdx(MspData[8]);
             if (MspData[6] >= 4) // If packet has 4 bytes it also contains power idx and pitmode.
             {
                 vtxSPIPowerIdx = MspData[10];