From 8a27c9e1946bcbc04ef4b93f1bc39504bae826fd Mon Sep 17 00:00:00 2001 From: ethomas999 Date: Sun, 1 Oct 2017 13:54:58 -0400 Subject: [PATCH] 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 | 2 + src/main/cms/cms_menu_vtx_rtc6705.c | 15 +-- src/main/cms/cms_menu_vtx_smartaudio.c | 54 +++++--- src/main/cms/cms_menu_vtx_smartaudio.h | 2 + src/main/cms/cms_menu_vtx_tramp.c | 7 +- src/main/config/parameter_group_ids.h | 2 +- src/main/drivers/vtx_common.c | 22 +++ src/main/drivers/vtx_common.h | 8 ++ src/main/fc/cli.c | 12 +- src/main/fc/fc_msp.c | 38 ++++-- src/main/fc/settings.c | 14 +- src/main/io/vtx_rtc6705.c | 44 +++--- src/main/io/vtx_rtc6705.h | 20 ++- src/main/io/vtx_settings_config.c | 148 ++++++++++++++++++++ src/main/io/vtx_settings_config.h | 71 ++++++++++ src/main/io/vtx_smartaudio.c | 180 +++++++++++++++++++++---- src/main/io/vtx_smartaudio.h | 32 +++++ src/main/io/vtx_string.c | 23 +++- src/main/io/vtx_string.h | 1 + src/main/io/vtx_tramp.c | 131 ++++++++++++++++-- src/main/io/vtx_tramp.h | 30 +++++ 21 files changed, 729 insertions(+), 127 deletions(-) create mode 100644 src/main/io/vtx_settings_config.c create mode 100644 src/main/io/vtx_settings_config.h diff --git a/make/source.mk b/make/source.mk index ab38dc7c4e..0e56d79688 100644 --- a/make/source.mk +++ b/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 \ diff --git a/src/main/cms/cms_menu_vtx_rtc6705.c b/src/main/cms/cms_menu_vtx_rtc6705.c index cf9a2b1293..3826033d67 100644 --- a/src/main/cms/cms_menu_vtx_rtc6705.c +++ b/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_TAB_t entryVtxPower = {&cmsx_vtxPower, RTC6705_POWER_COUNT - 1, &rtc6705PowerNames[0]}; +static OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, VTX_RTC6705_CHANNEL_COUNT, 1}; +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_vtxChannel = vtxRTC6705Config()->channel; - cmsx_vtxPower = vtxRTC6705Config()->power; + cmsx_vtxBand = vtxSettingsConfig()->band - 1; + cmsx_vtxChannel = vtxSettingsConfig()->channel; + cmsx_vtxPower = vtxSettingsConfig()->power; } static void cmsx_Vtx_ConfigWriteback(void) { - vtxRTC6705ConfigMutable()->band = cmsx_vtxBand + 1; - vtxRTC6705ConfigMutable()->channel = cmsx_vtxChannel; - vtxRTC6705ConfigMutable()->power = cmsx_vtxPower; + vtxSettingsSaveBandChanAndPower(cmsx_vtxBand + 1, cmsx_vtxChannel, cmsx_vtxPower); } static long cmsx_Vtx_onEnter(void) diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index 6636e8b4ab..1ce6d7cca3 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/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) { - // CHAN - saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1); - } else { - // USER: User frequency mode is only available in FREE opmodel. - if (saCmsOpmodel == SACMS_OPMODEL_FREE) { - saSetFreq(saCmsUserFreq); - } else { - // Bounce back - saCmsFselMode = 0; - } + // if trying to do user frequency mode in RACE opmodel then + // revert because user-freq only available in FREE opmodel + if (saCmsFselModeNew != 0 && saCmsOpmodel != SACMS_OPMODEL_FREE) { + saCmsFselModeNew = 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; diff --git a/src/main/cms/cms_menu_vtx_smartaudio.h b/src/main/cms/cms_menu_vtx_smartaudio.h index 206c3a719a..f2891306e1 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.h +++ b/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(); diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index ff0f9afaf2..1aa2780705 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/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) { diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 0c4d3a2f2e..5fbd7dab13 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index b58121487d..7b0d5d83e1 100644 --- a/src/main/drivers/vtx_common.c +++ b/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) diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index fc74f130d8..776dba7aca 100644 --- a/src/main/drivers/vtx_common.h +++ b/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); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9fa72d0a98..68a7e68756 100755 --- a/src/main/fc/cli.c +++ b/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++; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index fd4a1467ee..b7e56f3c23 100644 --- a/src/main/fc/fc_msp.c +++ b/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(¤t_band,¤t_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(¤t_band, ¤t_channel); + if ((current_band != band) || (current_channel != channel)) { + vtxCommonSetBandAndChannel(band, channel); + } + } else { //value is frequency in MHz + uint16_t currentFreq; + vtxCommonGetFrequency(¤tFreq); + if (currentFreq != tmp) { + vtxCommonSetFrequency(tmp); + } + } + + if (sbufBytesRemaining(src) < 2) { break; + } uint8_t power = sbufReadU8(src); uint8_t current_power = 0; vtxCommonGetPowerIndex(¤t_power); - if (current_power != power) + if (current_power != power) { vtxCommonSetPowerByIndex(power); + } uint8_t pitmode = sbufReadU8(src); uint8_t current_pitmode = 0; vtxCommonGetPitMode(¤t_pitmode); - if (current_pitmode != pitmode) + if (current_pitmode != pitmode) { vtxCommonSetPitMode(pitmode); + } } } break; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 7f0ed8b121..0467d7a9ce 100644 --- a/src/main/fc/settings.c +++ b/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 -#ifdef VTX_RTC6705 - { "vtx_band", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, band) }, - { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, channel) }, - { "vtx_power", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, RTC6705_POWER_COUNT - 1 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, power) }, +// PG_VTX_CONFIG +#ifdef VTX_SETTINGS_CONFIG + { "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 = { 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, 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 diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c index 315265438b..faf3d793e3 100644 --- a/src/main/io/vtx_rtc6705.c +++ b/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.channelCount = 8, - .capability.powerCount = RTC6705_POWER_COUNT, + .capability.bandCount = VTX_RTC6705_BAND_COUNT, + .capability.channelCount = VTX_RTC6705_CHANNEL_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.channel = vtxRTC6705Config()->channel; - vtxRTC6705.powerIndex = vtxRTC6705Config()->power; + vtxRTC6705.band = vtxSettingsConfig()->band; + vtxRTC6705.channel = vtxSettingsConfig()->channel; + 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; - vtxRTC6705ConfigMutable()->channel = channel; + vtxSettingsSaveBandAndChannel(band, 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 diff --git a/src/main/io/vtx_rtc6705.h b/src/main/io/vtx_rtc6705.h index 002800fea8..39f99dff2a 100644 --- a/src/main/io/vtx_rtc6705.h +++ b/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 { - 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 -} vtxRTC6705Config_t; - -PG_DECLARE(vtxRTC6705Config_t, vtxRTC6705Config); +#define VTX_RTC6705_BAND_COUNT (VTX_RTC6705_MAX_BAND - VTX_RTC6705_MIN_BAND + 1) +#define VTX_RTC6705_CHANNEL_COUNT (VTX_RTC6705_MAX_CHANNEL - VTX_RTC6705_MIN_CHANNEL + 1) #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); diff --git a/src/main/io/vtx_settings_config.c b/src/main/io/vtx_settings_config.c new file mode 100644 index 0000000000..92846da2ed --- /dev/null +++ b/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 . + */ + +#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 diff --git a/src/main/io/vtx_settings_config.h b/src/main/io/vtx_settings_config.h new file mode 100644 index 0000000000..cfd4d78732 --- /dev/null +++ b/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 . + */ + +#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 diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 61b1fef5fa..9d1c1caf74 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/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.channelCount = 8, - .capability.powerCount = 4, + .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT, + .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT, + .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 saSetPitFreq(uint16_t freq) +static void saDevSetFreq(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; - *pChannel = (saDevice.channel % 8) + 1; + // if in user-freq mode then report band as zero + *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 diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index a4a0cf820d..6a862cb0ea 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/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 . + */ + #pragma once #include @@ -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 diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index c341a1699f..e28655db29 100644 --- a/src/main/io/vtx_string.c +++ b/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 channel = 0 ; channel < 8 ; channel++) { + for (int band = VTX_STRING_BAND_COUNT - 1 ; band >= 0 ; band--) { + 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 diff --git a/src/main/io/vtx_string.h b/src/main/io/vtx_string.h index 736ac8daa3..44854128ce 100644 --- a/src/main/io/vtx_string.h +++ b/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 diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 09e9fb4e9e..e5bb0c4134 100644 --- a/src/main/io/vtx_tramp.c +++ b/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.channelCount = 8, + .capability.bandCount = VTX_TRAMP_BAND_COUNT, + .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) { - trampSetRFPower(trampPowerTable[index - 1]); - trampCommitChanges(); + if (trampDevSetPowerByIndex(index)) { + vtxSettingsSavePowerByIndex(index); } } @@ -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 diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index edb420cd1b..6fd2995fbc 100644 --- a/src/main/io/vtx_tramp.h +++ b/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 . + */ + #pragma once #include +#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];