From 7b223eb215e7601c82fc8015987161b957ccdfe1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Tue, 19 Jun 2018 19:07:29 +0100 Subject: [PATCH] Port new VTX API and settings from Betaflight - VTX with support for FC control are now totally managed from the FC. The FC will store the VTX configuration and override any changes made manually (e.g. with a button). Users can disable VTX control to manage channels manually. - OSD VTX item now shows the power level too. - Added new parameters for VTX configuration: vtx_band, vtx_channel, vtx_freq, vtx_halfduplex, vtx_low_power_disarm, vtx_pit_mode_freq and vtx_power. - Added support for automatically switching the VTX power when arming (fixes #3112). Note that there are a several changes from the BF code. We do support an additional setting for the low power during disarm option and the MSP messages contain more data to allow users to configure the VTX channel/power from the configurator. Our MSP messages also work when the VTX is offline (the settings are stored in the FC and applied later once the VTX is powered up). Thanks to Matek (http://www.mateksys.com) for providing an FCHUB-VTX to test the Tramp protocol. Thanks to AKK (https://www.akktek.com) for providing an X2-ultimate to test SmartAudio. --- docs/Cli.md | 7 + make/source.mk | 1 + src/main/build/debug.h | 1 + src/main/cms/cms_menu_builtin.c | 6 +- src/main/cms/cms_menu_osd.c | 4 +- src/main/cms/cms_menu_vtx_smartaudio.c | 255 +++++++++------ src/main/cms/cms_menu_vtx_smartaudio.h | 23 +- src/main/cms/cms_menu_vtx_tramp.c | 55 ++-- src/main/config/parameter_group_ids.h | 3 + src/main/drivers/serial.h | 3 +- src/main/drivers/serial_softserial.c | 6 +- src/main/drivers/vtx_common.c | 124 +++---- src/main/drivers/vtx_common.h | 95 ++++-- src/main/drivers/vtx_rtc6705.c | 12 +- src/main/drivers/vtx_rtc6705.h | 19 +- src/main/drivers/vtx_rtc6705_soft_spi.c | 2 +- src/main/fc/fc_init.c | 12 +- src/main/fc/fc_msp.c | 95 +++--- src/main/fc/fc_tasks.c | 23 +- src/main/fc/settings.yaml | 47 ++- src/main/io/osd.c | 19 +- src/main/io/vtx.c | 279 ++++++++++++++++ src/main/io/vtx.h | 49 +++ src/main/io/vtx_control.c | 64 +++- src/main/io/vtx_control.h | 2 + src/main/io/vtx_smartaudio.c | 411 +++++++++++++++--------- src/main/io/vtx_smartaudio.h | 52 +-- src/main/io/vtx_string.c | 26 +- src/main/io/vtx_string.h | 1 + src/main/io/vtx_tramp.c | 286 +++++++++++------ src/main/io/vtx_tramp.h | 2 +- src/main/scheduler/scheduler.h | 2 +- src/main/target/SPRACINGF3NEO/target.h | 6 +- src/main/target/SPRACINGF4EVO/target.h | 2 +- src/main/target/common.h | 9 +- src/main/target/common_post.h | 2 +- src/utils/rename-ifdefs.py | 5 + 37 files changed, 1419 insertions(+), 591 deletions(-) create mode 100644 src/main/io/vtx.c create mode 100644 src/main/io/vtx.h diff --git a/docs/Cli.md b/docs/Cli.md index 4726c3c8d37..862deb52bdd 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -417,6 +417,13 @@ Re-apply any new defaults as desired. | model_preview_type | -1 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. | | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | +| vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | +| vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | +| vtx_freq | 5740 | Set the VTX frequency using raw MHz. This parameter is ignored unless `vtx_band` is 0. | +| vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | +| vtx_low_power_disarm | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | +| vtx_pit_mode_freq | Frequency to use (in MHz) when the VTX is in pit mode. | +| vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | This Markdown table is made by MarkdwonTableMaker addon for google spreadsheet. Original Spreadsheet used to make this table can be found here https://docs.google.com/spreadsheets/d/1ubjYdMGmZ2aAMUNYkdfe3hhIF7wRfIjcuPOi_ysmp00/edit?usp=sharing diff --git a/make/source.mk b/make/source.mk index 43197df0419..c91a9f93484 100644 --- a/make/source.mk +++ b/make/source.mk @@ -181,6 +181,7 @@ COMMON_SRC = \ telemetry/msp_shared.c \ telemetry/smartport.c \ telemetry/telemetry.c \ + io/vtx.c \ io/vtx_string.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index d471f56a888..47cdd5557d4 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -66,6 +66,7 @@ typedef enum { DEBUG_VIBE, DEBUG_CRUISE, DEBUG_REM_FLIGHT_TIME, + DEBUG_SMARTAUDIO, DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index a716a873d22..32a9dd68aba 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -112,11 +112,11 @@ static const OSD_Entry menuFeaturesEntries[] = #if defined(VTX) || defined(USE_RTC6705) OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtx), #endif // VTX || USE_RTC6705 -#if defined(VTX_CONTROL) -#if defined(VTX_SMARTAUDIO) +#if defined(USE_VTX_CONTROL) +#if defined(USE_VTX_SMARTAUDIO) OSD_SUBMENU_ENTRY("VTX SA", &cmsx_menuVtxSmartAudio), #endif -#if defined(VTX_TRAMP) +#if defined(USE_VTX_TRAMP) OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp), #endif #endif // VTX_CONTROL diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index e62e17d605a..f3f729ab64c 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -165,7 +165,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS), OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR), OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES), -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL), #endif // VTX OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW), @@ -215,7 +215,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_END_ENTRY, }; -#if defined(VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) +#if defined(USE_VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) // All CMS OSD elements should be enabled in this case. The menu has 3 extra // elements (label, back and end), but there's an OSD element that we intentionally // don't show here (OSD_DEBUG). diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index 7e28dd3ec23..a7e81bd989a 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/src/main/cms/cms_menu_vtx_smartaudio.c @@ -1,27 +1,31 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * along with this software. + * + * If not, see . */ #include #include +#include #include #include "platform.h" -#if defined(USE_CMS) && defined(VTX_SMARTAUDIO) +#if defined(USE_CMS) && defined(USE_VTX_SMARTAUDIO) #include "common/printf.h" #include "common/utils.h" @@ -36,6 +40,7 @@ #include "io/vtx_string.h" #include "io/vtx_smartaudio.h" +#include "io/vtx.h" // Interface to CMS @@ -65,8 +70,10 @@ uint16_t saCmsDeviceFreq = 0; uint8_t saCmsDeviceStatus = 0; uint8_t saCmsPower; -uint8_t saCmsPitFMode; // In-Range or Out-Range +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 @@ -74,44 +81,33 @@ uint16_t saCmsORFreqNew; // POR frequency uint16_t saCmsUserFreq = 0; // User defined frequency uint16_t saCmsUserFreqNew; // User defined frequency -void saCmsUpdate(void) -{ -// XXX Take care of pit mode update somewhere??? - - if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { - // This is a first valid response to GET_SETTINGS. - saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; - - saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0; - - saCmsBand = (saDevice.channel / 8) + 1; - saCmsChan = (saDevice.channel % 8) + 1; - saCmsFreqRef = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8]; - - saCmsDeviceFreq = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8]; - - if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { - saCmsRFState = SACMS_TXMODE_ACTIVE; - } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { - saCmsRFState = SACMS_TXMODE_PIT_INRANGE; - } else { - saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE; - } - - if (saDevice.version == 2) { - saCmsPower = saDevice.power + 1; // XXX Take care V1 - } else { - saCmsPower = saDacToPowerIndex(saDevice.power) + 1; - } - } -} - static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self); static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self); static long saCmsConfigBandByGvar(displayPort_t *, const void *self); static long saCmsConfigChanByGvar(displayPort_t *, const void *self); static long saCmsConfigPowerByGvar(displayPort_t *, const void *self); +static bool saCmsUpdateCopiedState(void) +{ + if (saDevice.version == 0) + return false; + + if (saCmsDeviceStatus == 0 && saDevice.version != 0) + saCmsDeviceStatus = saDevice.version; + if (saCmsORFreq == 0 && saDevice.orfreq != 0) + saCmsORFreq = saDevice.orfreq; + if (saCmsUserFreq == 0 && saDevice.freq != 0) + saCmsUserFreq = saDevice.freq; + + if (saDevice.version == 2) { + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + saCmsPitFMode = 1; + else + saCmsPitFMode = 0; + } + return true; +} + static bool saCmsDrawStatusString(char *buf, unsigned bufsize) { const char *defaultString = "- -- ---- ---"; @@ -124,21 +120,10 @@ static bool saCmsDrawStatusString(char *buf, unsigned bufsize) strcpy(buf, defaultString); - if (saDevice.version == 0) + if (!saCmsUpdateCopiedState()) { + // VTX is not ready return true; - - // XXX These should be done somewhere else - if (saCmsDeviceStatus == 0 && saDevice.version != 0) - saCmsDeviceStatus = saDevice.version; - if (saCmsORFreq == 0 && saDevice.orfreq != 0) - saCmsORFreq = saDevice.orfreq; - if (saCmsUserFreq == 0 && saDevice.freq != 0) - saCmsUserFreq = saDevice.freq; - - if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) - saCmsPitFMode = 1; - else - saCmsPitFMode = 0; + } buf[0] = "-FR"[saCmsOpmodel]; @@ -176,6 +161,47 @@ static bool saCmsDrawStatusString(char *buf, unsigned bufsize) return true; } +void saCmsUpdate(void) +{ +// XXX Take care of pit mode update somewhere??? + if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { + // This is a first valid response to GET_SETTINGS. + saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; + + saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0; + + saCmsBand = vtxSettingsConfig()->band; + saCmsChan = vtxSettingsConfig()->channel; + saCmsFreqRef = vtxSettingsConfig()->freq; + saCmsDeviceFreq = saCmsFreqRef; + + if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { + saCmsRFState = SACMS_TXMODE_ACTIVE; + } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { + saCmsRFState = SACMS_TXMODE_PIT_INRANGE; + } else { + saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE; + } + + saCmsPower = vtxSettingsConfig()->power; + + // if user-freq mode then track possible change + if (saCmsFselMode && vtxSettingsConfig()->freq) { + saCmsUserFreq = vtxSettingsConfig()->freq; + } + + saCmsFselModeNew = saCmsFselMode; //init mode for menu + } + + saCmsUpdateCopiedState(); +} + +void saCmsResetOpmodel() +{ + // trigger data refresh in 'saCmsUpdate()' + saCmsOpmodel = SACMS_OPMODEL_UNDEF; +} + static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); @@ -243,8 +269,9 @@ static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self) return 0; } - if (saCmsOpmodel == SACMS_OPMODEL_FREE) - saSetPowerByIndex(saCmsPower - 1); + if (saCmsOpmodel == SACMS_OPMODEL_FREE && !saDeferred) { + vtxSettingsConfigMutable()->power = saCmsPower; + } return 0; } @@ -254,9 +281,21 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); + if (saDevice.version == 1) { + // V1 device doesn't support PIT mode; bounce back. + saCmsPitFMode = 0; + return 0; + } + dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode)); if (saCmsPitFMode == 0) { + // Bounce back + saCmsPitFMode = 1; + return 0; + } + + if (saCmsPitFMode == 1) { saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); } else { saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE); @@ -272,6 +311,12 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); + if (saDevice.version == 1) { + if (saCmsOpmodel != SACMS_OPMODEL_FREE) + saCmsOpmodel = SACMS_OPMODEL_FREE; + return 0; + } + uint8_t opmodel = saCmsOpmodel; dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel)); @@ -288,7 +333,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 @@ -298,6 +343,7 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) return 0; } +#ifdef USE_EXTENDED_CMS_MENUS static const char * const saCmsDeviceStatusNames[] = { "OFFL", "ONL V1", @@ -332,20 +378,13 @@ static const CMS_Menu saCmsMenuStats = { .onGlobalExit = NULL, .entries = saCmsMenuStatsEntries }; +#endif /* USE_EXTENDED_CMS_MENUS */ -static const OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames }; - -static const OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChannelNames }; +static const OSD_TAB_t saCmsEntBand = { &saCmsBand, VTX_SMARTAUDIO_BAND_COUNT, vtx58BandNames }; -static const char * const saCmsPowerNames[] = { - "---", - "25 ", - "200", - "500", - "800", -}; +static const OSD_TAB_t saCmsEntChan = { &saCmsChan, VTX_SMARTAUDIO_CHANNEL_COUNT, vtx58ChannelNames }; -static const OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames}; +static const OSD_TAB_t saCmsEntPower = { &saCmsPower, VTX_SMARTAUDIO_POWER_COUNT, saPowerNames}; static const char * const saCmsOpmodelNames[] = { "----", @@ -359,6 +398,7 @@ static const char * const saCmsFselModeNames[] = { }; static const char * const saCmsPitFModeNames[] = { + "---", "PIR", "POR" }; @@ -372,19 +412,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(NULL); return 0; @@ -395,12 +431,22 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); + const vtxSettingsConfig_t prevSettings = { + .band = vtxSettingsConfig()->band, + .channel = vtxSettingsConfig()->channel, + .freq = vtxSettingsConfig()->freq, + .power = vtxSettingsConfig()->power, + .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm, + }; + vtxSettingsConfig_t newSettings = prevSettings; + if (saCmsOpmodel == SACMS_OPMODEL_RACE) { // Race model // Setup band, freq and power. - saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1); - + newSettings.band = saCmsBand; + newSettings.channel = saCmsChan; + newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan); // If in pit mode, cancel it. if (saCmsPitFMode == 0) @@ -410,13 +456,26 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) } else { // Freestyle model // Setup band and freq / user freq - if (saCmsFselMode == 0) - saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1); - else - saSetFreq(saCmsUserFreq); + if (saCmsFselModeNew == 0) { + newSettings.band = saCmsBand; + newSettings.channel = saCmsChan; + newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan); + } else { + saSetMode(0); //make sure FREE mode is setup + newSettings.band = 0; + newSettings.freq = saCmsUserFreq; + } } - saSetPowerByIndex(saCmsPower - 1); + newSettings.power = saCmsPower; + + if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) { + vtxSettingsConfigMutable()->band = newSettings.band; + vtxSettingsConfigMutable()->channel = newSettings.channel; + vtxSettingsConfigMutable()->power = newSettings.power; + vtxSettingsConfigMutable()->freq = newSettings.freq; + saveConfigAndNotify(); + } return MENU_CHAIN_BACK; } @@ -425,6 +484,9 @@ static long saCmsSetPORFreqOnEnter(const OSD_Entry *from) { UNUSED(from); + if (saDevice.version == 1) + return MENU_CHAIN_BACK; + saCmsORFreqNew = saCmsORFreq; return 0; @@ -435,7 +497,7 @@ static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT); + saSetPitFreq(saCmsORFreqNew); return 0; } @@ -473,7 +535,6 @@ static long saCmsConfigUserFreq(displayPort_t *pDisp, const void *self) UNUSED(self); saCmsUserFreq = saCmsUserFreqNew; - //saSetFreq(saCmsUserFreq); return MENU_CHAIN_BACK; } @@ -526,7 +587,7 @@ static const CMS_Menu saCmsMenuUserFreq = .entries = saCmsMenuUserFreqEntries, }; -static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames }; +static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselModeNew, 1, saCmsFselModeNames }; static const OSD_Entry saCmsMenuConfigEntries[] = { @@ -536,7 +597,9 @@ static const OSD_Entry saCmsMenuConfigEntries[] = { "FSEL MODE", OME_TAB, {.func = saCmsConfigFreqModeByGvar}, &saCmsEntFselMode, DYNAMIC }, OSD_TAB_CALLBACK_ENTRY("PIT FMODE", saCmsConfigPitFModeByGvar, &saCmsEntPitFMode), { "POR FREQ", OME_Submenu, {.menufunc = saCmsORFreqGetString}, (void *)&saCmsMenuPORFreq, OPTSTRING }, +#ifdef USE_EXTENDED_CMS_MENUS OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats), +#endif /* USE_EXTENDED_CMS_MENUS */ OSD_BACK_ENTRY, OSD_END_ENTRY, @@ -602,7 +665,7 @@ static const OSD_Entry saCmsMenuChanModeEntries[] = OSD_TAB_CALLBACK_ENTRY("CHAN", saCmsConfigChanByGvar, &saCmsEntChan), OSD_UINT16_RO_ENTRY("(FREQ)", &saCmsFreqRef), OSD_TAB_CALLBACK_ENTRY("POWER", saCmsConfigPowerByGvar, &saCmsEntPower), - OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence), + OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence), OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig), OSD_BACK_ENTRY, @@ -614,7 +677,9 @@ static const OSD_Entry saCmsMenuOfflineEntries[] = OSD_LABEL_ENTRY("- VTX SMARTAUDIO -"), OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString), +#ifdef USE_EXTENDED_CMS_MENUS OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats), +#endif /* USE_EXTENDED_CMS_MENUS */ OSD_BACK_ENTRY, OSD_END_ENTRY, @@ -627,7 +692,7 @@ static long sacms_SetupTopMenu(const OSD_Entry *from) UNUSED(from); 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 b660f863f54..d8aeea3ab58 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.h +++ b/src/main/cms/cms_menu_vtx_smartaudio.h @@ -1,18 +1,21 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * along with this software. + * + * If not, see . */ #pragma once diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index c67b1d01f48..735d480339e 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/src/main/cms/cms_menu_vtx_tramp.c @@ -1,27 +1,31 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * along with this software. + * + * If not, see . */ +#include #include #include #include #include "platform.h" -#if defined(USE_CMS) && defined(VTX_TRAMP) +#if defined(USE_CMS) && defined(USE_VTX_TRAMP) #include "common/printf.h" #include "common/utils.h" @@ -35,6 +39,7 @@ #include "io/vtx_string.h" #include "io/vtx_tramp.h" +#include "io/vtx.h" static bool trampCmsDrawStatusString(char *buf, unsigned bufsize) { @@ -48,7 +53,8 @@ static bool trampCmsDrawStatusString(char *buf, unsigned bufsize) strcpy(buf, defaultString); - if (!trampIsAvailable()) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_TRAMP || !vtxCommonDeviceIsReady(vtxDevice)) { return true; } @@ -65,8 +71,7 @@ static bool trampCmsDrawStatusString(char *buf, unsigned bufsize) if (trampData.power) { tfp_sprintf(&buf[9], " %c%3d", (trampData.power == trampData.configuredPower) ? ' ' : '*', trampData.power); - } - else { + } else { tfp_sprintf(&buf[9], " ----"); } @@ -78,13 +83,13 @@ uint8_t trampCmsBand = 1; uint8_t trampCmsChan = 1; uint16_t trampCmsFreqRef; -static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames }; +static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, VTX_TRAMP_BAND_COUNT, vtx58BandNames }; -static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames }; +static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, VTX_TRAMP_CHANNEL_COUNT, vtx58ChannelNames }; static uint8_t trampCmsPower = 1; -static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames }; +static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, sizeof(trampPowerTable), trampPowerNames }; static void trampCmsUpdateFreqRef(void) { @@ -164,20 +169,27 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) // If it fails, the user should retry later trampCommitChanges(); + // update'vtx_' settings + vtxSettingsConfigMutable()->band = trampCmsBand; + vtxSettingsConfigMutable()->channel = trampCmsChan; + vtxSettingsConfigMutable()->power = trampCmsPower; + vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(trampCmsBand, trampCmsChan); + + saveConfigAndNotify(); return MENU_CHAIN_BACK; } static void trampCmsInitSettings(void) { - if(trampData.band > 0) trampCmsBand = trampData.band; - if(trampData.channel > 0) trampCmsChan = trampData.channel; + if (trampData.band > 0) trampCmsBand = trampData.band; + if (trampData.channel > 0) trampCmsChan = trampData.channel; trampCmsUpdateFreqRef(); trampCmsPitMode = trampData.pitMode + 1; if (trampData.configuredPower > 0) { - for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { + for (uint8_t i = 0; i < VTX_TRAMP_POWER_COUNT; i++) { if (trampData.configuredPower <= trampPowerTable[i]) { trampCmsPower = i + 1; break; @@ -194,7 +206,8 @@ static long trampCmsOnEnter(const OSD_Entry *from) return 0; } -static const OSD_Entry trampCmsMenuCommenceEntries[] = { +static const OSD_Entry trampCmsMenuCommenceEntries[] = +{ OSD_LABEL_ENTRY("CONFIRM"), OSD_FUNC_CALL_ENTRY("YES", trampCmsCommence), diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 5993e1e908a..a3e23b392b1 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -81,6 +81,9 @@ //#define PG_DRIVER_PWM_RX_CONFIG 100 //#define PG_DRIVER_FLASHCHIP_CONFIG 101 +// cleanflight v2 specific parameter group ids start at 256 +#define PG_VTX_SETTINGS_CONFIG 259 + // iNav specific parameter group ids start at 1000 #define PG_INAV_START 1000 #define PG_PITOTMETER_CONFIG 1000 diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index abfdd9f9eaf..29364228bd4 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -41,7 +41,8 @@ typedef enum portOptions_t { * to actual data bytes. */ SERIAL_BIDIR_OD = 0 << 4, - SERIAL_BIDIR_PP = 1 << 4 + SERIAL_BIDIR_PP = 1 << 4, + SERIAL_BIDIR_NOPULL = 1 << 5, // disable pulls in BIDIR RX mode } portOptions_t; typedef void (*serialReceiveCallbackPtr)(uint16_t data, void *rxCallbackData); // used by serial drivers to return frames to app diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index b73d9aa8a69..e9f77cbb109 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -130,9 +130,11 @@ static void serialEnableCC(softSerial_t *softSerial) static void serialInputPortActivate(softSerial_t *softSerial) { if (softSerial->port.options & SERIAL_INVERTED) { - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction); + const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_PD; + IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction); } else { - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction); + const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_UP; + IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction); } softSerial->rxActive = true; diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index dfb45af32a0..8893dfd8177 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -25,42 +25,52 @@ #include "platform.h" #include "build/debug.h" -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) #include "vtx_common.h" -vtxDevice_t *vtxDevice = NULL; +static vtxDevice_t *commonVtxDevice = NULL; void vtxCommonInit(void) { } -// Whatever registered last will win - -void vtxCommonRegisterDevice(vtxDevice_t *pDevice) +void vtxCommonSetDevice(vtxDevice_t *vtxDevice) { - vtxDevice = pDevice; + commonVtxDevice = vtxDevice; } -void vtxCommonProcess(timeUs_t currentTimeUs) +vtxDevice_t *vtxCommonDevice(void) { - if (!vtxDevice) - return; + return commonVtxDevice; +} - if (vtxDevice->vTable->process) - vtxDevice->vTable->process(currentTimeUs); +void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + if (vtxDevice && vtxDevice->vTable->process) { + vtxDevice->vTable->process(vtxDevice, currentTimeUs); + } } -vtxDevType_e vtxCommonGetDeviceType(void) +vtxDevType_e vtxCommonGetDeviceType(vtxDevice_t *vtxDevice) { - if (!vtxDevice || !vtxDevice->vTable->getDeviceType) + if (!vtxDevice || !vtxDevice->vTable->getDeviceType) { return VTXDEV_UNKNOWN; + } - return vtxDevice->vTable->getDeviceType(); + return vtxDevice->vTable->getDeviceType(vtxDevice); +} + +bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice) +{ + if (vtxDevice && vtxDevice->vTable->isReady) { + return vtxDevice->vTable->isReady(vtxDevice); + } + return false; } // band and channel are 1 origin -void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel) +void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { if (!vtxDevice) return; @@ -68,12 +78,13 @@ void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel) if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) return; - if (vtxDevice->vTable->setBandAndChannel) - vtxDevice->vTable->setBandAndChannel(band, channel); + if (vtxDevice->vTable->setBandAndChannel) { + vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel); + } } // index is zero origin, zero = power off completely -void vtxCommonSetPowerByIndex(uint8_t index) +void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { if (!vtxDevice) return; @@ -81,59 +92,64 @@ void vtxCommonSetPowerByIndex(uint8_t index) if (index > vtxDevice->capability.powerCount) return; - if (vtxDevice->vTable->setPowerByIndex) - vtxDevice->vTable->setPowerByIndex(index); + if (vtxDevice->vTable->setPowerByIndex) { + vtxDevice->vTable->setPowerByIndex(vtxDevice, index); + } } // on = 1, off = 0 -void vtxCommonSetPitMode(uint8_t onoff) +void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { - if (!vtxDevice) - return; - - if (vtxDevice->vTable->setPitMode) - vtxDevice->vTable->setPitMode(onoff); + if (vtxDevice && vtxDevice->vTable->setPitMode) { + vtxDevice->vTable->setPitMode(vtxDevice, onoff); + } } -bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency) { - if (!vtxDevice) - return false; - - if (vtxDevice->vTable->getBandAndChannel) - return vtxDevice->vTable->getBandAndChannel(pBand, pChannel); - else - return false; + if (vtxDevice && vtxDevice->vTable->setFrequency) { + vtxDevice->vTable->setFrequency(vtxDevice, frequency); + } } -bool vtxCommonGetPowerIndex(uint8_t *pIndex) +bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (!vtxDevice) - return false; - - if (vtxDevice->vTable->getPowerIndex) - return vtxDevice->vTable->getPowerIndex(pIndex); - else - return false; + if (vtxDevice && vtxDevice->vTable->getBandAndChannel) { + return vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel); + } + return false; } -bool vtxCommonGetPitMode(uint8_t *pOnOff) +bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxDevice) - return false; + if (vtxDevice && vtxDevice->vTable->getPowerIndex) { + return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex); + } + return false; +} - if (vtxDevice->vTable->getPitMode) - return vtxDevice->vTable->getPitMode(pOnOff); - else - return false; +bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff) +{ + if (vtxDevice && vtxDevice->vTable->getPitMode) { + return vtxDevice->vTable->getPitMode(vtxDevice, pOnOff); + } + return false; } -bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability) +bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency) { - if (!vtxDevice) - return false; + if (vtxDevice && vtxDevice->vTable->getFrequency) { + return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency); + } + return false; +} - memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t)); - return true; +bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability) +{ + if (vtxDevice) { + memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t)); + return true; + } + return false; } #endif diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 8975f78c837..b8d9cec880d 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -19,6 +19,53 @@ #include "common/time.h" +#define VTX_SETTINGS_NO_BAND 0 // used for custom frequency selection mode +#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 +#define VTX_SETTINGS_DEFAULT_CHANNEL 1 +#define VTX_SETTINGS_DEFAULT_FREQ 5740 +#define VTX_SETTINGS_DEFAULT_PITMODE_FREQ 0 +#define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0 + +#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting +#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting + +#if defined(USE_VTX_RTC6705) + +#include "drivers/vtx_rtc6705.h" + +#endif + +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) + +#define VTX_SETTINGS_POWER_COUNT 5 +#define VTX_SETTINGS_DEFAULT_POWER 1 +#define VTX_SETTINGS_MIN_POWER 0 +#define VTX_SETTINGS_MIN_USER_FREQ 5000 +#define VTX_SETTINGS_MAX_USER_FREQ 5999 +#define VTX_SETTINGS_FREQCMD + +#elif defined(USE_VTX_RTC6705) + +#define VTX_SETTINGS_POWER_COUNT VTX_RTC6705_POWER_COUNT +#define VTX_SETTINGS_DEFAULT_POWER VTX_RTC6705_DEFAULT_POWER +#define VTX_SETTINGS_MIN_POWER VTX_RTC6705_MIN_POWER + +#endif + +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - 1) + +// 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, @@ -58,17 +105,19 @@ typedef struct vtxDevice_s { // {set,get}PitMode: 0 = OFF, 1 = ON typedef struct vtxVTable_s { - void (*process)(uint32_t currentTimeUs); - vtxDevType_e (*getDeviceType)(void); - bool (*isReady)(void); - - void (*setBandAndChannel)(uint8_t band, uint8_t channel); - void (*setPowerByIndex)(uint8_t level); - void (*setPitMode)(uint8_t onoff); - - bool (*getBandAndChannel)(uint8_t *pBand, uint8_t *pChannel); - bool (*getPowerIndex)(uint8_t *pIndex); - bool (*getPitMode)(uint8_t *pOnOff); + void (*process)(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); + vtxDevType_e (*getDeviceType)(const vtxDevice_t *vtxDevice); + bool (*isReady)(const vtxDevice_t *vtxDevice); + + void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); + void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level); + void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff); + void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq); + + bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); + bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex); + bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff); + bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq); } vtxVTable_t; // 3.1.0 @@ -77,15 +126,19 @@ typedef struct vtxVTable_s { // - It is *NOT* RF on/off control ? void vtxCommonInit(void); -void vtxCommonRegisterDevice(vtxDevice_t *pDevice); +void vtxCommonSetDevice(vtxDevice_t *vtxDevice); +vtxDevice_t *vtxCommonDevice(void); // VTable functions -void vtxCommonProcess(timeUs_t currentTimeUs); -uint8_t vtxCommonGetDeviceType(void); -void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel); -void vtxCommonSetPowerByIndex(uint8_t level); -void vtxCommonSetPitMode(uint8_t onoff); -bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel); -bool vtxCommonGetPowerIndex(uint8_t *pIndex); -bool vtxCommonGetPitMode(uint8_t *pOnOff); -bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability); +void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); +vtxDevType_e vtxCommonGetDeviceType(vtxDevice_t *vtxDevice); +bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice); +void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); +void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index); +void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff); +void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency); +bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); +bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex); +bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff); +bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq); +bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability); diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 3471d928ddf..591f22efc3b 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -27,7 +27,7 @@ #include "platform.h" -#if defined(VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI) +#if defined(USE_VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI) #include "common/maths.h" @@ -118,7 +118,7 @@ static IO_t vtxCLKPin = IO_NONE; // Define variables -static const uint32_t channelArray[RTC6705_BAND_COUNT][RTC6705_CHANNEL_COUNT] = { +static const uint32_t channelArray[VTX_RTC6705_BAND_COUNT][VTX_RTC6705_CHANNEL_COUNT] = { { RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 }, { RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 }, { RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 }, @@ -200,8 +200,8 @@ static void rtc6705Transfer(uint32_t command) */ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel) { - band = constrain(band, 0, RTC6705_BAND_COUNT - 1); - channel = constrain(channel, 0, RTC6705_CHANNEL_COUNT - 1); + band = constrain(band, 0, VTX_RTC6705_BAND_COUNT - 1); + channel = constrain(channel, 0, VTX_RTC6705_CHANNEL_COUNT - 1); spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); @@ -215,7 +215,7 @@ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel) */ void rtc6705SetFreq(uint16_t frequency) { - frequency = constrain(frequency, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX); + frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX); uint32_t val_hex = 0; @@ -235,7 +235,7 @@ void rtc6705SetFreq(uint16_t frequency) void rtc6705SetRFPower(uint8_t rf_power) { - rf_power = constrain(rf_power, 0, RTC6705_RF_POWER_COUNT - 1); + rf_power = constrain(rf_power, 0, VTX_RTC6705_POWER_COUNT - 1); spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h index 3baa5945734..7a419386075 100644 --- a/src/main/drivers/vtx_rtc6705.h +++ b/src/main/drivers/vtx_rtc6705.h @@ -26,14 +26,21 @@ #include -#define RTC6705_BAND_COUNT 5 -#define RTC6705_CHANNEL_COUNT 8 -#define RTC6705_RF_POWER_COUNT 2 +#define VTX_RTC6705_BAND_COUNT 5 +#define VTX_RTC6705_CHANNEL_COUNT 8 +#define VTX_RTC6705_POWER_COUNT 3 +#define VTX_RTC6705_DEFAULT_POWER 1 -#define RTC6705_FREQ_MIN 5600 -#define RTC6705_FREQ_MAX 5950 +#if defined(RTC6705_POWER_PIN) +#define VTX_RTC6705_MIN_POWER 0 +#else +#define VTX_RTC6705_MIN_POWER 1 +#endif -#define RTC6705_BOOT_DELAY 350 // milliseconds +#define VTX_RTC6705_FREQ_MIN 5600 +#define VTX_RTC6705_FREQ_MAX 5950 + +#define VTX_RTC6705_BOOT_DELAY 350 // milliseconds void rtc6705IOInit(void); void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel); diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c index 0acd4e683ee..d7795e6c475 100644 --- a/src/main/drivers/vtx_rtc6705_soft_spi.c +++ b/src/main/drivers/vtx_rtc6705_soft_spi.c @@ -20,7 +20,7 @@ #include "platform.h" -#if defined(VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI) +#if defined(USE_VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI) #include "drivers/bus_spi.h" #include "drivers/io.h" diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index cd95f9098e7..5ef50fcd264 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -103,6 +103,7 @@ #include "io/rcdevice_cam.h" #include "io/serial.h" #include "io/displayport_msp.h" +#include "io/vtx.h" #include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" @@ -653,20 +654,23 @@ void init(void) pitotStartCalibration(); #endif -#ifdef VTX_CONTROL +#ifdef USE_VTX_CONTROL vtxControlInit(); +#if defined(USE_VTX_COMMON) vtxCommonInit(); + vtxInit(); +#endif -#ifdef VTX_SMARTAUDIO +#ifdef USE_VTX_SMARTAUDIO vtxSmartAudioInit(); #endif -#ifdef VTX_TRAMP +#ifdef USE_VTX_TRAMP vtxTrampInit(); #endif -#endif // VTX_CONTROL +#endif // USE_VTX_CONTROL // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f4c94ec2b45..d6da1e65ceb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -76,6 +76,8 @@ #include "io/osd.h" #include "io/serial.h" #include "io/serial_4way.h" +#include "io/vtx.h" +#include "io/vtx_string.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -1266,30 +1268,33 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) case MSP_VTX_CONFIG: { - uint8_t deviceType = vtxCommonGetDeviceType(); - if (deviceType != VTXDEV_UNKNOWN) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { - uint8_t band=0, channel=0; - vtxCommonGetBandAndChannel(&band,&channel); + uint8_t deviceType = 0; + vtxCommonGetDeviceType(vtxDevice); - uint8_t powerIdx=0; // debug - vtxCommonGetPowerIndex(&powerIdx); - - uint8_t pitmode=0; - vtxCommonGetPitMode(&pitmode); + // Return band, channel and power from vtxSettingsConfig_t + // since the VTX might be configured but temporarily offline. + uint8_t pitmode = 0; + vtxCommonGetPitMode(vtxDevice, &pitmode); sbufWriteU8(dst, deviceType); - sbufWriteU8(dst, band); - sbufWriteU8(dst, channel); - sbufWriteU8(dst, powerIdx); + sbufWriteU8(dst, vtxSettingsConfig()->band); + sbufWriteU8(dst, vtxSettingsConfig()->channel); + sbufWriteU8(dst, vtxSettingsConfig()->power); sbufWriteU8(dst, pitmode); + + // Betaflight doesn't send these fields + sbufWriteU8(dst, vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0); + sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); // future extensions here... } else { - sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected + sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured } } break; @@ -2128,36 +2133,44 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif // USE_OSD -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) case MSP_SET_VTX_CONFIG: - if (dataSize >= 4) { - tmp_u16 = sbufReadU16(src); - const uint8_t band = (tmp_u16 / 8) + 1; - const uint8_t channel = (tmp_u16 % 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) - break; - - uint8_t power = sbufReadU8(src); - uint8_t current_power = 0; - vtxCommonGetPowerIndex(¤t_power); - if (current_power != power) - vtxCommonSetPowerByIndex(power); - - uint8_t pitmode = sbufReadU8(src); - uint8_t current_pitmode = 0; - vtxCommonGetPitMode(¤t_pitmode); - if (current_pitmode != pitmode) - vtxCommonSetPitMode(pitmode); + if (dataSize >= 2) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) { + uint16_t newFrequency = sbufReadU16(src); + if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel + const uint8_t newBand = (newFrequency / 8) + 1; + const uint8_t newChannel = (newFrequency % 8) + 1; + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; + vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel); + } else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { //value is frequency in MHz. Ignore it if it's invalid + vtxSettingsConfigMutable()->band = 0; + vtxSettingsConfigMutable()->channel = 0; + vtxSettingsConfigMutable()->freq = newFrequency; + } + + if (sbufBytesRemaining(src) > 1) { + vtxSettingsConfigMutable()->power = sbufReadU8(src); + // Delegate pitmode to vtx directly + const uint8_t newPitmode = sbufReadU8(src); + uint8_t currentPitmode = 0; + vtxCommonGetPitMode(vtxDevice, ¤tPitmode); + if (currentPitmode != newPitmode) { + vtxCommonSetPitMode(vtxDevice, newPitmode); + } + + if (sbufBytesRemaining(src) > 0) { + vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); + } + } + } } - } else + } else { return MSP_RESULT_ERROR; + } break; #endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 95eeed53f1f..cc273df19a6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -32,7 +32,6 @@ #include "drivers/sensor.h" #include "drivers/serial.h" #include "drivers/stack_check.h" -#include "drivers/vtx_common.h" #include "fc/cli.h" #include "fc/config.h" @@ -58,6 +57,7 @@ #include "io/pwmdriver_i2c.h" #include "io/serial.h" #include "io/rcdevice_cam.h" +#include "io/vtx.h" #include "msp/msp_serial.h" @@ -273,19 +273,6 @@ void taskUpdateOsd(timeUs_t currentTimeUs) } #endif -#ifdef VTX_CONTROL -// Everything that listens to VTX devices -void taskVtxControl(timeUs_t currentTimeUs) -{ - if (ARMING_FLAG(ARMED)) - return; - -#ifdef VTX_COMMON - vtxCommonProcess(currentTimeUs); -#endif -} -#endif - void fcTasksInit(void) { schedulerInit(); @@ -369,8 +356,8 @@ void fcTasksInit(void) #ifdef USE_OPTICAL_FLOW setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW)); #endif -#ifdef VTX_CONTROL -#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP) +#ifdef USE_VTX_CONTROL +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif @@ -616,10 +603,10 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef VTX_CONTROL +#ifdef USE_VTX_CONTROL [TASK_VTXCTRL] = { .taskName = "VTXCTRL", - .taskFunc = taskVtxControl, + .taskFunc = vtxUpdate, .desiredPeriod = TASK_PERIOD_HZ(5), // 5Hz @200msec .staticPriority = TASK_PRIORITY_IDLE, }, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4dab9437cf1..8f978b11685 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -67,7 +67,9 @@ tables: - name: i2c_speed values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] - name: debug_modes - values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME"] + values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", + "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", + "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -97,6 +99,9 @@ tables: - name: tz_automatic_dst values: ["OFF", "EU", "USA"] enum: tz_automatic_dst_e + - name: vtx_low_power_disarm + values: ["OFF", "ON", "UNTIL_FIRST_ARM"] + enum: vtxLowerPowerDisarm_e groups: - name: PG_GYRO_CONFIG @@ -1635,3 +1640,43 @@ groups: - name: display_force_sw_blink field: force_sw_blink type: bool + + - name: PG_VTX_CONFIG + type: vtxConfig_t + headers: ["io/vtx_control.h"] + condition: USE_VTX_CONTROL && USE_VTX_COMMON + members: + - name: vtx_halfduplex + field: halfDuplex + type: bool + + - name: PG_VTX_SETTINGS_CONFIG + type: vtxSettingsConfig_t + headers: ["drivers/vtx_common.h", "io/vtx.h"] + members: + - name: vtx_band + field: band + min: VTX_SETTINGS_NO_BAND + max: VTX_SETTINGS_MAX_BAND + - name: vtx_channel + field: channel + min: VTX_SETTINGS_MIN_CHANNEL + max: VTX_SETTINGS_MAX_CHANNEL + - name: vtx_power + field: power + min: VTX_SETTINGS_MIN_POWER + max: VTX_SETTINGS_MAX_POWER + - name: vtx_low_power_disarm + field: lowPowerDisarm + table: vtx_low_power_disarm + type: uint8_t + - name: vtx_freq + field: freq + min: VTX_SETTINGS_MIN_FREQUENCY_MHZ + max: VTX_SETTINGS_MAX_FREQUENCY_MHZ + condition: VTX_SETTINGS_FREQCMD + - name: vtx_pit_mode_freq + field: pitModeFreq + min: VTX_SETTINGS_MIN_FREQUENCY_MHZ + max: VTX_SETTINGS_MAX_FREQUENCY_MHZ + condition: VTX_SETTINGS_FREQCMD \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ee4b78c3a11..e89812511a7 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1351,7 +1351,7 @@ static bool osdDrawSingleElement(uint8_t item) break; } -#if defined(VTX) || defined(VTX_COMMON) +#if defined(VTX) || defined(USE_VTX_COMMON) case OSD_VTX_CHANNEL: #if defined(VTX) // FIXME: This doesn't actually work. It's for boards with @@ -1361,8 +1361,21 @@ static bool osdDrawSingleElement(uint8_t item) { uint8_t band = 0; uint8_t channel = 0; - vtxCommonGetBandAndChannel(&band, &channel); - tfp_sprintf(buff, "CH:%c%s", vtx58BandLetter[band], vtx58ChannelNames[channel]); + uint8_t powerIndex = 0; + char bandChr = '-'; + const char *channelStr = "-"; + char powerChr = '-'; + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + if (vtxCommonGetBandAndChannel(vtxDevice, &band, &channel)) { + bandChr = vtx58BandLetter[band]; + channelStr = vtx58ChannelNames[channel]; + } + if (vtxCommonGetPowerIndex(vtxDevice, &powerIndex)) { + powerChr = '0' + powerIndex; + } + } + tfp_sprintf(buff, "CH:%c%s:%c", bandChr, channelStr, powerChr); } #endif break; diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c new file mode 100644 index 00000000000..1dc0e0d50aa --- /dev/null +++ b/src/main/io/vtx.c @@ -0,0 +1,279 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_VTX_COMMON) + +#include "common/maths.h" +#include "common/time.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/vtx_common.h" + +#include "fc/cli.h" +#include "fc/config.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/failsafe.h" + +#include "io/vtx.h" +#include "io/vtx_string.h" +#include "io/vtx_control.h" + +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 = VTX_SETTINGS_DEFAULT_FREQ, + .pitModeFreq = VTX_SETTINGS_DEFAULT_PITMODE_FREQ, + .lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF, +); + +typedef enum { + VTX_PARAM_POWER = 0, + VTX_PARAM_BANDCHAN, + VTX_PARAM_PITMODE, + VTX_PARAM_CONFIRM, + VTX_PARAM_COUNT +} vtxScheduleParams_e; + +void vtxInit(void) +{ + bool settingsUpdated = false; + + // sync frequency in parameter group when band/channel are specified + const uint16_t freq = vtx58_Bandchan2Freq(vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) { + vtxSettingsConfigMutable()->freq = freq; + settingsUpdated = true; + } + +#if defined(VTX_SETTINGS_FREQCMD) + // constrain pit mode frequency + if (vtxSettingsConfig()->pitModeFreq) { + const uint16_t constrainedPitModeFreq = MAX(vtxSettingsConfig()->pitModeFreq, VTX_SETTINGS_MIN_USER_FREQ); + if (constrainedPitModeFreq != vtxSettingsConfig()->pitModeFreq) { + vtxSettingsConfigMutable()->pitModeFreq = constrainedPitModeFreq; + settingsUpdated = true; + } + } +#endif + + if (settingsUpdated) { + saveConfigAndNotify(); + } +} + +static vtxSettingsConfig_t vtxGetSettings(void) +{ + vtxSettingsConfig_t settings = { + .band = vtxSettingsConfig()->band, + .channel = vtxSettingsConfig()->channel, + .power = vtxSettingsConfig()->power, + .freq = vtxSettingsConfig()->freq, + .pitModeFreq = vtxSettingsConfig()->pitModeFreq, + .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm, + }; + +#if 0 +#if defined(VTX_SETTINGS_FREQCMD) + if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) { + settings.band = 0; + settings.freq = settings.pitModeFreq; + settings.power = VTX_SETTINGS_DEFAULT_POWER; + } +#endif +#endif + + if (!ARMING_FLAG(ARMED) && !failsafeIsActive() && + ((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) || + (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))) { + + settings.power = VTX_SETTINGS_DEFAULT_POWER; + } + + return settings; +} + +static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice) +{ + if(!ARMING_FLAG(ARMED)) { + uint8_t vtxBand; + uint8_t vtxChan; + if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) { + const vtxSettingsConfig_t settings = vtxGetSettings(); + if (vtxBand != settings.band || vtxChan != settings.channel) { + vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel); + return true; + } + } + } + return false; +} + +#if defined(VTX_SETTINGS_FREQCMD) +static bool vtxProcessFrequency(vtxDevice_t *vtxDevice) +{ + if(!ARMING_FLAG(ARMED)) { + uint16_t vtxFreq; + if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) { + const vtxSettingsConfig_t settings = vtxGetSettings(); + if (vtxFreq != settings.freq) { + vtxCommonSetFrequency(vtxDevice, settings.freq); + return true; + } + } + } + return false; +} +#endif + +static bool vtxProcessPower(vtxDevice_t *vtxDevice) +{ + uint8_t vtxPower; + if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) { + const vtxSettingsConfig_t settings = vtxGetSettings(); + if (vtxPower != settings.power) { + vtxCommonSetPowerByIndex(vtxDevice, settings.power); + return true; + } + } + return false; +} + +static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) +{ + uint8_t pitOnOff; + + bool currPmSwitchState = false; + static bool prevPmSwitchState = false; + + if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) { + + // Not supported on INAV yet. It might not be that useful. +#if 0 + currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE); +#endif + + if (currPmSwitchState != prevPmSwitchState) { + prevPmSwitchState = currPmSwitchState; + + if (currPmSwitchState) { +#if defined(VTX_SETTINGS_FREQCMD) + if (vtxSettingsConfig()->pitModeFreq) { + return false; + } +#endif + +#if 0 + if (isModeActivationConditionPresent(BOXVTXPITMODE)) { +#endif + if (0) { + if (!pitOnOff) { + vtxCommonSetPitMode(vtxDevice, true); + return true; + } + } + } else { + if (pitOnOff) { + vtxCommonSetPitMode(vtxDevice, false); + return true; + } + } + } + } + return false; +} + +static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice) +{ + const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings(); + vtxSettingsConfig_t vtxState = vtxSettingsState; + + if (vtxSettingsState.band) { + vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel); +#if defined(VTX_SETTINGS_FREQCMD) + } else { + vtxCommonGetFrequency(vtxDevice, &vtxState.freq); +#endif + } + + vtxCommonGetPowerIndex(vtxDevice, &vtxState.power); + + return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t)); +} + +void vtxUpdate(timeUs_t currentTimeUs) +{ + static uint8_t currentSchedule = 0; + + if (cliMode) { + return; + } + + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + // Check input sources for config updates + vtxControlInputPoll(); + + const uint8_t startingSchedule = currentSchedule; + bool vtxUpdatePending = false; + do { + switch (currentSchedule) { + case VTX_PARAM_POWER: + vtxUpdatePending = vtxProcessPower(vtxDevice); + break; + case VTX_PARAM_BANDCHAN: + if (vtxGetSettings().band) { + vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice); +#if defined(VTX_SETTINGS_FREQCMD) + } else { + vtxUpdatePending = vtxProcessFrequency(vtxDevice); +#endif + } + break; + case VTX_PARAM_PITMODE: + vtxUpdatePending = vtxProcessPitMode(vtxDevice); + break; + case VTX_PARAM_CONFIRM: + vtxUpdatePending = vtxProcessStateUpdate(vtxDevice); + break; + default: + break; + } + currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT; + } while (!vtxUpdatePending && currentSchedule != startingSchedule); + + if (!ARMING_FLAG(ARMED) || vtxUpdatePending) { + vtxCommonProcess(vtxDevice, currentTimeUs); + } + } +} + +#endif diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h new file mode 100644 index 00000000000..35500d799ff --- /dev/null +++ b/src/main/io/vtx.h @@ -0,0 +1,49 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#pragma once + +#include + +#include "platform.h" + +#include "common/time.h" + +#include "config/parameter_group.h" + +typedef enum { + VTX_LOW_POWER_DISARM_OFF = 0, + VTX_LOW_POWER_DISARM_ALWAYS = 1, + VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM = 2, // Set low power until arming for the first time +} vtxLowerPowerDisarm_e; + +typedef struct vtxSettingsConfig_s { + uint8_t band; // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Racebande + uint8_t channel; // 1-8 + uint8_t power; // 0 = lowest + uint16_t freq; // sets freq in MHz if band=0 + uint16_t pitModeFreq; // sets out-of-range pitmode frequency + uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e +} vtxSettingsConfig_t; + +PG_DECLARE(vtxSettingsConfig_t, vtxSettingsConfig); + +void vtxInit(void); +void vtxUpdate(timeUs_t currentTimeUs); diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index cb410be1c7a..6daebbe8e74 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -37,9 +37,14 @@ #include "io/vtx_control.h" -#if defined(VTX_CONTROL) && defined(VTX_COMMON) +#if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) -PG_REGISTER(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2); + +PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, +// .vtxChannelActivationConditions = { 0 }, + .halfDuplex = true, +); static uint8_t locked = 0; @@ -48,6 +53,13 @@ void vtxControlInit(void) // NOTHING TO DO } +void vtxControlInputPoll(void) +{ + // Check variuos input sources for VTX config updates + + // XXX: None supported in INAV +} + static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep) { if (ARMING_FLAG(ARMED)) { @@ -55,9 +67,12 @@ static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep) } if (!locked) { - uint8_t band = 0, channel = 0; - vtxCommonGetBandAndChannel(&band, &channel); - vtxCommonSetBandAndChannel(band + bandStep, channel + channelStep); + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + uint8_t band = 0, channel = 0; + vtxCommonGetBandAndChannel(vtxDevice, &band, &channel); + vtxCommonSetBandAndChannel(vtxDevice, band + bandStep, channel + channelStep); + } } } @@ -88,17 +103,20 @@ void vtxUpdateActivatedChannel(void) } if (!locked) { - static uint8_t lastIndex = -1; + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + static uint8_t lastIndex = -1; - for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { - const vtxChannelActivationCondition_t *vtxChannelActivationCondition = &vtxConfig()->vtxChannelActivationConditions[index]; + for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { + const vtxChannelActivationCondition_t *vtxChannelActivationCondition = &vtxConfig()->vtxChannelActivationConditions[index]; - if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range) - && index != lastIndex) { - lastIndex = index; + if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range) + && index != lastIndex) { + lastIndex = index; - vtxCommonSetBandAndChannel(vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel); - break; + vtxCommonSetBandAndChannel(vtxDevice, vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel); + break; + } } } } @@ -106,10 +124,16 @@ void vtxUpdateActivatedChannel(void) void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + + if (!vtxDevice) { + return; + } + uint8_t band = 0, channel = 0; vtxDeviceCapability_t capability; - bool haveAllNeededInfo = vtxCommonGetBandAndChannel(&band, &channel) && vtxCommonGetDeviceCapability(&capability); + bool haveAllNeededInfo = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel) && vtxCommonGetDeviceCapability(vtxDevice, &capability); if (!haveAllNeededInfo) { return; } @@ -128,15 +152,21 @@ void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep) newBand = capability.bandCount; } - vtxCommonSetBandAndChannel(newBand, newChannel); + vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel); } void vtxCyclePower(const uint8_t powerStep) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + + if (!vtxDevice) { + return; + } + uint8_t power = 0; vtxDeviceCapability_t capability; - bool haveAllNeededInfo = vtxCommonGetPowerIndex(&power) && vtxCommonGetDeviceCapability(&capability); + bool haveAllNeededInfo = vtxCommonGetPowerIndex(vtxDevice, &power) && vtxCommonGetDeviceCapability(vtxDevice, &capability); if (!haveAllNeededInfo) { return; } @@ -148,7 +178,7 @@ void vtxCyclePower(const uint8_t powerStep) newPower = capability.powerCount; } - vtxCommonSetPowerByIndex(newPower); + vtxCommonSetPowerByIndex(vtxDevice, newPower); } #endif diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 3715b60e00d..e39e447a9f0 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -30,11 +30,13 @@ typedef struct vtxChannelActivationCondition_s { typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; + uint8_t halfDuplex; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); void vtxControlInit(void); +void vtxControlInputPoll(void); void vtxIncrementBand(void); void vtxDecrementBand(void); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index b03c995196d..f9722affd10 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -1,18 +1,21 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * along with this software. + * + * If not, see . */ /* Created by jflyper */ @@ -20,64 +23,64 @@ #include #include #include +#include #include "platform.h" -#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) -#include "build/build_config.h" +#if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL) + #include "build/debug.h" #include "cms/cms.h" -#include "cms/cms_types.h" #include "cms/cms_menu_vtx_smartaudio.h" -#include "common/axis.h" +#include "common/maths.h" #include "common/printf.h" #include "common/utils.h" -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#include "drivers/system.h" -#include "drivers/serial.h" #include "drivers/time.h" #include "drivers/vtx_common.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "flight/pid.h" -#include "config/config_master.h" - #include "io/serial.h" +#include "io/vtx.h" +#include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_string.h" -//#define SMARTAUDIO_DEBUG_MONITOR +// Timing parameters +// Note that vtxSAProcess() is normally called at 200ms interval +#define SMARTAUDIO_CMD_TIMEOUT 120 // Time until the command is considered lost +#define SMARTAUDIO_POLLING_INTERVAL 150 // Minimum time between state polling +#define SMARTAUDIO_POLLING_WINDOW 1000 // Time window after command polling for state change + +//#define USE_SMARTAUDIO_DPRINTF +//#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1 + +#ifdef USE_SMARTAUDIO_DPRINTF +serialPort_t *debugSerialPort = NULL; +#endif // USE_SMARTAUDIO_DPRINTF static serialPort_t *smartAudioSerialPort = NULL; -#if defined(USE_CMS) || defined(VTX_COMMON) -static const char * const saPowerNames[] = { +#if defined(USE_CMS) || defined(USE_VTX_COMMON) +const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = { "---", "25 ", "200", "500", "800", }; #endif -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON static const vtxVTable_t saVTable; // Forward static vtxDevice_t vtxSmartAudio = { .vTable = &saVTable, - .capability = { - .bandCount = 5, - .channelCount = 8, - .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, }; -#endif // VTX_COMMON +#endif // SmartAudio command and response codes enum { @@ -93,10 +96,18 @@ enum { // This is not a good design; can't distinguish command from response this way. #define SACMD(cmd) (((cmd) << 1) | 1) + #define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE) #define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE)) #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 = { @@ -109,7 +120,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 }, @@ -169,9 +180,9 @@ static uint8_t CRC8(const uint8_t *data, const int8_t len) } +#ifdef USE_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")); @@ -184,18 +195,17 @@ static void saPrintSettings(void) dprintf(("power: %d ", saDevice.power)); dprintf(("pitfreq: %d ", saDevice.orfreq)); dprintf(("\r\n")); -#endif } +#endif int saDacToPowerIndex(int dac) { - int idx; - - for (idx = 3 ; idx >= 0 ; idx--) { - if (saPowerTable[idx].valueV1 <= dac) - return(idx); + for (int idx = 3 ; idx >= 0 ; idx--) { + if (saPowerTable[idx].valueV1 <= dac) { + return idx; + } } - return(0); + return 0; } // @@ -208,13 +218,12 @@ uint16_t sa_smartbaud = SMARTBAUD_MIN; static int sa_adjdir = 1; // -1=going down, 1=going up static int sa_baudstep = 50; -#define SMARTAUDIO_CMD_TIMEOUT 120 - static void saAutobaud(void) { - if (saStat.pktsent < 10) + if (saStat.pktsent < 10) { // Not enough samples collected return; + } #if 0 dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", @@ -250,7 +259,7 @@ static void saAutobaud(void) // Transport level variables -static uint32_t sa_lastTransmission = 0; +static timeUs_t sa_lastTransmissionMs = 0; 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 @@ -268,11 +277,12 @@ static void saProcessResponse(uint8_t *buf, int len) dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp)); } - switch(resp) { + switch (resp) { case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings - if (len < 7) + if (len < 7) { break; + } saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; saDevice.channel = buf[2]; @@ -280,12 +290,10 @@ static void saProcessResponse(uint8_t *buf, int len) saDevice.mode = buf[4]; saDevice.freq = (buf[5] << 8)|buf[6]; -#ifdef SMARTAUDIO_DEBUG_MONITOR - debug[0] = saDevice.version * 100 + saDevice.mode; - debug[1] = saDevice.channel; - debug[2] = saDevice.freq; - debug[3] = saDevice.power; -#endif + DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode); + DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel); + DEBUG_SET(DEBUG_SMARTAUDIO, 2, saDevice.freq); + DEBUG_SET(DEBUG_SMARTAUDIO, 3, saDevice.power); break; case SA_CMD_SET_POWER: // Set Power @@ -295,10 +303,11 @@ static void saProcessResponse(uint8_t *buf, int len) break; case SA_CMD_SET_FREQ: // Set Frequency - if (len < 5) + if (len < 5) { break; + } - uint16_t freq = (buf[2] << 8)|buf[3]; + const uint16_t freq = (buf[2] << 8)|buf[3]; if (freq & SA_FREQ_GETPIT) { saDevice.orfreq = freq & SA_FREQ_MASK; @@ -321,12 +330,17 @@ static void saProcessResponse(uint8_t *buf, int len) return; } - // Debug - if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) + if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) { +#ifdef USE_CMS //if changes then trigger saCms update + saCmsResetOpmodel(); +#endif +#ifdef USE_SMARTAUDIO_DPRINTF // Debug saPrintSettings(); +#endif + } saDevicePrev = saDevice; -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON // Todo: Update states in saVtxDevice? #endif @@ -355,7 +369,7 @@ static void saReceiveFramer(uint8_t c) static int len; static int dlen; - switch(state) { + switch (state) { case S_WAITPRE1: if (c == 0xAA) { state = S_WAITPRE2; @@ -420,16 +434,20 @@ static void saReceiveFramer(uint8_t c) static void saSendFrame(uint8_t *buf, int len) { - int i; - - serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit + switch (smartAudioSerialPort->identifier) { + case SERIAL_PORT_SOFTSERIAL1: + case SERIAL_PORT_SOFTSERIAL2: + break; + default: + serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit + break; + } - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { serialWrite(smartAudioSerialPort, buf[i]); + } - serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this - - sa_lastTransmission = millis(); + sa_lastTransmissionMs = millis(); saStat.pktsent++; } @@ -460,10 +478,9 @@ static void saResendCmd(void) static void saSendCmd(uint8_t *buf, int len) { - int i; - - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { sa_osbuf[i] = buf[i]; + } sa_oslen = len; sa_outstanding = (buf[2] >> 1); @@ -478,22 +495,11 @@ typedef struct saCmdQueue_s { int len; } saCmdQueue_t; -#define SA_QSIZE 4 // 1 heartbeat (GetSettings) + 2 commands + 1 slack +#define SA_QSIZE 6 // 1 heartbeat (GetSettings) + 2 commands + 1 slack static saCmdQueue_t sa_queue[SA_QSIZE]; static uint8_t sa_qhead = 0; static uint8_t sa_qtail = 0; -#ifdef DPRINTF_SMARTAUDIO -static int saQueueLength(void) -{ - if (sa_qhead >= sa_qtail) { - return sa_qhead - sa_qtail; - } else { - return SA_QSIZE + sa_qhead - sa_qtail; - } -} -#endif - static bool saQueueEmpty(void) { return sa_qhead == sa_qtail; @@ -506,8 +512,9 @@ static bool saQueueFull(void) static void saQueueCmd(uint8_t *buf, int len) { - if (saQueueFull()) + if (saQueueFull()) { return; + } sa_queue[sa_qhead].buf = buf; sa_queue[sa_qhead].len = len; @@ -516,8 +523,9 @@ static void saQueueCmd(uint8_t *buf, int len) static void saSendQueue(void) { - if (saQueueEmpty()) + if (saQueueEmpty()) { return; + } saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len); sa_qtail = (sa_qtail + 1) % SA_QSIZE; @@ -532,9 +540,15 @@ 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 }; + static uint8_t switchBuf[7]; if (freq & SA_FREQ_GETPIT) { dprintf(("smartAudioSetFreq: GETPIT\r\n")); @@ -548,31 +562,60 @@ void saSetFreq(uint16_t freq) buf[5] = freq & 0xff; buf[6] = CRC8(buf, 6); + // 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) { + memcpy(&switchBuf, &buf, sizeof(buf)); + const uint16_t switchFreq = freq + ((freq == VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ) ? -1 : 1); + switchBuf[4] = (switchFreq >> 8); + switchBuf[5] = switchFreq & 0xff; + switchBuf[6] = CRC8(switchBuf, 6); + + saQueueCmd(switchBuf, 7); + } + saQueueCmd(buf, 7); } -#if 0 -static void saSetPitFreq(uint16_t freq) +void saSetFreq(uint16_t freq) +{ + saDoDevSetFreq(freq); +} + +void saSetPitFreq(uint16_t freq) { - saSetFreq(freq | SA_FREQ_SETPIT); + 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); +} + void saSetMode(int mode) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; @@ -583,7 +626,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 }; @@ -594,17 +637,23 @@ void saSetPowerByIndex(uint8_t index) return; } - if (index > 3) + if (index >= VTX_SMARTAUDIO_POWER_COUNT) { return; + } buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } +void saSetPowerByIndex(uint8_t index) +{ + saDevSetPowerByIndex(index); +} + bool vtxSmartAudioInit(void) { -#ifdef SMARTAUDIO_DPRINTF +#ifdef USE_SMARTAUDIO_DPRINTF // Setup debugSerialPort debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, NULL, 115200, MODE_RXTX, 0); @@ -616,24 +665,40 @@ bool vtxSmartAudioInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + portOptions_t portOptions = SERIAL_STOPBITS_2 | SERIAL_BIDIR_NOPULL; +#if defined(USE_VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions); } if (!smartAudioSerialPort) { return false; } - vtxCommonRegisterDevice(&vtxSmartAudio); + vtxCommonSetDevice(&vtxSmartAudio); return true; } -void vtxSAProcess(uint32_t now) +#define SA_INITPHASE_START 0 +#define SA_INITPHASE_WAIT_SETTINGS 1 // SA_CMD_GET_SETTINGS was sent and waiting for reply. +#define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply. +#define SA_INITPHASE_DONE 3 + +static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { - static bool initialSent = false; + UNUSED(vtxDevice); + UNUSED(currentTimeUs); - if (smartAudioSerialPort == NULL) + static char initPhase = SA_INITPHASE_START; + + if (smartAudioSerialPort == NULL) { return; + } while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { uint8_t c = serialRead(smartAudioSerialPort); @@ -643,72 +708,84 @@ void vtxSAProcess(uint32_t now) // Re-evaluate baudrate after each frame reception saAutobaud(); - if (!initialSent) { + switch (initPhase) { + case SA_INITPHASE_START: saGetSettings(); - saSetFreq(SA_FREQ_GETPIT); - saSendQueue(); - initialSent = true; - return; + //saSendQueue(); + initPhase = SA_INITPHASE_WAIT_SETTINGS; + break; + + case SA_INITPHASE_WAIT_SETTINGS: + // 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) { + if (saDevice.version == 2) { + saDoDevSetFreq(SA_FREQ_GETPIT); + initPhase = SA_INITPHASE_WAIT_PITFREQ; + } else { + initPhase = SA_INITPHASE_DONE; + } + } + break; + + case SA_INITPHASE_WAIT_PITFREQ: + if (saDevice.orfreq) { + initPhase = SA_INITPHASE_DONE; + } + break; + + case SA_INITPHASE_DONE: + break; } - if ((sa_outstanding != SA_CMD_NONE) - && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { + // Command queue control + + timeMs_t nowMs = millis(); // Don't substitute with "currentTimeUs / 1000"; sa_lastTransmissionMs is based on millis(). + static timeMs_t lastCommandSentMs = 0; // Last non-GET_SETTINGS sent + + if ((sa_outstanding != SA_CMD_NONE) && (nowMs - sa_lastTransmissionMs > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out // dprintf(("process: resending 0x%x\r\n", sa_outstanding)); // XXX Todo: Resend termination and possible offline transition saResendCmd(); + lastCommandSentMs = nowMs; } else if (!saQueueEmpty()) { // Command pending. Send it. // dprintf(("process: sending queue\r\n")); saSendQueue(); - } else if (now - sa_lastTransmission >= 1000) { - // Heart beat for autobauding - //dprintf(("process: sending heartbeat\r\n")); - saGetSettings(); - saSendQueue(); - } - -#ifdef SMARTAUDIO_TEST_VTX_COMMON - // Testing VTX_COMMON API - { - static uint32_t lastMonitorUs = 0; - if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) - return; - - static uint8_t monBand; - static uint8_t monChan; - static uint8_t monPower; - - vtxCommonGetBandAndChannel(&monBand, &monChan); - vtxCommonGetPowerIndex(&monPower); - debug[0] = monBand; - debug[1] = monChan; - debug[2] = monPower; + lastCommandSentMs = nowMs; + } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) { + //dprintf(("process: sending status change polling\r\n")); + saGetSettings(); + saSendQueue(); } -#endif } -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON // Interface to common VTX API -vtxDevType_e vtxSAGetDeviceType(void) +vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) { + UNUSED(vtxDevice); return VTXDEV_SMARTAUDIO; } -bool vtxSAIsReady(void) +static bool vtxSAIsReady(const vtxDevice_t *vtxDevice) { - return !(saDevice.version == 0); + return vtxDevice!=NULL && !(saDevice.version == 0); } -void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) +static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - if (band && channel) + UNUSED(vtxDevice); + if (saValidateBandAndChannel(band, channel)) { saSetBandAndChannel(band - 1, channel - 1); + } } -void vtxSASetPowerByIndex(uint8_t index) +static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { + UNUSED(vtxDevice); if (index == 0) { // SmartAudio doesn't support power off. return; @@ -717,10 +794,11 @@ void vtxSASetPowerByIndex(uint8_t index) saSetPowerByIndex(index - 1); } -void vtxSASetPitMode(uint8_t onoff) +static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { return; + } if (onoff) { // SmartAudio can not turn pit mode on by software. @@ -729,45 +807,74 @@ void vtxSASetPitMode(uint8_t onoff) uint8_t newmode = SA_MODE_CLR_PITMODE; - if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { newmode |= SA_MODE_SET_IN_RANGE_PITMODE; + } - if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) { newmode |= SA_MODE_SET_OUT_RANGE_PITMODE; + } saSetMode(newmode); return; } -bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq) { - if (!vtxSAIsReady()) + UNUSED(vtxDevice); + if (saValidateFreq(freq)) { + saSetMode(0); //need to be in FREE mode to set freq + saSetFreq(freq); + } +} + +static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!vtxSAIsReady(vtxDevice)) { 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; } -bool vtxSAGetPowerIndex(uint8_t *pIndex) +static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxSAIsReady()) + if (!vtxSAIsReady(vtxDevice)) { return false; + } *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1; return true; } -bool vtxSAGetPitMode(uint8_t *pOnOff) +static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { return false; + } *pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0; return true; } +static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + if (!vtxSAIsReady(vtxDevice)) { + 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, @@ -775,9 +882,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 149aaa1b4a1..a0ad9ee5048 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -1,18 +1,21 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * along with this software. + * + * If not, see . */ #pragma once @@ -22,6 +25,19 @@ #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 @@ -73,9 +89,11 @@ typedef struct smartAudioStat_s { extern smartAudioDevice_t saDevice; extern saPowerTable_t saPowerTable[]; +extern const char * const saPowerNames[]; extern smartAudioStat_t saStat; -extern bool saDeferred; + extern uint16_t sa_smartbaud; +extern bool saDeferred; int saDacToPowerIndex(int dac); void saSetBandAndChannel(uint8_t band, uint8_t channel); @@ -85,17 +103,9 @@ void saSetFreq(uint16_t freq); void saSetPitFreq(uint16_t freq); bool vtxSmartAudioInit(void); -#ifdef SMARTAUDIO_DPRINTF -#ifdef OMNIBUSF4 -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 -#else -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1 -#endif +#ifdef USE_SMARTAUDIO_DPRINTF extern serialPort_t *debugSerialPort; #define dprintf(x) if (debugSerialPort) printf x #else #define dprintf(x) -#endif // SMARTAUDIO_DPRINTF - -int saDacToPowerIndex(int dac); -bool vtxSmartAudioInit(void); +#endif // USE_SMARTAUDIO_DPRINTF diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index 71f0e4047a1..bbaa3eb7b11 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -25,9 +25,12 @@ #include "platform.h" #include "build/debug.h" -#if defined(VTX_COMMON) +#if defined(USE_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 @@ -36,7 +39,7 @@ const uint16_t vtx58frequencyTable[5][8] = { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand }; -const char * const vtx58BandNames[] = { +const char * const vtx58BandNames[VTX_STRING_BAND_COUNT + 1] = { "--------", "BOSCAM A", "BOSCAM B", @@ -45,9 +48,9 @@ const char * const vtx58BandNames[] = { "RACEBAND", }; -const char vtx58BandLetter[] = "-ABEFR"; +const char vtx58BandLetter[VTX_STRING_BAND_COUNT + 1] = "-ABEFR"; -const char * const vtx58ChannelNames[] = { +const char * const vtx58ChannelNames[VTX_STRING_CHAN_COUNT + 1] = { "-", "1", "2", "3", "4", "5", "6", "7", "8", }; @@ -74,4 +77,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 cd9c40fb419..f7a56184111 100644 --- a/src/main/io/vtx_string.h +++ b/src/main/io/vtx_string.h @@ -8,3 +8,4 @@ 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); diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index f7235be0ee6..e6a93eb4624 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -1,61 +1,66 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 and Betaflight are free software. You can redistribute + * this software and/or modify this software 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. + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * along with this software. + * + * If not, see . */ /* Created by jflyper */ #include #include +#include #include #include "platform.h" -#if defined(VTX_TRAMP) && defined(VTX_CONTROL) +#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL) #include "build/debug.h" +#include "common/maths.h" #include "common/utils.h" +#include "cms/cms_menu_vtx_tramp.h" + #include "drivers/vtx_common.h" #include "io/serial.h" #include "io/vtx_tramp.h" +#include "io/vtx_control.h" +#include "io/vtx.h" #include "io/vtx_string.h" -#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) - -#if defined(USE_CMS) || defined(VTX_COMMON) -const uint16_t trampPowerTable[] = { +#if defined(USE_CMS) || defined(USE_VTX_COMMON) +const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = { 25, 100, 200, 400, 600 }; -const char * const trampPowerNames[] = { +const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = { "---", "25 ", "100", "200", "400", "600" }; #endif -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) static const vtxVTable_t trampVTable; // forward static vtxDevice_t vtxTramp = { .vTable = &trampVTable, - .capability = { - .bandCount = 5, - .channelCount = 8, - .powerCount = sizeof(trampPowerTable), - }, + .capability.bandCount = VTX_TRAMP_BAND_COUNT, + .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT, + .capability.powerCount = sizeof(trampPowerTable), .bandNames = (char **)vtx58BandNames, .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)trampPowerNames, @@ -76,38 +81,42 @@ typedef enum { trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE; -// TODO: These fields are currently removed by the compiler because they're -// never read. Decide if we want to use them for something or remove them. -static uint16_t trampRFFreqMin; -static uint16_t trampRFFreqMax; -static uint16_t trampRFPowerMax; - +uint32_t trampRFFreqMin; +uint32_t trampRFFreqMax; +uint32_t trampRFPowerMax; trampData_t trampData; // Maximum number of requests sent to try a config change #define TRAMP_MAX_RETRIES 2 -uint16_t trampConfFreq = 0; +uint32_t trampConfFreq = 0; uint8_t trampFreqRetries = 0; uint16_t trampConfPower = 0; uint8_t trampPowerRetries = 0; +static void trampWriteBuf(uint8_t *buf) +{ + serialWriteBuf(trampSerialPort, buf, 16); +} + static uint8_t trampChecksum(uint8_t *trampBuf) { uint8_t cksum = 0; - for (int i = 1 ; i < 14 ; i++) + for (int i = 1 ; i < 14 ; i++) { cksum += trampBuf[i]; + } return cksum; } void trampCmdU16(uint8_t cmd, uint16_t param) { - if (!trampSerialPort) + if (!trampSerialPort) { return; + } uint8_t trampReqBuffer[16]; memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer)); @@ -116,14 +125,26 @@ void trampCmdU16(uint8_t cmd, uint16_t param) trampReqBuffer[2] = param & 0xff; trampReqBuffer[3] = (param >> 8) & 0xff; trampReqBuffer[14] = trampChecksum(trampReqBuffer); - serialWriteBuf(trampSerialPort, trampReqBuffer, sizeof(trampReqBuffer)); + 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 != trampData.curFreq) + if (trampConfFreq != trampData.curFreq) { trampFreqRetries = TRAMP_MAX_RETRIES; + } +} + +void trampSetFreq(uint16_t freq) +{ + trampData.setByFreqFlag = true; //set freq via MHz value + trampDevSetFreq(freq); } void trampSendFreq(uint16_t freq) @@ -131,16 +152,29 @@ 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]); + trampData.setByFreqFlag = false; //set freq via band/channel + trampDevSetBandAndChannel(band, channel); } void trampSetRFPower(uint16_t level) { trampConfPower = level; - if(trampConfPower != trampData.power) + if (trampConfPower != trampData.power) { trampPowerRetries = TRAMP_MAX_RETRIES; + } } void trampSendRFPower(uint16_t level) @@ -148,21 +182,28 @@ void trampSendRFPower(uint16_t level) trampCmdU16('P', level); } -bool trampIsAvailable(void) -{ - return trampStatus != TRAMP_STATUS_BAD_DEVICE && trampStatus != TRAMP_STATUS_OFFLINE; -} - // return false if error bool trampCommitChanges(void) { - if(trampStatus != TRAMP_STATUS_ONLINE) + if (trampStatus != TRAMP_STATUS_ONLINE) { return false; + } trampStatus = TRAMP_STATUS_SET_FREQ_PW; 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); @@ -171,13 +212,13 @@ void trampSetPitMode(uint8_t onoff) // returns completed response code char trampHandleResponse(void) { - uint8_t respCode = trampRespBuffer[1]; + const uint8_t respCode = trampRespBuffer[1]; switch (respCode) { case 'r': { - uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - if(min_freq != 0) { + const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if (min_freq != 0) { trampRFFreqMin = min_freq; trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); @@ -190,16 +231,20 @@ char trampHandleResponse(void) case 'v': { - uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - if(freq != 0) { + const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if (freq != 0) { trampData.curFreq = freq; trampData.configuredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampData.pitMode = trampRespBuffer[7]; trampData.power = trampRespBuffer[8]|(trampRespBuffer[9] << 8); - vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel); - if(trampConfFreq == 0) trampConfFreq = trampData.curFreq; - if(trampConfPower == 0) trampConfPower = trampData.power; + // if no band/chan match then make sure set-by-freq mode is flagged + if (!vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel)) { + trampData.setByFreqFlag = true; + } + + if (trampConfFreq == 0) trampConfFreq = trampData.curFreq; + if (trampConfPower == 0) trampConfPower = trampData.power; return 'v'; } @@ -209,8 +254,8 @@ char trampHandleResponse(void) case 's': { - uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); - if(temp != 0) { + const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); + if (temp != 0) { trampData.temperature = temp; return 's'; } @@ -238,10 +283,11 @@ static void trampResetReceiver(void) static bool trampIsValidResponseCode(uint8_t code) { - if (code == 'r' || code == 'v' || code == 's') + if (code == 'r' || code == 'v' || code == 's') { return true; - else + } else { return false; + } } // returns completed response code or 0 @@ -249,14 +295,15 @@ static char trampReceive(uint32_t currentTimeUs) { UNUSED(currentTimeUs); - if (!trampSerialPort) + if (!trampSerialPort) { return 0; + } while (serialRxBytesWaiting(trampSerialPort)) { - uint8_t c = serialRead(trampSerialPort); + const uint8_t c = serialRead(trampSerialPort); trampRespBuffer[trampReceivePos++] = c; - switch(trampReceiveState) { + switch (trampReceiveState) { case S_WAIT_LEN: if (c == 0x0F) { trampReceiveState = S_WAIT_CODE; @@ -287,6 +334,7 @@ static char trampReceive(uint32_t currentTimeUs) default: trampResetReceiver(); + break; } } @@ -314,50 +362,63 @@ void trampQueryS(void) trampQuery('s'); } -void vtxTrampProcess(uint32_t currentTimeUs) +static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { - static uint32_t lastQueryTimeUs = 0; + UNUSED(vtxDevice); + + static timeUs_t lastQueryTimeUs = 0; + static bool initSettingsDoneFlag = false; #ifdef TRAMP_DEBUG static uint16_t debugFreqReqCounter = 0; static uint16_t debugPowReqCounter = 0; #endif - if (trampStatus == TRAMP_STATUS_BAD_DEVICE) + if (trampStatus == TRAMP_STATUS_BAD_DEVICE) { return; + } - char replyCode = trampReceive(currentTimeUs); + const char replyCode = trampReceive(currentTimeUs); #ifdef TRAMP_DEBUG debug[0] = trampStatus; #endif - switch(replyCode) { + switch (replyCode) { case 'r': - if (trampStatus <= TRAMP_STATUS_OFFLINE) + 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) + } + } break; case 'v': - if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) + if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) { trampStatus = TRAMP_STATUS_SET_FREQ_PW; + } break; } - switch(trampStatus) { + switch (trampStatus) { case TRAMP_STATUS_OFFLINE: case TRAMP_STATUS_ONLINE: if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s - if (trampStatus == TRAMP_STATUS_OFFLINE) + if (trampStatus == TRAMP_STATUS_OFFLINE) { trampQueryR(); - else { + } else { static unsigned int cnt = 0; - if(((cnt++) & 1) == 0) + if (((cnt++) & 1) == 0) { trampQueryV(); - else + } else { trampQueryS(); + } } lastQueryTimeUs = currentTimeUs; @@ -374,8 +435,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) debugFreqReqCounter++; #endif done = false; - } - else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) { + } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) { trampSendRFPower(trampConfPower); trampPowerRetries--; #ifdef TRAMP_DEBUG @@ -384,13 +444,12 @@ void vtxTrampProcess(uint32_t currentTimeUs) done = false; } - if(!done) { + if (!done) { trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; // delay next status query by 300ms lastQueryTimeUs = currentTimeUs + 300 * 1000; - } - else { + } else { // everything has been done, let's return to original state trampStatus = TRAMP_STATUS_ONLINE; // reset configuration value in case it failed (no more retries) @@ -420,55 +479,68 @@ void vtxTrampProcess(uint32_t currentTimeUs) } -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON // Interface to common VTX API -vtxDevType_e vtxTrampGetDeviceType(void) +static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice) { + UNUSED(vtxDevice); return VTXDEV_TRAMP; } -bool vtxTrampIsReady(void) +static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice) { - return trampStatus > TRAMP_STATUS_OFFLINE; + return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE; } -void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel) +static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - if (band && channel) { + UNUSED(vtxDevice); + if (trampValidateBandAndChannel(band, channel)) { trampSetBandAndChannel(band, channel); trampCommitChanges(); } } -void vtxTrampSetPowerByIndex(uint8_t index) +static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - if (index) { - trampSetRFPower(trampPowerTable[index - 1]); - trampCommitChanges(); - } + UNUSED(vtxDevice); + trampDevSetPowerByIndex(index); } -void vtxTrampSetPitMode(uint8_t onoff) +static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { + UNUSED(vtxDevice); trampSetPitMode(onoff); } -bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) { - if (!vtxTrampIsReady()) + UNUSED(vtxDevice); + if (trampValidateFreq(freq)) { + trampSetFreq(freq); + trampCommitChanges(); + } +} + +static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!vtxTrampIsReady(vtxDevice)) { return false; + } - *pBand = trampData.band; + // if in user-freq mode then report band as zero + *pBand = trampData.setByFreqFlag ? 0 : trampData.band; *pChannel = trampData.channel; return true; } -bool vtxTrampGetPowerIndex(uint8_t *pIndex) +static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady(vtxDevice)) { return false; + } if (trampData.configuredPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { @@ -482,15 +554,26 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex) return true; } -bool vtxTrampGetPitMode(uint8_t *pOnOff) +static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady(vtxDevice)) { return false; + } *pOnOff = trampData.pitMode; return true; } +static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + if (!vtxTrampIsReady(vtxDevice)) { + return false; + } + + *pFreq = trampData.curFreq; + return true; +} + static const vtxVTable_t trampVTable = { .process = vtxTrampProcess, .getDeviceType = vtxTrampGetDeviceType, @@ -498,9 +581,11 @@ static const vtxVTable_t trampVTable = { .setBandAndChannel = vtxTrampSetBandAndChannel, .setPowerByIndex = vtxTrampSetPowerByIndex, .setPitMode = vtxTrampSetPitMode, + .setFrequency = vtxTrampSetFreq, .getBandAndChannel = vtxTrampGetBandAndChannel, .getPowerIndex = vtxTrampGetPowerIndex, .getPitMode = vtxTrampGetPitMode, + .getFrequency = vtxTrampGetFreq, }; #endif @@ -510,17 +595,24 @@ bool vtxTrampInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + portOptions_t portOptions = 0; +#if defined(USE_VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions); } if (!trampSerialPort) { return false; } -#if defined(VTX_COMMON) - vtxCommonRegisterDevice(&vtxTramp); +#if defined(USE_VTX_COMMON) + vtxCommonSetDevice(&vtxTramp); #endif - + return true; } diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 48e7a0e9289..a9c7cd04b2c 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -37,6 +37,7 @@ extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT]; extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1]; typedef struct trampData_s { + bool setByFreqFlag; //false = set via band/channel uint8_t band; uint8_t channel; uint16_t power; // Actual transmitting power @@ -49,7 +50,6 @@ typedef struct trampData_s { extern trampData_t trampData; bool vtxTrampInit(void); -bool trampIsAvailable(void); bool trampCommitChanges(void); void trampSetPitMode(uint8_t onoff); void trampSetBandAndChannel(uint8_t band, uint8_t channel); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index e76f7f96e3a..f90af8fb8fb 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -114,7 +114,7 @@ typedef enum { #ifdef USE_RCDEVICE TASK_RCDEVICE, #endif -#ifdef VTX_CONTROL +#ifdef USE_VTX_CONTROL TASK_VTXCTRL, #endif diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index a273f79497f..0c4dc58a190 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -112,11 +112,11 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define VTX_RTC6705 +#define USE_VTX_RTC6705 #define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL -#undef VTX_SMARTAUDIO // Disabled due to flash size -#undef VTX_TRAMP // Disabled due to flash size +#undef USE_VTX_SMARTAUDIO // Disabled due to flash size +#undef USE_VTX_TRAMP // Disabled due to flash size #define RTC6705_CS_PIN PF4 #define RTC6705_SPI_INSTANCE SPI3 diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 69a3b93972b..b33946b76e9 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -122,7 +122,7 @@ #define SPI3_MISO_PIN PB4 // NC #define SPI3_MOSI_PIN PB5 // NC -#define VTX_RTC6705 +#define USE_VTX_RTC6705 #define VTX_RTC6705_OPTIONAL // SPI3 on an F4 EVO may be used for RTC6705 VTX control. #define RTC6705_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/common.h b/src/main/target/common.h index 97d92fcf5cf..298fa00dd64 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -58,6 +58,7 @@ #endif #if (FLASH_SIZE > 256) +#define USE_EXTENDED_CMS_MENUS #define USE_UAV_INTERCONNECT #define USE_RX_UIB #endif @@ -111,10 +112,10 @@ #define USE_PITOT_ADC //Enable VTX controll -#define VTX_COMMON -#define VTX_CONTROL -#define VTX_SMARTAUDIO -#define VTX_TRAMP +#define USE_VTX_COMMON +#define USE_VTX_CONTROL +#define USE_VTX_SMARTAUDIO +#define USE_VTX_TRAMP //Enable DST calculations #define RTC_AUTOMATIC_DST diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index fe285d989ca..58a1f4fc4f8 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -21,7 +21,7 @@ // Targets with built-in vtx do not need external vtx #if defined(VTX) || defined(USE_RTC6705) -# undef VTX_CONTROL +# undef USE_VTX_CONTROL #endif // Backward compatibility for I2C OLED display diff --git a/src/utils/rename-ifdefs.py b/src/utils/rename-ifdefs.py index 8cffac4c633..095d1d55e67 100644 --- a/src/utils/rename-ifdefs.py +++ b/src/utils/rename-ifdefs.py @@ -45,6 +45,11 @@ 'BOOTLOG', 'STATS', 'FIXED_WING_LANDING', + 'VTX_CONTROL', + 'VTX_SMARTAUDIO', + 'VTX_TRAMP', + 'VTX_RTC6705', + 'VTX_COMMON', ] NEW_NAMES = {