Implemented 'vtx_' settings and SetFreqByMHzMsp support

Implemented 'vtx_' settings for SmartAudio and Tramp

Added 'vtx_freq' setting for SmartAudio and Tramp

SmartAudio-CMS fixes and improvements

Added SetFreqByMHzMsp support for SmartAudio and Tramp

make/source.mk

@@ -168,6 +168,7 @@ FC_SRC = \
             telemetry/ibus_shared.c \
             sensors/esc_sensor.c \
             io/vtx_string.c \
+            io/vtx_settings_config.c \
             io/vtx_rtc6705.c \
             io/vtx_smartaudio.c \
             io/vtx_tramp.c \
@@ -266,6 +267,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             cms/cms_menu_misc.c \
             cms/cms_menu_osd.c \
             io/vtx_string.c \
+            io/vtx_settings_config.c \
             io/vtx_rtc6705.c \
             io/vtx_smartaudio.c \
             io/vtx_tramp.c \

src/main/cms/cms_menu_vtx_rtc6705.c

@@ -31,6 +31,7 @@
 
 #include "io/vtx_string.h"
 #include "io/vtx_rtc6705.h"
+#include "io/vtx_settings_config.h"
 
 
 static uint8_t cmsx_vtxBand;
@@ -46,21 +47,19 @@ static const char * const rtc6705BandNames[] = {
 };
 
 static OSD_TAB_t entryVtxBand =         {&cmsx_vtxBand, ARRAYLEN(rtc6705BandNames) - 1, &rtc6705BandNames[0]};
-static OSD_UINT8_t entryVtxChannel =    {&cmsx_vtxChannel, 1, 8, 1};
+static OSD_UINT8_t entryVtxChannel =    {&cmsx_vtxChannel, 1, VTX_RTC6705_CHANNEL_COUNT, 1};
-static OSD_TAB_t entryVtxPower =        {&cmsx_vtxPower, RTC6705_POWER_COUNT - 1, &rtc6705PowerNames[0]};
+static OSD_TAB_t entryVtxPower =        {&cmsx_vtxPower, VTX_RTC6705_POWER_COUNT - 1, &rtc6705PowerNames[0]};
 
 static void cmsx_Vtx_ConfigRead(void)
 {
-    cmsx_vtxBand = vtxRTC6705Config()->band - 1;
+    cmsx_vtxBand = vtxSettingsConfig()->band - 1;
-    cmsx_vtxChannel = vtxRTC6705Config()->channel;
+    cmsx_vtxChannel = vtxSettingsConfig()->channel;
-    cmsx_vtxPower = vtxRTC6705Config()->power;
+    cmsx_vtxPower = vtxSettingsConfig()->power;
 }
 
 static void cmsx_Vtx_ConfigWriteback(void)
 {
-    vtxRTC6705ConfigMutable()->band = cmsx_vtxBand + 1;
+    vtxSettingsSaveBandChanAndPower(cmsx_vtxBand + 1, cmsx_vtxChannel, cmsx_vtxPower);
-    vtxRTC6705ConfigMutable()->channel = cmsx_vtxChannel;
-    vtxRTC6705ConfigMutable()->power = cmsx_vtxPower;
 }
 
 static long cmsx_Vtx_onEnter(void)

src/main/cms/cms_menu_vtx_smartaudio.c

@@ -32,6 +32,7 @@
 
 #include "io/vtx_string.h"
 #include "io/vtx_smartaudio.h"
+#include "io/vtx_settings_config.h"
 
 // Interface to CMS
 
@@ -62,7 +63,9 @@ uint16_t saCmsDeviceFreq = 0;
 uint8_t  saCmsDeviceStatus = 0;
 uint8_t  saCmsPower;
 uint8_t  saCmsPitFMode;          // Undef(0), In-Range(1) or Out-Range(2)
+
 uint8_t  saCmsFselMode;          // Channel(0) or User defined(1)
+uint8_t  saCmsFselModeNew;       // Channel(0) or User defined(1)
 
 uint16_t saCmsORFreq = 0;       // POR frequency
 uint16_t saCmsORFreqNew;        // POR frequency
@@ -99,6 +102,13 @@ void saCmsUpdate(void)
         } else {
             saCmsPower = saDacToPowerIndex(saDevice.power) + 1;
         }
+
+        // if user-freq mode then track possible change
+        if (saCmsFselMode != 0 && saDevice.freq != 0) {
+            saCmsUserFreq = saDevice.freq;
+        }
+
+        saCmsFselModeNew = saCmsFselMode;   //init mode for menu
     }
 
     saUpdateStatusString();
@@ -169,6 +179,12 @@ if (saDevice.version == 2) {
     }
 }
 
+void saCmsResetOpmodel()
+{
+    // trigger data refresh in 'saCmsUpdate()'
+    saCmsOpmodel = SACMS_OPMODEL_UNDEF;
+}
+
 static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self)
 {
     UNUSED(pDisp);
@@ -299,7 +315,7 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
         saCmsConfigPitFModeByGvar(pDisp, self);
 
         // Direct frequency mode is not available in RACE opmodel
-        saCmsFselMode = 0;
+        saCmsFselModeNew = 0;
         saCmsConfigFreqModeByGvar(pDisp, self);
     } else {
         // Trying to go back to unknown state; bounce back
@@ -340,9 +356,9 @@ static CMS_Menu saCmsMenuStats = {
     .entries = saCmsMenuStatsEntries
 };
 
-static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames };
+static OSD_TAB_t saCmsEntBand = { &saCmsBand, VTX_SMARTAUDIO_BAND_COUNT, vtx58BandNames };
 
-static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChannelNames };
+static OSD_TAB_t saCmsEntChan = { &saCmsChan, VTX_SMARTAUDIO_CHANNEL_COUNT, vtx58ChannelNames };
 
 static const char * const saCmsPowerNames[] = {
     "---",
@@ -352,7 +368,7 @@ static const char * const saCmsPowerNames[] = {
     "800",
 };
 
-static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames};
+static OSD_TAB_t saCmsEntPower = { &saCmsPower, VTX_SMARTAUDIO_POWER_COUNT, saCmsPowerNames};
 
 static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 };
 
@@ -382,19 +398,15 @@ static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
-    if (saCmsFselMode == 0) {
+    // if trying to do user frequency mode in RACE opmodel then
-        // CHAN
+    // revert because user-freq only available in FREE opmodel
-        saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
+    if (saCmsFselModeNew != 0 && saCmsOpmodel != SACMS_OPMODEL_FREE) {
-    } else {
+        saCmsFselModeNew = 0;
-        // USER: User frequency mode is only available in FREE opmodel.
-        if (saCmsOpmodel == SACMS_OPMODEL_FREE) {
-            saSetFreq(saCmsUserFreq);
-        } else {
-            // Bounce back
-            saCmsFselMode = 0;
-        }
     }
 
+    // don't call 'saSetBandAndChannel()' / 'saSetFreq()' here,
+    // wait until SET / 'saCmsCommence()' is activated
+
     sacms_SetupTopMenu();
 
     return 0;
@@ -420,10 +432,12 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self)
     } else {
         // Freestyle model
         // Setup band and freq / user freq
-        if (saCmsFselMode == 0)
+        if (saCmsFselModeNew == 0) {
             saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
-        else
+        } else {
+            saSetMode(0);    //make sure FREE mode is setup
             saSetFreq(saCmsUserFreq);
+        }
     }
 
     saSetPowerByIndex(saCmsPower - 1);
