Merge pull request #4879 from MotoLab/rtc6705_power_fixes

Fix power control for RTC6705 VTXs

src/main/cms/cms_menu_vtx_rtc6705.c

@@ -21,7 +21,7 @@
 
 #include "platform.h"
 
-#if defined(USE_CMS)
+#if defined(USE_CMS) && defined(VTX_RTC6705)
 
 #include "common/printf.h"
 #include "common/utils.h"

src/main/drivers/vtx_common.c

@@ -74,7 +74,7 @@ void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel)
 // index is zero origin, zero = power off completely
 void vtxCommonSetPowerByIndex(uint8_t index)
 {
-    if (vtxDevice && (index <= vtxDevice->capability.powerCount)) {
+    if (vtxDevice && (index < vtxDevice->capability.powerCount)) {
         if (vtxDevice->vTable->setPowerByIndex) {
             vtxDevice->vTable->setPowerByIndex(index);
         }

src/main/drivers/vtx_common.h

@@ -41,6 +41,12 @@
 
 #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999          //max freq (in MHz) for 'vtx_freq' setting
 
+#if defined(VTX_RTC6705)
+
+#include "drivers/vtx_rtc6705.h"
+
+#endif
+
 #if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
 
 #define VTX_SETTINGS_POWER_COUNT        5
@@ -52,8 +58,6 @@
 
 #elif defined(VTX_RTC6705)
 
-#include "io/vtx_rtc6705.h"
-
 #define VTX_SETTINGS_POWER_COUNT    VTX_RTC6705_POWER_COUNT
 #define VTX_SETTINGS_DEFAULT_POWER  VTX_RTC6705_DEFAULT_POWER
 #define VTX_SETTINGS_MIN_POWER      VTX_RTC6705_MIN_POWER

src/main/drivers/vtx_rtc6705.c

@@ -117,7 +117,7 @@ static IO_t vtxCLKPin       = IO_NONE;
 
 
 // Define variables
-static const uint32_t channelArray[RTC6705_BAND_COUNT][RTC6705_CHANNEL_COUNT] = {
+static const uint32_t channelArray[VTX_RTC6705_BAND_COUNT][VTX_RTC6705_CHANNEL_COUNT] = {
     { RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 },
     { RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 },
     { RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 },
@@ -196,8 +196,8 @@ static void rtc6705Transfer(uint32_t command)
  */
 void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    band = constrain(band, 0, RTC6705_BAND_COUNT - 1);
+    band = constrain(band, 0, VTX_RTC6705_BAND_COUNT - 1);
-    channel = constrain(channel, 0, RTC6705_CHANNEL_COUNT - 1);
+    channel = constrain(channel, 0, VTX_RTC6705_CHANNEL_COUNT - 1);
 
     spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
 
@@ -211,7 +211,7 @@ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
  */
 void rtc6705SetFreq(uint16_t frequency)
 {
-    frequency = constrain(frequency, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX);
+    frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX);
 
     const uint32_t val_a = ((((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers)
     const uint32_t val_n = (((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers)
@@ -229,13 +229,13 @@ void rtc6705SetFreq(uint16_t frequency)
 
 void rtc6705SetRFPower(uint8_t rf_power)
 {
-    rf_power = constrain(rf_power, 0, RTC6705_RF_POWER_COUNT - 1);
+    rf_power = constrain(rf_power, VTX_RTC6705_MIN_POWER, VTX_RTC6705_POWER_COUNT - 1);
 
     spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
 
     uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write
     val_hex |= RTC6705_ADDRESS; // address
-    const uint32_t data = rf_power == 0 ? (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)) : PA_CONTROL_DEFAULT;
+    const uint32_t data = rf_power > 1 ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK));
     val_hex |= data << 5; // 4 address bits and 1 rw bit.
 
     rtc6705Transfer(val_hex);

src/main/drivers/vtx_rtc6705.h

@@ -26,14 +26,21 @@
 
 #include <stdint.h>
 
-#define RTC6705_BAND_COUNT          5
+#define VTX_RTC6705_BAND_COUNT      5
-#define RTC6705_CHANNEL_COUNT       8
+#define VTX_RTC6705_CHANNEL_COUNT   8
-#define RTC6705_RF_POWER_COUNT      2
+#define VTX_RTC6705_POWER_COUNT     3
+#define VTX_RTC6705_DEFAULT_POWER   1
 
-#define RTC6705_FREQ_MIN            5600
+#if defined(RTC6705_POWER_PIN)
-#define RTC6705_FREQ_MAX            5950
+#define VTX_RTC6705_MIN_POWER   0
+#else
+#define VTX_RTC6705_MIN_POWER   1
+#endif
 
-#define RTC6705_BOOT_DELAY 350 // milliseconds
+#define VTX_RTC6705_FREQ_MIN    5600
+#define VTX_RTC6705_FREQ_MAX    5950
+
+#define VTX_RTC6705_BOOT_DELAY  350 // milliseconds
 
 void rtc6705IOInit(void);
 void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel);

src/main/drivers/vtx_rtc6705_soft_spi.c

@@ -125,7 +125,7 @@ void rtc6705SetFreq(uint16_t channel_freq)
 
 void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    const uint8_t freqIndex = (band * RTC6705_CHANNEL_COUNT) + channel;
+    const uint8_t freqIndex = (band * VTX_RTC6705_CHANNEL_COUNT) + channel;
 
     const uint16_t freq = vtx_freq[freqIndex];
     rtc6705SetFreq(freq);
@@ -133,7 +133,7 @@ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
 
 void rtc6705SetRFPower(uint8_t rf_power)
 {
-    rtc6705_write_register(7, (rf_power ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK))));
+    rtc6705_write_register(7, (rf_power > 1 ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK))));
 }
 
 void rtc6705Disable(void)

src/main/interface/settings.c

@@ -787,9 +787,9 @@ const clivalue_t valueTable[] = {
 
 // PG_VTX_CONFIG
 #ifdef VTX_COMMON
-    { "vtx_band",                   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_MAX_BAND }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, band) },
+    { "vtx_band",                   VAR_UINT8  | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, band) },
     { "vtx_channel",                VAR_UINT8  | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, channel) },