@@ -446,7 +460,7 @@ static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
-    saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT);
+    saSetPitFreq(saCmsORFreqNew);
 
     return 0;
 }
@@ -529,7 +543,7 @@ static CMS_Menu saCmsMenuUserFreq =
     .entries = saCmsMenuUserFreqEntries,
 };
 
-static OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames };
+static OSD_TAB_t saCmsEntFselMode = { &saCmsFselModeNew, 1, saCmsFselModeNames };
 
 static OSD_Entry saCmsMenuConfigEntries[] = {
     { "- SA CONFIG -", OME_Label, NULL, NULL, 0 },
@@ -616,7 +630,7 @@ CMS_Menu cmsx_menuVtxSmartAudio; // Forward
 static long sacms_SetupTopMenu(void)
 {
     if (saCmsDeviceStatus) {
-        if (saCmsFselMode == 0)
+        if (saCmsFselModeNew == 0)
             cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
         else
             cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries;

src/main/cms/cms_menu_vtx_smartaudio.h

@@ -22,4 +22,6 @@
 
 extern CMS_Menu cmsx_menuVtxSmartAudio;
 
+void saCmsUpdate(void);
 void saUpdateStatusString(void);
+void saCmsResetOpmodel();

src/main/cms/cms_menu_vtx_tramp.c

@@ -31,6 +31,7 @@
 
 #include "io/vtx_string.h"
 #include "io/vtx_tramp.h"
+#include "io/vtx_settings_config.h"
 
 
 char trampCmsStatusString[31] = "- -- ---- ----";
@@ -62,15 +63,15 @@ uint8_t trampCmsBand = 1;
 uint8_t trampCmsChan = 1;
 uint16_t trampCmsFreqRef;
 
-static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames };
+static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, VTX_TRAMP_BAND_COUNT, vtx58BandNames };
 
-static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames };
+static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, VTX_TRAMP_CHANNEL_COUNT, vtx58ChannelNames };
 
 static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
 
 static uint8_t trampCmsPower = 1;
 
-static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames };
+static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, sizeof(trampPowerTable), trampPowerNames };
 
 static void trampCmsUpdateFreqRef(void)
 {

src/main/config/parameter_group_ids.h

@@ -85,7 +85,7 @@
 #define PG_CURRENT_SENSOR_ADC_CONFIG 256
 #define PG_CURRENT_SENSOR_VIRTUAL_CONFIG 257
 #define PG_VOLTAGE_SENSOR_ADC_CONFIG 258
-#define PG_VTX_RTC6705_CONFIG 259
+#define PG_VTX_SETTINGS_CONFIG 259
 
 
 // betaflight specific parameter group ids start at 500

src/main/drivers/vtx_common.c

@@ -100,6 +100,16 @@ void vtxCommonSetPitMode(uint8_t onoff)
         vtxDevice->vTable->setPitMode(onoff);
 }
 
+void vtxCommonSetFrequency(uint16_t freq)
+{
+    if (!vtxDevice) {
+        return;
+    }
+    if (vtxDevice->vTable->setFrequency) {
+        vtxDevice->vTable->setFrequency(freq);
+    }
+}
+
 bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
 {
     if (!vtxDevice)
@@ -133,6 +143,18 @@ bool vtxCommonGetPitMode(uint8_t *pOnOff)
         return false;
 }
 
+bool vtxCommonGetFrequency(uint16_t *pFreq)
+{
+    if (!vtxDevice) {
+        return false;
+    }
+    if (vtxDevice->vTable->getFrequency) {
+        return vtxDevice->vTable->getFrequency(pFreq);
+    } else {
+        return false;
+    }
+}
+
 bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability)
 {
     if (!vtxDevice)

src/main/drivers/vtx_common.h

@@ -19,6 +19,10 @@
 
 #pragma once
 
+// check value for MSP_SET_VTX_CONFIG to determine if value is encoded
+// band/channel or frequency in MHz (3 bits for band and 3 bits for channel)
+#define VTXCOMMON_MSP_BANDCHAN_CHKVAL ((uint16_t)((7 << 3) + 7))
+
 typedef enum {
     VTXDEV_UNSUPPORTED = 0, // reserved for MSP
     VTXDEV_RTC6705    = 1,
@@ -65,10 +69,12 @@ typedef struct vtxVTable_s {
     void (*setBandAndChannel)(uint8_t band, uint8_t channel);
     void (*setPowerByIndex)(uint8_t level);
     void (*setPitMode)(uint8_t onoff);
+    void (*setFrequency)(uint16_t freq);
 
     bool (*getBandAndChannel)(uint8_t *pBand, uint8_t *pChannel);
     bool (*getPowerIndex)(uint8_t *pIndex);
     bool (*getPitMode)(uint8_t *pOnOff);
+    bool (*getFrequency)(uint16_t *pFreq);
 } vtxVTable_t;
 
 // 3.1.0
@@ -86,7 +92,9 @@ uint8_t vtxCommonGetDeviceType(void);
 void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel);
 void vtxCommonSetPowerByIndex(uint8_t level);
 void vtxCommonSetPitMode(uint8_t onoff);
+void vtxCommonSetFrequency(uint16_t freq);
 bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel);
 bool vtxCommonGetPowerIndex(uint8_t *pIndex);
 bool vtxCommonGetPitMode(uint8_t *pOnOff);
+bool vtxCommonGetFrequency(uint16_t *pFreq);
 bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability);

src/main/fc/cli.c

@@ -111,8 +111,8 @@ extern uint8_t __config_end;
 #include "io/osd.h"
 #include "io/serial.h"
 #include "io/transponder_ir.h"
-#include "io/vtx_rtc6705.h"
 #include "io/vtx_control.h"
+#include "io/vtx_settings_config.h"
 
 #include "msp/msp_protocol.h"
 
@@ -1866,12 +1866,6 @@ static void printVtx(uint8_t dumpMask, const vtxConfig_t *vtxConfig, const vtxCo
     }
 }
 