-    { "vtx_power",                  VAR_UINT8  | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_POWER_COUNT }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, power) },
+    { "vtx_power",                  VAR_UINT8  | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_POWER_COUNT-1 }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, power) },
     { "vtx_low_power_disarm",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, lowPowerDisarm) },
 #ifdef VTX_SETTINGS_FREQCMD
     { "vtx_freq",                   VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_MAX_FREQUENCY_MHZ }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, freq) },

src/main/io/vtx.c

@@ -52,27 +52,17 @@ PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig,
 #define VTX_PARAM_CYCLE_TIME_US 100000 // 10Hz
 
 typedef enum {
-    VTX_PARAM_BANDCHAN = 0,
+    VTX_PARAM_POWER = 0,
-    VTX_PARAM_POWER,
+    VTX_PARAM_BANDCHAN,
     VTX_PARAM_PITMODE,
     VTX_PARAM_CONFIRM,
     VTX_PARAM_COUNT
 } vtxScheduleParams_e;
 
-static uint8_t vtxParamScheduleCount;
-static uint8_t vtxParamSchedule[VTX_PARAM_COUNT];
-
 void vtxInit(void)
 {
-    uint8_t index = 0;
     bool settingsUpdated = false;
 
-    vtxParamSchedule[index++] = VTX_PARAM_BANDCHAN;
-    vtxParamSchedule[index++] = VTX_PARAM_POWER;
-    vtxParamSchedule[index++] = VTX_PARAM_PITMODE;
-    vtxParamSchedule[index++] = VTX_PARAM_CONFIRM;
-    vtxParamScheduleCount = index;
-
     // sync frequency in parameter group when band/channel are specified
     const uint16_t freq = vtx58_Bandchan2Freq(vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
     if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) {
@@ -209,15 +199,17 @@ static bool vtxProcessStateUpdate(void) {
 void vtxProcessSchedule(timeUs_t currentTimeUs)
 {
     static timeUs_t lastCycleTimeUs;
-    static uint8_t scheduleIndex;
+    static uint8_t currentSchedule = 0;
     bool vtxUpdatePending = false;
 
     if (vtxCommonDeviceRegistered()) {
-        const uint8_t currentSchedule = vtxParamSchedule[scheduleIndex];
         const vtxSettingsConfig_t settings = vtxGetSettings();
         // Process VTX changes from the parameter group at 10Hz
         if (currentTimeUs > lastCycleTimeUs + VTX_PARAM_CYCLE_TIME_US) {
             switch (currentSchedule) {
+            case VTX_PARAM_POWER:
+                vtxUpdatePending = vtxProcessPower();
+                break;
             case VTX_PARAM_BANDCHAN:
                 if (settings.band) {
                     vtxUpdatePending = vtxProcessBandAndChannel();
@@ -227,9 +219,6 @@ void vtxProcessSchedule(timeUs_t currentTimeUs)
 #endif
                 }
                 break;
-            case VTX_PARAM_POWER:
-                vtxUpdatePending = vtxProcessPower();
-                break;
             case VTX_PARAM_PITMODE:
                 vtxUpdatePending = vtxProcessPitMode();
                 break;
@@ -240,7 +229,7 @@ void vtxProcessSchedule(timeUs_t currentTimeUs)
                 break;
             }
             lastCycleTimeUs = currentTimeUs;
-            scheduleIndex = (scheduleIndex + 1) % vtxParamScheduleCount;
+            currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
         }
         if (!ARMING_FLAG(ARMED) || vtxUpdatePending) {
             vtxCommonProcess(currentTimeUs);

src/main/io/vtx_rtc6705.c

@@ -52,8 +52,8 @@
 #include "io/vtx_string.h"
 
 #if defined(USE_CMS) || defined(VTX_COMMON)
-const char * const rtc6705PowerNames[VTX_RTC6705_POWER_COUNT] = {
+const char * const rtc6705PowerNames[] = {
-    "---", "25 ", "200",
+    "OFF", "MIN", "MAX"
 };
 #endif
 
@@ -79,7 +79,7 @@ bool vtxRTC6705Init(void)
 
 void vtxRTC6705Configure(void)
 {
-    rtc6705SetRFPower(vtxRTC6705.powerIndex - 1);
+    rtc6705SetRFPower(vtxRTC6705.powerIndex);
     rtc6705SetBandAndChannel(vtxRTC6705.band - 1, vtxRTC6705.channel - 1);
 }
 
@@ -100,7 +100,7 @@ static void vtxRTC6705EnableAndConfigure(void)
 
     rtc6705Enable();
 
-    delay(RTC6705_BOOT_DELAY);
+    delay(VTX_RTC6705_BOOT_DELAY);
 
     vtxRTC6705Configure();
 }
@@ -131,10 +131,10 @@ void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel)
     if (band && channel) {
         if (vtxRTC6705.powerIndex > 0) {
             rtc6705SetBandAndChannel(band - 1, channel - 1);
-        }
 
-        vtxRTC6705.band = band;
+            vtxRTC6705.band = band;
-        vtxRTC6705.channel = channel;
+            vtxRTC6705.channel = channel;
+        }
     }
 }
 

src/main/io/vtx_rtc6705.h

@@ -22,17 +22,7 @@
 
 #include "platform.h"
 
-
+extern const char * const rtc6705PowerNames[];
-#define VTX_RTC6705_POWER_COUNT 3
-#define VTX_RTC6705_DEFAULT_POWER 1
-
-#if defined(RTC6705_POWER_PIN)
-#define VTX_RTC6705_MIN_POWER 0
-#else
-#define VTX_RTC6705_MIN_POWER 1
-#endif
-
-extern const char * const rtc6705PowerNames[VTX_RTC6705_POWER_COUNT];
 
 void vtxRTC6705Configure(void);
 bool vtxRTC6705CanUpdate(void);