-// FIXME remove these and use the VTX API
-#define VTX_BAND_MIN                            1
-#define VTX_BAND_MAX                            5
-#define VTX_CHANNEL_MIN                         1
-#define VTX_CHANNEL_MAX                         8
-
 static void cliVtx(char *cmdline)
 {
     int i, val = 0;
@@ -1897,7 +1891,7 @@ static void cliVtx(char *cmdline)
             if (ptr) {
                 val = atoi(ptr);
                 // FIXME Use VTX API to get min/max
-                if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) {
+                if (val >= VTX_SETTINGS_MIN_BAND && val <= VTX_SETTINGS_MAX_BAND) {
                     cac->band = val;
                     validArgumentCount++;
                 }
@@ -1906,7 +1900,7 @@ static void cliVtx(char *cmdline)
             if (ptr) {
                 val = atoi(ptr);
                 // FIXME Use VTX API to get min/max
-                if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) {
+                if (val >= VTX_SETTINGS_MIN_CHANNEL && val <= VTX_SETTINGS_MAX_CHANNEL) {
                     cac->channel = val;
                     validArgumentCount++;
                 }

src/main/fc/fc_msp.c

@@ -1207,14 +1207,17 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
                 uint8_t pitmode=0;
                 vtxCommonGetPitMode(&pitmode);
 
+                uint16_t freq = 0;
+                vtxCommonGetFrequency(&freq);
+
                 sbufWriteU8(dst, deviceType);
                 sbufWriteU8(dst, band);
                 sbufWriteU8(dst, channel);
                 sbufWriteU8(dst, powerIdx);
                 sbufWriteU8(dst, pitmode);
+                sbufWriteU16(dst, freq);
                 // future extensions here...
-            }
+            } else {
-            else {
                 sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
             }
         }
@@ -1668,29 +1671,42 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
     case MSP_SET_VTX_CONFIG:
         {
             const uint16_t tmp = sbufReadU16(src);
-            const uint8_t band    = (tmp / 8) + 1;
-            const uint8_t channel = (tmp % 8) + 1;
 
             if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
-                uint8_t current_band=0, current_channel=0;
-                vtxCommonGetBandAndChannel(&current_band,&current_channel);
-                if ((current_band != band) || (current_channel != channel))
-                    vtxCommonSetBandAndChannel(band,channel);
 
-                if (sbufBytesRemaining(src) < 2)
+                if (tmp <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
+                    const uint8_t band    = (tmp / 8) + 1;
+                    const uint8_t channel = (tmp % 8) + 1;
+                    uint8_t current_band = 0, current_channel = 0;
+                    vtxCommonGetBandAndChannel(&current_band, &current_channel);
+                    if ((current_band != band) || (current_channel != channel)) {
+                        vtxCommonSetBandAndChannel(band, channel);
+                    }
+                } else {  //value is frequency in MHz
+                    uint16_t currentFreq;
+                    vtxCommonGetFrequency(&currentFreq);
+                    if (currentFreq != tmp) {
+                        vtxCommonSetFrequency(tmp);
+                    }
+                }
+
+                if (sbufBytesRemaining(src) < 2) {
                     break;
+                }
 
                 uint8_t power = sbufReadU8(src);
                 uint8_t current_power = 0;
                 vtxCommonGetPowerIndex(&current_power);
-                if (current_power != power)
+                if (current_power != power) {
                     vtxCommonSetPowerByIndex(power);
+                }
 
                 uint8_t pitmode = sbufReadU8(src);
                 uint8_t current_pitmode = 0;
                 vtxCommonGetPitMode(&current_pitmode);
-                if (current_pitmode != pitmode)
+                if (current_pitmode != pitmode) {
                     vtxCommonSetPitMode(pitmode);
+                }
             }
         }
         break;

src/main/fc/settings.c

@@ -65,6 +65,7 @@
 #include "io/osd.h"
 #include "io/vtx_control.h"
 #include "io/vtx_rtc6705.h"
+#include "io/vtx_settings_config.h"
 
 #include "rx/rx.h"
 #include "rx/spektrum.h"
@@ -741,11 +742,14 @@ const clivalue_t valueTable[] = {
 #endif
     { "pwr_on_arm_grace",           VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, powerOnArmingGraceTime) },
 
-// PG_VTX_RTC6705_CONFIG
+// PG_VTX_CONFIG
-#ifdef VTX_RTC6705
+#ifdef VTX_SETTINGS_CONFIG
-    { "vtx_band",                   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, band) },
+    { "vtx_band",                   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_MAX_BAND }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, band) },
-    { "vtx_channel",                VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, channel) },
+    { "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 = { 0, RTC6705_POWER_COUNT - 1 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, power) },
+    { "vtx_power",                  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_POWER_COUNT }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, power) },
+#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) },
+#endif
 #endif
 
 // PG_VTX_CONFIG

src/main/io/vtx_rtc6705.c

@@ -44,27 +44,20 @@
 #include "fc/runtime_config.h"
 
 #include "io/vtx_rtc6705.h"
+#include "io/vtx_settings_config.h"
 #include "io/vtx_string.h"
 
 bool canUpdateVTX(void);
 
-PG_REGISTER_WITH_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config, PG_VTX_RTC6705_CONFIG, 0);
-
-PG_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config,
-    .band = 4,    //Fatshark/Airwaves
-    .channel = 1, //CH1
-    .power = VTX_RTC6705_DEFAULT_POWER
-);
-
 #define WAIT_FOR_VTX while (!canUpdateVTX()) {}
 
 #if defined(CMS) || defined(VTX_COMMON)
 #ifdef RTC6705_POWER_PIN
-const char * const rtc6705PowerNames[RTC6705_POWER_COUNT] = {
+const char * const rtc6705PowerNames[VTX_RTC6705_POWER_COUNT] = {
     "---", "25 ", "200",
 };
 #else
-const char * const rtc6705PowerNames[RTC6705_POWER_COUNT] = {
+const char * const rtc6705PowerNames[VTX_RTC6705_POWER_COUNT] = {
     "25 ", "200",
 };
 #endif
@@ -74,9 +67,9 @@ const char * const rtc6705PowerNames[RTC6705_POWER_COUNT] = {
 static vtxVTable_t rtc6705VTable;    // Forward
 static vtxDevice_t vtxRTC6705 = {
     .vTable = &rtc6705VTable,
-    .capability.bandCount = 5,
+    .capability.bandCount = VTX_RTC6705_BAND_COUNT,
-    .capability.channelCount = 8,
+    .capability.channelCount = VTX_RTC6705_CHANNEL_COUNT,
-    .capability.powerCount = RTC6705_POWER_COUNT,
+    .capability.powerCount = VTX_RTC6705_POWER_COUNT,
     .bandNames = (char **)vtx58BandNames,
     .channelNames = (char **)vtx58ChannelNames,
     .powerNames = (char **)rtc6705PowerNames,
@@ -115,9 +108,9 @@ void vtxRTC6705Process(uint32_t now)
 
     static bool configured = false;
     if (!configured) {
-        vtxRTC6705.band = vtxRTC6705Config()->band;
+        vtxRTC6705.band = vtxSettingsConfig()->band;
-        vtxRTC6705.channel = vtxRTC6705Config()->channel;
+        vtxRTC6705.channel = vtxSettingsConfig()->channel;
-        vtxRTC6705.powerIndex = vtxRTC6705Config()->power;
+        vtxRTC6705.powerIndex = vtxSettingsConfig()->power;
 
 #ifdef RTC6705_POWER_PIN
         if (vtxRTC6705.powerIndex > 0) {
@@ -158,8 +151,7 @@ void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel)
         vtxRTC6705.band = band;
         vtxRTC6705.channel = channel;
 
-        vtxRTC6705ConfigMutable()->band = band;
+        vtxSettingsSaveBandAndChannel(band, channel);
-        vtxRTC6705ConfigMutable()->channel = channel;
     }
 }
 
@@ -167,7 +159,7 @@ void vtxRTC6705SetPowerByIndex(uint8_t index)
 {
     WAIT_FOR_VTX;
 
-    vtxRTC6705ConfigMutable()->power = index;
+    vtxSettingsSavePowerByIndex(index);
 
 #ifdef RTC6705_POWER_PIN
     if (index == 0) {
@@ -205,6 +197,12 @@ void vtxRTC6705SetPitMode(uint8_t onoff)
     return;
 }
 
+void vtxRTC6705SetFreq(uint16_t freq)
+{
+    UNUSED(freq);
+    return;
+}
+
 bool vtxRTC6705GetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
 {
     *pBand = vtxRTC6705.band;
@@ -224,6 +222,12 @@ bool vtxRTC6705GetPitMode(uint8_t *pOnOff)
     return false;
 }
 
+bool vtxRTC6705GetFreq(uint16_t *pFreq)
+{
+    UNUSED(pFreq);
+    return false;
+}
+
 static vtxVTable_t rtc6705VTable = {
     .process = vtxRTC6705Process,
     .getDeviceType = vtxRTC6705GetDeviceType,
@@ -231,9 +235,11 @@ static vtxVTable_t rtc6705VTable = {
     .setBandAndChannel = vtxRTC6705SetBandAndChannel,
     .setPowerByIndex = vtxRTC6705SetPowerByIndex,
     .setPitMode = vtxRTC6705SetPitMode,
+    .setFrequency = vtxRTC6705SetFreq,
     .getBandAndChannel = vtxRTC6705GetBandAndChannel,
     .getPowerIndex = vtxRTC6705GetPowerIndex,
     .getPitMode = vtxRTC6705GetPitMode,
+    .getFrequency = vtxRTC6705GetFreq,
 };
 #endif // VTX_COMMON
 

src/main/io/vtx_rtc6705.h

@@ -22,25 +22,23 @@
 
 #include "platform.h"
 
-#include "config/parameter_group.h"
+#define VTX_RTC6705_MIN_BAND 1
+#define VTX_RTC6705_MAX_BAND 5
+#define VTX_RTC6705_MIN_CHANNEL 1
+#define VTX_RTC6705_MAX_CHANNEL 8
 
-typedef struct vtxRTC6705Config_s {
+#define VTX_RTC6705_BAND_COUNT (VTX_RTC6705_MAX_BAND - VTX_RTC6705_MIN_BAND + 1)
-    uint8_t band;       // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband
+#define VTX_RTC6705_CHANNEL_COUNT (VTX_RTC6705_MAX_CHANNEL - VTX_RTC6705_MIN_CHANNEL + 1)
-    uint8_t channel;    // 1-8
-    uint8_t power;      // 0 = lowest
-} vtxRTC6705Config_t;
-
-PG_DECLARE(vtxRTC6705Config_t, vtxRTC6705Config);
 
 #ifdef RTC6705_POWER_PIN
-#define RTC6705_POWER_COUNT 3
+#define VTX_RTC6705_POWER_COUNT 3
 #define VTX_RTC6705_DEFAULT_POWER 1
 #else
-#define RTC6705_POWER_COUNT 2
+#define VTX_RTC6705_POWER_COUNT 2
 #define VTX_RTC6705_DEFAULT_POWER 0
 #endif
 
-extern const char * const rtc6705PowerNames[RTC6705_POWER_COUNT];
+extern const char * const rtc6705PowerNames[VTX_RTC6705_POWER_COUNT];
 
 void vtxRTC6705Configure(void);
 bool vtxRTC6705Init(void);

src/main/io/vtx_settings_config.c

@@ -0,0 +1,148 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+#include "fc/config.h"
+
+#include "io/vtx_settings_config.h"
+#include "io/vtx_string.h"
+
+#ifdef VTX_SETTINGS_CONFIG
+
+PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0);
+
+PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig,
+    .band = VTX_SETTINGS_DEFAULT_BAND,
+    .channel = VTX_SETTINGS_DEFAULT_CHANNEL,
+    .power = VTX_SETTINGS_DEFAULT_POWER,
+    .freq = 0
+);
+
+//Saves the given band/channel values to configuration settings.
+// band:  Band value (1 to 5).
+// channel:  Channel value (1 to 8).
+void vtxSettingsSaveBandAndChannel(uint8_t band, uint8_t channel)
+{
+    bool modFlag = false;
+    if (band != vtxSettingsConfigMutable()->band) {
+        vtxSettingsConfigMutable()->band = band;
+        modFlag = true;
+    }
+    if (channel != vtxSettingsConfigMutable()->channel) {
+        vtxSettingsConfigMutable()->channel = channel;
+        modFlag = true;
+    }
+    if (band != 0) {    //enter freq value to band/channel
+        uint16_t freq;
+        if ((freq = vtx58_Bandchan2Freq(band, channel)) != 0 &&
+                        freq != vtxSettingsConfigMutable()->freq) {
+            vtxSettingsConfigMutable()->freq = freq;
+            modFlag = true;
+        }
+    }
+    if (modFlag) {
+        // need to save config so vtx settings in place after reboot
+        saveConfigAndNotify();
+    }
+}
+
+//Saves the given power-index value to the configuration setting.
+// index:  Power-index value.
+void vtxSettingsSavePowerByIndex(uint8_t index)
+{
+    if (index != vtxSettingsConfigMutable()->power) {
+        vtxSettingsConfigMutable()->power = index;
+        // need to save config so vtx settings in place after reboot
+        saveConfigAndNotify();
+    }
+}
+
+//Saves the given band/channel/power values to configuration settings.
+// band:  Band value (1 to 5).
+// channel:  Channel value (1 to 8).
+// index:  Power-index value.
+void vtxSettingsSaveBandChanAndPower(uint8_t band, uint8_t channel, uint8_t index)
+{
+    bool modFlag = false;
+    if (band != vtxSettingsConfigMutable()->band) {
+        vtxSettingsConfigMutable()->band = band;
+        modFlag = true;
+    }
+    if (channel != vtxSettingsConfigMutable()->channel) {
+        vtxSettingsConfigMutable()->channel = channel;
+        modFlag = true;
+    }
+    if (index != vtxSettingsConfigMutable()->power) {
+        vtxSettingsConfigMutable()->power = index;
+        modFlag = true;
+    }
+    if (band != 0) {    //enter freq value to band/channel
+        uint16_t freq;
+        if ((freq=vtx58_Bandchan2Freq(band, channel)) != 0 &&
+                                 freq != vtxSettingsConfigMutable()->freq) {
+            vtxSettingsConfigMutable()->freq = freq;
+            modFlag = true;
+        }
+    }
+    if (modFlag) {
+        // need to save config so vtx settings in place after reboot
+        saveConfigAndNotify();
+    }
+}
+
+//Saves the given frequency value to the configuration setting.
+// freq:  Frequency value in MHz.
+void vtxSettingsSaveFrequency(uint16_t freq)
+{
+    bool modFlag = false;
+    if (freq != vtxSettingsConfigMutable()->freq) {
+        vtxSettingsConfigMutable()->freq = freq;
+        modFlag = true;
+    }
+    if (vtxSettingsConfigMutable()->band != 0) {
+        // set band=0 so freq setting will be used
+        vtxSettingsConfigMutable()->band = 0;
+        modFlag = true;
+    }
+    if (modFlag) {
+        // need to save config so vtx settings in place after reboot
+        saveConfigAndNotify();
+    }
+}
+
+//Saves the given frequency/power values to configuration settings.
+// freq:  Frequency value in MHz.
+// index:  Power-index value.
+void vtxSettingsSaveFreqAndPower(uint16_t freq, uint8_t index)
+{
+    bool modFlag = false;
+    if (freq != vtxSettingsConfigMutable()->freq) {
+        vtxSettingsConfigMutable()->freq = freq;
+        modFlag = true;
+    }
+    if (index != vtxSettingsConfigMutable()->power) {
+        vtxSettingsConfigMutable()->power = index;
+        modFlag = true;
+    }
+    if (modFlag) {
+        // need to save config so vtx settings in place after reboot
+        saveConfigAndNotify();
+    }
+}
+
+#endif  //VTX_SETTINGS_CONFIG

src/main/io/vtx_settings_config.h

@@ -0,0 +1,71 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define VTX_SETTINGS_MIN_BAND 1
+#define VTX_SETTINGS_MAX_BAND 5
+#define VTX_SETTINGS_MIN_CHANNEL 1
+#define VTX_SETTINGS_MAX_CHANNEL 8
+
+#define VTX_SETTINGS_BAND_COUNT (VTX_SETTINGS_MAX_BAND - VTX_SETTINGS_MIN_BAND + 1)
+#define VTX_SETTINGS_CHANNEL_COUNT (VTX_SETTINGS_MAX_CHANNEL - VTX_SETTINGS_MIN_CHANNEL + 1)
+
+#define VTX_SETTINGS_DEFAULT_BAND 4         //Fatshark/Airwaves
+#define VTX_SETTINGS_DEFAULT_CHANNEL 1      //CH1
+
+#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999          //max freq (in MHz) for 'vtx_freq' setting
+
+#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
+
+#define VTX_SETTINGS_POWER_COUNT 5
+#define VTX_SETTINGS_DEFAULT_POWER 1
+#define VTX_SETTINGS_CONFIG
+#define VTX_SETTINGS_FREQCMD
+
+#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_CONFIG
+
+#endif
+
+
+#ifdef VTX_SETTINGS_CONFIG
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+typedef struct vtxSettingsConfig_s {
+    uint8_t band;       // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband
+    uint8_t channel;    // 1-8
+    uint8_t power;      // 0 = lowest
+    uint16_t freq;      // sets freq in MHz if band=0
+} vtxSettingsConfig_t;
+
+PG_DECLARE(vtxSettingsConfig_t, vtxSettingsConfig);
+
+void vtxSettingsSaveBandAndChannel(uint8_t band, uint8_t channel);
+void vtxSettingsSavePowerByIndex(uint8_t index);
+void vtxSettingsSaveBandChanAndPower(uint8_t band, uint8_t channel, uint8_t index);
+void vtxSettingsSaveFrequency(uint16_t freq);
+void vtxSettingsSaveFreqAndPower(uint16_t freq, uint8_t index);
+
+#endif  //VTX_SETTINGS_CONFIG

src/main/io/vtx_smartaudio.c

@@ -33,6 +33,7 @@
 #include "cms/cms_types.h"
 #include "cms/cms_menu_vtx_smartaudio.h"
 
+#include "common/maths.h"
 #include "common/printf.h"
 #include "common/utils.h"
 
@@ -51,6 +52,7 @@
 #include "io/serial.h"
 #include "io/vtx_control.h"
 #include "io/vtx_smartaudio.h"
+#include "io/vtx_settings_config.h"
 #include "io/vtx_string.h"
 
 //#define SMARTAUDIO_DPRINTF
@@ -63,7 +65,7 @@ serialPort_t *debugSerialPort = NULL;
 static serialPort_t *smartAudioSerialPort = NULL;
 
 #if defined(CMS) || defined(VTX_COMMON)
-static const char * const saPowerNames[] = {
+static const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = {
     "---", "25 ", "200", "500", "800",
 };
 #endif
@@ -72,9 +74,9 @@ static const char * const saPowerNames[] = {
 static const vtxVTable_t saVTable;    // Forward
 static vtxDevice_t vtxSmartAudio = {
     .vTable = &saVTable,
-    .capability.bandCount = 5,
+    .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
-    .capability.channelCount = 8,
+    .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
-    .capability.powerCount = 4,
+    .capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
     .bandNames = (char **)vtx58BandNames,
     .channelNames = (char **)vtx58ChannelNames,
     .powerNames = (char **)saPowerNames,
@@ -101,6 +103,12 @@ enum {
 #define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE))
 
 
+// convert between 'saDevice.channel' and band/channel values
+#define SA_DEVICE_CHVAL_TO_BAND(val) ((val) / (uint8_t)8)
+#define SA_DEVICE_CHVAL_TO_CHANNEL(val) ((val) % (uint8_t)8)
+#define SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel) ((band) * (uint8_t)8 + (channel))
+
+
 // Statistical counters, for user side trouble shooting.
 
 smartAudioStat_t saStat = {
@@ -113,7 +121,7 @@ smartAudioStat_t saStat = {
     .badcode = 0,
 };
 
-saPowerTable_t saPowerTable[] = {
+saPowerTable_t saPowerTable[VTX_SMARTAUDIO_POWER_COUNT] = {
     {  25,   7,   0 },
     { 200,  16,   1 },
     { 500,  25,   2 },
@@ -173,9 +181,9 @@ static uint8_t CRC8(const uint8_t *data, const int8_t len)
 }
 
 
+#ifdef SMARTAUDIO_DPRINTF
 static void saPrintSettings(void)
 {
-#ifdef SMARTAUDIO_DPRINTF
     dprintf(("Current status: version: %d\r\n", saDevice.version));
     dprintf(("  mode(0x%x): fmode=%s", saDevice.mode,  (saDevice.mode & 1) ? "freq" : "chan"));
     dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off"));
@@ -188,8 +196,8 @@ static void saPrintSettings(void)
     dprintf(("power: %d ", saDevice.power));
     dprintf(("pitfreq: %d ", saDevice.orfreq));
     dprintf(("\r\n"));
-#endif
 }
+#endif
 
 int saDacToPowerIndex(int dac)
 {
@@ -259,10 +267,6 @@ static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
 static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
 static int sa_oslen;         // And associate length
 
-#ifdef CMS
-void saCmsUpdate(void);
-#endif
-
 static void saProcessResponse(uint8_t *buf, int len)
 {
     uint8_t resp = buf[0];
@@ -331,9 +335,13 @@ static void saProcessResponse(uint8_t *buf, int len)
         return;
     }
 
-    // Debug
     if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
+#ifdef CMS    //if changes then trigger saCms update
+        saCmsResetOpmodel();
+#endif
+#ifdef SMARTAUDIO_DPRINTF    // Debug
         saPrintSettings();
+#endif
     }
     saDevicePrev = saDevice;
 
@@ -544,7 +552,12 @@ static void saGetSettings(void)
     saQueueCmd(bufGetSettings, 5);
 }
 
-void saSetFreq(uint16_t freq)
+static bool saValidateFreq(uint16_t freq)
+{
+    return (freq >= VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ && freq <= VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ);
+}
+
+static void saDoDevSetFreq(uint16_t freq)
 {
     static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
 
@@ -563,28 +576,61 @@ void saSetFreq(uint16_t freq)
     saQueueCmd(buf, 7);
 }
 
-#if 0
+static void saDevSetFreq(uint16_t freq)
-static void saSetPitFreq(uint16_t freq)
 {
-    saSetFreq(freq | SA_FREQ_SETPIT);
+    // Need to work around apparent SmartAudio bug when going from 'channel'
+    // to 'user-freq' mode, where the set-freq command will fail if the freq
+    // value is unchanged from the previous 'user-freq' mode
+    if ((saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) == 0 &&
+                                                    freq == saDevice.freq) {
+        saDoDevSetFreq(freq + 1);
+        saSendQueue();
+        saGetSettings();
+        saSendQueue();
+    }
+    saDoDevSetFreq(freq);          //enter desired frequency
 }
 
+void saSetFreq(uint16_t freq)
+{
+    saDevSetFreq(freq);
+    vtxSettingsSaveFrequency(freq);
+}
+
+void saSetPitFreq(uint16_t freq)
+{
+    saDoDevSetFreq(freq | SA_FREQ_SETPIT);
+}
+
+#if 0
 static void saGetPitFreq(void)
 {
-    saSetFreq(SA_FREQ_GETPIT);
+    saDoDevSetFreq(SA_FREQ_GETPIT);
 }
 #endif
 
-void saSetBandAndChannel(uint8_t band, uint8_t channel)
+static bool saValidateBandAndChannel(uint8_t band, uint8_t channel)
+{
+    return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= VTX_SMARTAUDIO_MAX_BAND &&
+             channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
+}
+
+static void saDevSetBandAndChannel(uint8_t band, uint8_t channel)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
 
-    buf[4] = band * 8 + channel;
+    buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
     buf[5] = CRC8(buf, 5);
 
     saQueueCmd(buf, 6);
 }
 
+void saSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    saDevSetBandAndChannel(band, channel);
+    vtxSettingsSaveBandAndChannel(band + 1, channel + 1);
+}
+
 void saSetMode(int mode)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
@@ -595,7 +641,7 @@ void saSetMode(int mode)
     saQueueCmd(buf, 6);
 }
 
-void saSetPowerByIndex(uint8_t index)
+static void saDevSetPowerByIndex(uint8_t index)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
 
@@ -606,7 +652,7 @@ void saSetPowerByIndex(uint8_t index)
         return;
     }
 
-    if (index > 3) {
+    if (index >= VTX_SMARTAUDIO_POWER_COUNT) {
         return;
     }
 
@@ -615,6 +661,43 @@ void saSetPowerByIndex(uint8_t index)
     saQueueCmd(buf, 6);
 }
 
+void saSetPowerByIndex(uint8_t index)
+{
+    saDevSetPowerByIndex(index);
+    vtxSettingsSavePowerByIndex(index + 1);
+}
+
+static bool saEnterInitBandChanAndPower(uint8_t band, uint8_t channel, uint8_t power)
+{
+    if (!saValidateBandAndChannel(band, channel)) {
+        return false;
+    }
+    saDevSetBandAndChannel(band - 1, channel - 1);
+
+    uint8_t pwrIdx = constrain(power - 1, 0, VTX_SMARTAUDIO_POWER_COUNT - 1);
+    saDevSetPowerByIndex(pwrIdx);
+
+    // update 'vtx_freq' via band/channel table and enter
+    //  power-index value (in case current value is out of range)
+    vtxSettingsSaveFreqAndPower(vtx58_Bandchan2Freq(band, channel), pwrIdx + 1);
+
+    return true;
+}
+
+static void saEnterInitFreqAndPower(uint16_t freq, uint8_t power)
+{
+    if (saValidateFreq(freq)) {
+        saSetMode(0);        //need to be in FREE mode to set freq
+        saDevSetFreq(freq);
+    }
+
+    uint8_t pwrIdx = constrain(power - 1, 0, VTX_SMARTAUDIO_POWER_COUNT - 1);
+    saDevSetPowerByIndex(pwrIdx);
+
+    // enter power-index value (in case current value is out of range)
+    vtxSettingsSavePowerByIndex(pwrIdx + 1);
+}
+
 bool vtxSmartAudioInit(void)
 {
 #ifdef SMARTAUDIO_DPRINTF
@@ -651,6 +734,7 @@ bool vtxSmartAudioInit(void)
 void vtxSAProcess(uint32_t now)
 {
     static char initPhase = 0;
+    static bool initSettingsDoneFlag = false;
 
     if (smartAudioSerialPort == NULL) {
         return;
@@ -675,7 +759,7 @@ void vtxSAProcess(uint32_t now)
         // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ,
         // and put the device into user frequency mode with uninitialized freq.
         if (saDevice.version == 2)
-            saSetFreq(SA_FREQ_GETPIT);
+            saDoDevSetFreq(SA_FREQ_GETPIT);
         ++initPhase;
         break;
     }
@@ -689,13 +773,32 @@ void vtxSAProcess(uint32_t now)
         // Command pending. Send it.
         // dprintf(("process: sending queue\r\n"));
         saSendQueue();
-    } else if (now - sa_lastTransmission >= 1000) {
+    } else if (saDevice.version != 0 && now - sa_lastTransmission >= 1000) {
         // Heart beat for autobauding
         //dprintf(("process: sending heartbeat\r\n"));
         saGetSettings();
         saSendQueue();
     }
 
+    // once device is ready enter vtx settings
+    if (!initSettingsDoneFlag) {
+        if (saDevice.version != 0) {
+            initSettingsDoneFlag = true;
+            // if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
+            if (!saEnterInitBandChanAndPower(vtxSettingsConfig()->band,
+                           vtxSettingsConfig()->channel, vtxSettingsConfig()->power)) {
+                // if vtx_band==0 then enter 'vtx_freq' value (and power)
+                if (vtxSettingsConfig()->band == 0) {
+                    saEnterInitFreqAndPower(vtxSettingsConfig()->freq, vtxSettingsConfig()->power);
+                }
+            }
+        } else if (now - sa_lastTransmission >= 100) {
+            // device is not ready; repeat query
+            saGetSettings();
+            saSendQueue();
+        }
+    }
+
 #ifdef SMARTAUDIO_TEST_VTX_COMMON
     // Testing VTX_COMMON API
     {
@@ -732,7 +835,7 @@ bool vtxSAIsReady(void)
 
 void vtxSASetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    if (band && channel) {
+    if (saValidateBandAndChannel(band, channel)) {
         saSetBandAndChannel(band - 1, channel - 1);
     }
 }
@@ -773,14 +876,24 @@ void vtxSASetPitMode(uint8_t onoff)
     return;
 }
 
+void vtxSASetFreq(uint16_t freq)
+{
+    if (saValidateFreq(freq)) {
+        saSetMode(0);        //need to be in FREE mode to set freq
+        saSetFreq(freq);
+    }
+}
+
 bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
 {
     if (!vtxSAIsReady()) {
         return false;
     }
 
-    *pBand = (saDevice.channel / 8) + 1;
+    // if in user-freq mode then report band as zero
-    *pChannel = (saDevice.channel % 8) + 1;
+    *pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 :
+             (SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1);
+    *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1;
     return true;
 }
 
@@ -804,6 +917,19 @@ bool vtxSAGetPitMode(uint8_t *pOnOff)
     return true;
 }
 
+bool vtxSAGetFreq(uint16_t *pFreq)
+{
+    if (!vtxSAIsReady()) {
+        return false;
+    }
+
+    // if not in user-freq mode then convert band/chan to frequency
+    *pFreq = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? saDevice.freq :
+             vtx58_Bandchan2Freq(SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1,
+                                 SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1);
+    return true;
+}
+
 static const vtxVTable_t saVTable = {
     .process = vtxSAProcess,
     .getDeviceType = vtxSAGetDeviceType,
@@ -811,9 +937,11 @@ static const vtxVTable_t saVTable = {
     .setBandAndChannel = vtxSASetBandAndChannel,
     .setPowerByIndex = vtxSASetPowerByIndex,
     .setPitMode = vtxSASetPitMode,
+    .setFrequency = vtxSASetFreq,
     .getBandAndChannel = vtxSAGetBandAndChannel,
     .getPowerIndex = vtxSAGetPowerIndex,
     .getPitMode = vtxSAGetPitMode,
+    .getFrequency = vtxSAGetFreq,
 };
 #endif // VTX_COMMON
 

src/main/io/vtx_smartaudio.h

@@ -1,3 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
 #pragma once
 
 #include <stdbool.h>
@@ -5,6 +22,20 @@
 
 #include "platform.h"
 
+#define VTX_SMARTAUDIO_MIN_BAND 1
+#define VTX_SMARTAUDIO_MAX_BAND 5
+#define VTX_SMARTAUDIO_MIN_CHANNEL 1
+#define VTX_SMARTAUDIO_MAX_CHANNEL 8
+
+#define VTX_SMARTAUDIO_BAND_COUNT (VTX_SMARTAUDIO_MAX_BAND - VTX_SMARTAUDIO_MIN_BAND + 1)
+#define VTX_SMARTAUDIO_CHANNEL_COUNT (VTX_SMARTAUDIO_MAX_CHANNEL - VTX_SMARTAUDIO_MIN_CHANNEL + 1)
+
+#define VTX_SMARTAUDIO_POWER_COUNT 4
+#define VTX_SMARTAUDIO_DEFAULT_POWER 1
+
+#define VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ 5000        //min freq in MHz
+#define VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ 5999        //max freq in MHz
+
 // opmode flags, GET side
 #define SA_MODE_GET_FREQ_BY_FREQ            1
 #define SA_MODE_GET_PITMODE                 2
@@ -65,6 +96,7 @@ void saSetBandAndChannel(uint8_t band, uint8_t channel);
 void saSetMode(int mode);
 void saSetPowerByIndex(uint8_t index);
 void saSetFreq(uint16_t freq);
+void saSetPitFreq(uint16_t freq);
 bool vtxSmartAudioInit(void);
 
 #ifdef SMARTAUDIO_DPRINTF

src/main/io/vtx_string.c

@@ -26,7 +26,10 @@
 
 #if defined(VTX_COMMON)
 
-const uint16_t vtx58frequencyTable[5][8] =
+#define VTX_STRING_BAND_COUNT 5
+#define VTX_STRING_CHAN_COUNT 8
+
+const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] =
 {
     { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A
     { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B
@@ -50,12 +53,13 @@ const char * const vtx58ChannelNames[] = {
     "-", "1", "2", "3", "4", "5", "6", "7", "8",
 };
 
+//Converts frequency (in MHz) to band and channel values.
 bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
 {
     // Use reverse lookup order so that 5880Mhz
     // get Raceband 7 instead of Fatshark 8.
-    for (int band = 4 ; band >= 0 ; band--) {
+    for (int band = VTX_STRING_BAND_COUNT - 1 ; band >= 0 ; band--) {
-        for (int channel = 0 ; channel < 8 ; channel++) {
+        for (int channel = 0 ; channel < VTX_STRING_CHAN_COUNT ; channel++) {
             if (vtx58frequencyTable[band][channel] == freq) {
                 *pBand = band + 1;
                 *pChannel = channel + 1;
@@ -70,4 +74,17 @@ bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
     return false;
 }
 
+//Converts band and channel values to a frequency (in MHz) value.
+// band:  Band value (1 to 5).
+// channel:  Channel value (1 to 8).
+// Returns frequency value (in MHz), or 0 if band/channel out of range.
+uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel)
+{
+    if (band > 0 && band <= VTX_STRING_BAND_COUNT &&
+                          channel > 0 && channel <= VTX_STRING_CHAN_COUNT) {
+        return vtx58frequencyTable[band - 1][channel - 1];
+    }
+    return 0;
+}
+
 #endif

src/main/io/vtx_string.h

@@ -12,5 +12,6 @@ extern const char * const vtx58ChannelNames[];
 extern const char vtx58BandLetter[];
 
 bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel);
+uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel);
 
 #endif

src/main/io/vtx_tramp.c

@@ -28,6 +28,7 @@
 
 #include "build/debug.h"
 
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "cms/cms_menu_vtx_tramp.h"
@@ -35,9 +36,10 @@
 #include "drivers/vtx_common.h"
 
 #include "io/serial.h"
+#include "io/vtx_tramp.h"
+#include "io/vtx_settings_config.h"
 #include "io/vtx_control.h"
 #include "io/vtx_string.h"
-#include "io/vtx_tramp.h"
 
 #if defined(CMS) || defined(VTX_COMMON)
 const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
@@ -53,8 +55,8 @@ const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
 static const vtxVTable_t trampVTable; // forward
 static vtxDevice_t vtxTramp = {
     .vTable = &trampVTable,
-    .capability.bandCount = 5,
+    .capability.bandCount = VTX_TRAMP_BAND_COUNT,
-    .capability.channelCount = 8,
+    .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT,
     .capability.powerCount = sizeof(trampPowerTable),
     .bandNames = (char **)vtx58BandNames,
     .channelNames = (char **)vtx58ChannelNames,
@@ -81,6 +83,7 @@ uint32_t trampRFFreqMin;
 uint32_t trampRFFreqMax;
 uint32_t trampRFPowerMax;
 
+bool trampSetByFreqFlag = false;  //false = set via band/channel
 uint32_t trampCurFreq = 0;
 uint8_t trampBand = 0;
 uint8_t trampChannel = 0;
@@ -129,7 +132,12 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
     trampWriteBuf(trampReqBuffer);
 }
 
-void trampSetFreq(uint16_t freq)
+static bool trampValidateFreq(uint16_t freq)
+{
+    return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
+}
+
+static void trampDevSetFreq(uint16_t freq)
 {
     trampConfFreq = freq;
     if (trampConfFreq != trampCurFreq) {
@@ -137,14 +145,34 @@ void trampSetFreq(uint16_t freq)
     }
 }
 
+void trampSetFreq(uint16_t freq)
+{
+    trampSetByFreqFlag = true;         //set freq via MHz value
+    trampDevSetFreq(freq);
+    vtxSettingsSaveFrequency(freq);
+}
+
 void trampSendFreq(uint16_t freq)
 {
     trampCmdU16('F', freq);
 }
 
+static bool trampValidateBandAndChannel(uint8_t band, uint8_t channel)
+{
+    return (band >= VTX_TRAMP_MIN_BAND && band <= VTX_TRAMP_MAX_BAND &&
+            channel >= VTX_TRAMP_MIN_CHANNEL && channel <= VTX_TRAMP_MAX_CHANNEL);
+}
+
+static void trampDevSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    trampDevSetFreq(vtx58_Bandchan2Freq(band, channel));
+}
+
 void trampSetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    trampSetFreq(vtx58frequencyTable[band - 1][channel - 1]);
+    trampSetByFreqFlag = false;        //set freq via band/channel
+    trampDevSetBandAndChannel(band, channel);
+    vtxSettingsSaveBandAndChannel(band, channel);
 }
 
 void trampSetRFPower(uint16_t level)
@@ -171,6 +199,17 @@ bool trampCommitChanges(void)
     return true;
 }
 
+// return false if index out of range
+static bool trampDevSetPowerByIndex(uint8_t index)
+{
+    if (index > 0 && index <= sizeof(trampPowerTable)) {
+        trampSetRFPower(trampPowerTable[index - 1]);
+        trampCommitChanges();
+        return true;
+    }
+    return false;
+}
+
 void trampSetPitMode(uint8_t onoff)
 {
     trampCmdU16('I', onoff ? 0 : 1);
@@ -204,7 +243,11 @@ char trampHandleResponse(void)
                 trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
                 trampPitMode = trampRespBuffer[7];
                 trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
-                vtx58_Freq2Bandchan(trampCurFreq, &trampBand, &trampChannel);
+
+                // if no band/chan match then make sure set-by-freq mode is flagged
+                if (!vtx58_Freq2Bandchan(trampCurFreq, &trampBand, &trampChannel)) {
+                    trampSetByFreqFlag = true;
+                }
 
                 if (trampConfFreq == 0)  trampConfFreq  = trampCurFreq;
                 if (trampConfPower == 0) trampConfPower = trampPower;
@@ -325,9 +368,42 @@ void trampQueryS(void)
     trampQuery('s');
 }
 
+static bool trampEnterInitBandChanAndPower(uint8_t band, uint8_t channel, uint8_t power)
+{
+    if (!trampValidateBandAndChannel(band, channel)) {
+        return false;
+    }
+    trampSetByFreqFlag = false;        //set freq via band/channel
+    trampDevSetBandAndChannel(band, channel);
+
+    uint8_t pwrIdx = constrain(power, 1, sizeof(trampPowerTable));
+    trampDevSetPowerByIndex(pwrIdx);
+
+    // update 'vtx_freq' via band/channel table and enter
+    //  power-index value (in case current value is out of range)
+    vtxSettingsSaveFreqAndPower(vtx58_Bandchan2Freq(band, channel), pwrIdx);
+
+    return true;
+}
+
+static void trampEnterInitFreqAndPower(uint16_t freq, uint8_t power)
+{
+    if (trampValidateFreq(freq)) {
+        trampSetByFreqFlag = true;          //set freq via MHz value
+        trampDevSetFreq(freq);
+    }
+
+    uint8_t pwrIdx = constrain(power, 1, sizeof(trampPowerTable));
+    trampDevSetPowerByIndex(pwrIdx);
+
+    // enter power-index value (in case current value is out of range)
+    vtxSettingsSavePowerByIndex(pwrIdx);
+}
+
 void vtxTrampProcess(uint32_t currentTimeUs)
 {
     static uint32_t lastQueryTimeUs = 0;
+    static bool initSettingsDoneFlag = false;
 
 #ifdef TRAMP_DEBUG
     static uint16_t debugFreqReqCounter = 0;
@@ -348,6 +424,19 @@ void vtxTrampProcess(uint32_t currentTimeUs)
     case 'r':
         if (trampStatus <= TRAMP_STATUS_OFFLINE) {
             trampStatus = TRAMP_STATUS_ONLINE;
+
+            // once device is ready enter vtx settings
+            if (!initSettingsDoneFlag) {
+                initSettingsDoneFlag = true;
+                // if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
+                if (!trampEnterInitBandChanAndPower(vtxSettingsConfig()->band, vtxSettingsConfig()->channel,
+                                                    vtxSettingsConfig()->power)) {
+                    // if vtx_band=0 then enter 'vtx_freq' value (and power)
+                    if (vtxSettingsConfig()->band == 0) {
+                        trampEnterInitFreqAndPower(vtxSettingsConfig()->freq, vtxSettingsConfig()->power);
+                    }
+                }
+            }
         }
         break;
 
@@ -453,7 +542,7 @@ bool vtxTrampIsReady(void)
 
 void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    if (band && channel) {
+    if (trampValidateBandAndChannel(band, channel)) {
         trampSetBandAndChannel(band, channel);
         trampCommitChanges();
     }
@@ -461,9 +550,8 @@ void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel)
 
 void vtxTrampSetPowerByIndex(uint8_t index)
 {
-    if (index) {
+    if (trampDevSetPowerByIndex(index)) {
-        trampSetRFPower(trampPowerTable[index - 1]);
+        vtxSettingsSavePowerByIndex(index);
-        trampCommitChanges();
     }
 }
 
@@ -472,13 +560,22 @@ void vtxTrampSetPitMode(uint8_t onoff)
     trampSetPitMode(onoff);
 }
 
+void vtxTrampSetFreq(uint16_t freq)
+{
+    if (trampValidateFreq(freq)) {
+        trampSetFreq(freq);
+        trampCommitChanges();
+    }
+}
+
 bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
 {
     if (!vtxTrampIsReady()) {
         return false;
     }
 
-    *pBand = trampBand;
+    // if in user-freq mode then report band as zero
+    *pBand = trampSetByFreqFlag ? 0 : trampBand;
     *pChannel = trampChannel;
     return true;
 }
@@ -511,6 +608,16 @@ bool vtxTrampGetPitMode(uint8_t *pOnOff)
     return true;
 }
 
+bool vtxTrampGetFreq(uint16_t *pFreq)
+{
+    if (!vtxTrampIsReady()) {
+        return false;
+    }
+
+    *pFreq = trampCurFreq;
+    return true;
+}
+
 static const vtxVTable_t trampVTable = {
     .process = vtxTrampProcess,
     .getDeviceType = vtxTrampGetDeviceType,
@@ -518,9 +625,11 @@ static const vtxVTable_t trampVTable = {
     .setBandAndChannel = vtxTrampSetBandAndChannel,
     .setPowerByIndex = vtxTrampSetPowerByIndex,
     .setPitMode = vtxTrampSetPitMode,
+    .setFrequency = vtxTrampSetFreq,
     .getBandAndChannel = vtxTrampGetBandAndChannel,
     .getPowerIndex = vtxTrampGetPowerIndex,
     .getPitMode = vtxTrampGetPitMode,
+    .getFrequency = vtxTrampGetFreq,
 };
 
 #endif

src/main/io/vtx_tramp.h

@@ -1,8 +1,38 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
 #pragma once
 
 #include <stdint.h>
 
+#define VTX_TRAMP_MIN_BAND 1
+#define VTX_TRAMP_MAX_BAND 5
+#define VTX_TRAMP_MIN_CHANNEL 1
+#define VTX_TRAMP_MAX_CHANNEL 8
+
+#define VTX_TRAMP_BAND_COUNT (VTX_TRAMP_MAX_BAND - VTX_TRAMP_MIN_BAND + 1)
+#define VTX_TRAMP_CHANNEL_COUNT (VTX_TRAMP_MAX_CHANNEL - VTX_TRAMP_MIN_CHANNEL + 1)
+
 #define VTX_TRAMP_POWER_COUNT 5
+#define VTX_TRAMP_DEFAULT_POWER 1
+
+#define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000             //min freq in MHz
+#define VTX_TRAMP_MAX_FREQUENCY_MHZ 5999             //max freq in MHz
+
 extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT];
 extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1];