From c2a814674b3615df8b2ef0c0d981aacd4db288b7 Mon Sep 17 00:00:00 2001 From: SomeTestStuffDoesntWork Date: Sun, 28 Dec 2025 22:26:13 -0500 Subject: [PATCH 1/2] Added IRC Tramp pseudo-vtx tables. Allows users to override configuration values --- src/main/config/parameter_group_ids.h | 3 +- src/main/fc/cli.c | 78 ++++++++++++++ src/main/io/vtx_tramp.c | 145 ++++++++++++++++++-------- src/main/io/vtx_tramp.h | 20 ++++ 4 files changed, 199 insertions(+), 47 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 2acb9c8172e..d88581f9eba 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -132,7 +132,8 @@ #define PG_GEOZONE_CONFIG 1042 #define PG_GEOZONES 1043 #define PG_GEOZONE_VERTICES 1044 -#define PG_INAV_END PG_GEOZONE_VERTICES +#define PG_TRAMP_TABLE_O_RIDE 1045 +#define PG_INAV_END PG_TRAMP_TABLE_O_RIDE // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6c60f08c6ed..84ca01b7a0d 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -95,6 +95,7 @@ bool cliMode = false; #include "io/osd.h" #include "io/osd/custom_elements.h" #include "io/serial.h" +#include "io/vtx_tramp.h" #include "fc/fc_msp_box.h" @@ -3220,6 +3221,74 @@ static void cliTimerOutputMode(char *cmdline) } +static void printTrampConfig(char* cmdline) +{ + (void) cmdline; + dumpLiveVtxTrampConfig(cliPrintf); +} + +static void cliTrampPwOr(char *cmdline) +{ + static const char parseErrString[] = "ERROR. Cannot parse tramp PL override.\n"; + if(isEmpty(cmdline)) + { + // Nothing sent, so fail and bail. + cliPrint(parseErrString); + return; + } + + // Command structure is: + const char* current_cmd = cmdline++; + + // Get the PL index. + int powerIdx = fastA2I(current_cmd); + if(powerIdx > VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS || powerIdx < 1) + { + // Invalid range, fail and bail + cliPrint(parseErrString); + return; + } + + // Convert PL Index to array indexable value + powerIdx--; + + // Get the PL value. + int reqPlMw = VTX_TRAMP_NULL_PW_CONFIG; + current_cmd = nextArg(current_cmd); + if(current_cmd) + { + reqPlMw = fastA2I(current_cmd); + } + + // If invalid, set to null (user may request that the VTX table is used as-is or it is not implemented) + if(reqPlMw < 0) + { + reqPlMw = VTX_TRAMP_NULL_PW_CONFIG; + } + + cliPrintf("Tramp Override stat - PL IDX: %d; PL (mw): %d", powerIdx + 1, reqPlMw); + + vtxTrampPwOverride_t* powerConfig = vtxTrampPwOverrideMutable(powerIdx); + powerConfig->vtxPwOverrideMw = reqPlMw; +} + +static void printTrampOverride(uint8_t dumpMask, const vtxTrampPwOverride_t* trampOverrideConfig, const vtxTrampPwOverride_t* defaultTrampOverrideConfig) +{ + const char* dumpFormat = VTX_TRAMP_PL_OR_CMD " %d %d"; + for (uint32_t currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) + { + const vtxTrampPwOverride_t* current_config = &trampOverrideConfig[currentPwLvl]; + bool equalsDefault = false; + if (defaultTrampOverrideConfig) { + const vtxTrampPwOverride_t* default_config = &defaultTrampOverrideConfig[currentPwLvl]; + equalsDefault = current_config->vtxPwOverrideMw == default_config->vtxPwOverrideMw; + cliDefaultPrintLinef(dumpMask, equalsDefault, dumpFormat, currentPwLvl + 1, default_config->vtxPwOverrideMw); + } + + cliDumpPrintLinef(dumpMask, equalsDefault, dumpFormat, currentPwLvl + 1, current_config->vtxPwOverrideMw); + } +} + static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) { uint32_t mask = featureConfig->enabledFeatures; @@ -4531,6 +4600,11 @@ static void printConfig(const char *cmdline, bool doDiff) printOsdLayout(dumpMask, &osdLayoutsConfig_Copy, osdLayoutsConfig(), -1, -1); #endif +#ifdef USE_VTX_TRAMP + cliPrintHashLine("IRC Tramp VTX Table Overrides"); + printTrampOverride(dumpMask, vtxTrampPwOverride_CopyArray, vtxTrampPwOverride(0)); +#endif + #ifdef USE_PROGRAMMING_FRAMEWORK cliPrintHashLine("Programming: logic"); printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0), -1); @@ -4942,6 +5016,10 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), +#ifdef USE_VTX_TRAMP + CLI_COMMAND_DEF(VTX_TRAMP_PL_OR_CMD, "Set the power level override of an IRC tramp VTX table", NULL, cliTrampPwOr), + CLI_COMMAND_DEF(VTX_TRAMP_CONFIG_DUMP_CMD, "Dump the live operating tramp config", NULL, printTrampConfig), +#endif }; static void cliHelp(char *cmdline) diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 540c9c9f222..dc714a9d30b 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -24,6 +24,7 @@ #include #include #include +#include #include "platform.h" @@ -42,6 +43,8 @@ #include "io/vtx_control.h" #include "io/vtx.h" #include "io/vtx_string.h" +#include "config/parameter_group_ids.h" +#include "config/config_reset.h" #define VTX_PKT_SIZE 16 #define VTX_PROTO_STATE_TIMEOUT_MS 1000 @@ -52,6 +55,18 @@ #define VTX_UPDATE_REQ_POWER 0x02 #define VTX_UPDATE_REQ_PITMODE 0x04 +PG_REGISTER_ARRAY_WITH_RESET_FN(vtxTrampPwOverride_t, VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS, vtxTrampPwOverride, PG_TRAMP_TABLE_O_RIDE, VTX_TRAMP_PW_OVERRIDE_VER); + +void pgResetFn_vtxTrampPwOverride(vtxTrampPwOverride_t* table) +{ + for (int current_pw_lvl = 0; current_pw_lvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; current_pw_lvl++) + { + RESET_CONFIG(vtxTrampPwOverride_t, &table[current_pw_lvl], + .vtxPwOverrideMw = -1 + ); + } +} + typedef enum { VTX_STATE_RESET = 0, VTX_STATE_OFFILE = 1, // Not detected @@ -560,41 +575,99 @@ static vtxDevice_t impl_vtxDevice = { .capability.powerNames = NULL, }; -const uint16_t trampPowerTable_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 200, 200 }; -const char * const trampPowerNames_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "200", "200" }; +// Backwards compatable mutable data structures to allow for configuration to be added. +static uint16_t mutableTablePowers[VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS]; +#define MAX_VTX_PWR_NAME_CHARS 6 +static char mutableTablePowerNames[VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS + 1][MAX_VTX_PWR_NAME_CHARS]; +// Default table power levels +const uint16_t trampPowerTable_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 200, 200 }; const uint16_t trampPowerTable_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 400, 400 }; -const char * const trampPowerNames_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "400" }; - const uint16_t trampPowerTable_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 400, 600 }; -const char * const trampPowerNames_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" }; - const uint16_t trampPowerTable_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 500, 800 }; -const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "500", "800" }; - const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 800 }; -const char * const trampPowerNames_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800" }; +const uint16_t trampPowerTable_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 2000 }; + +// Dump the VTX operating params to the console. Used for configuration validation to ensure that the VTX is being commanded as +// expected. Used to debug VTX issues such as inconsistent power levels (IE command 1 mw for first power, 2 mw for second, etc), +// frequency issues (commanded != requested), etc. This does require that the CLI serial interface is exposed, but better that than +// exposing the VTX driver internals to the CLI. +void dumpLiveVtxTrampConfig(consolePrintf_t consolePrint) +{ + // Dump configuration + consolePrint("Configured power levels: %d\n", impl_vtxDevice.capability.powerCount); + for(uint8_t current_pl = 0; current_pl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; current_pl++) + { + consolePrint("PL %d: %d mw\n", current_pl + 1, mutableTablePowers[current_pl]); + } -const uint16_t trampPowerTable_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 2000 }; -const char * const trampPowerNames_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "2000" }; + consolePrint("Actual VTX Freq: %u\n", vtxState.state.freq); + consolePrint("Actual VTX Power: %u\n", vtxState.state.power); + + consolePrint("Requested VTX Freq: %u\n", vtxState.request.freq); + consolePrint("Requested VTX Power: %u\n", vtxState.request.power); + consolePrint("Requested VTX Power IDX: %u\n", vtxState.request.powerIndex); +} + +// Construct the power table. Takes into account any configured override power values. +// baseTable - Pointer to the base table configuration +// tableCount - Entries in the table configuration +static void constructPowerTable(const uint16_t* baseTable, const uint16_t tableCount) +{ + // Update the 0th power index + strcpy(mutableTablePowerNames[0], "---"); + + // Now construct each table + const vtxTrampPwOverride_t* pwr_config; + uint16_t currentPwLvl = 0; + for(currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) + { + pwr_config = vtxTrampPwOverride(currentPwLvl); + if(pwr_config->vtxPwOverrideMw >= 0) + { + // Only override if power is configured to a valid value. If negative, this value is not configured and + // will be set to the default table for the power level. + mutableTablePowers[currentPwLvl] = (uint16_t) pwr_config->vtxPwOverrideMw; + } + + else if(currentPwLvl < tableCount) + { + // Not configured, so use the default. + mutableTablePowers[currentPwLvl] = *(baseTable + currentPwLvl); + } + else + { + // No default value and no configured value. Nothing to write, so terminate from the loop. + break; + } + + // Update the "stringified" power. Always add 1 to the working power level since 0 is reserved. + sprintf(mutableTablePowerNames[currentPwLvl + 1], "%u", mutableTablePowers[currentPwLvl]); + } + + // Update the stored params. + vtxState.metadata.powerTablePtr = mutableTablePowers; + + // NOTE: If loop broke before max (IE 2 overrides and three default, < 5, idx = 3) the last loop index will be the total + // quantity configured. Set that here. + vtxState.metadata.powerTableCount = currentPwLvl; + + impl_vtxDevice.capability.powerNames = (char**) mutableTablePowerNames; + impl_vtxDevice.capability.powerCount = currentPwLvl; + + // NOTE: If less power levels than max supported are passed, then the upper values in the config are left as-is. + // The result is that they are not used. +} static void vtxProtoUpdatePowerMetadata(uint16_t maxPower) { switch (vtxSettingsConfig()->frequencyGroup) { case FREQUENCYGROUP_1G3: if (maxPower >= 2000) { - vtxState.metadata.powerTablePtr = trampPowerTable_1G3_2000; - vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_2000; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + constructPowerTable(trampPowerTable_1G3_2000, VTX_TRAMP_1G3_MAX_POWER_COUNT); } else { - vtxState.metadata.powerTablePtr = trampPowerTable_1G3_800; - vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + constructPowerTable(trampPowerTable_1G3_800, VTX_TRAMP_1G3_MAX_POWER_COUNT); } impl_vtxDevice.capability.bandCount = VTX_TRAMP_1G3_BAND_COUNT; impl_vtxDevice.capability.channelCount = VTX_TRAMP_1G3_CHANNEL_COUNT; @@ -604,43 +677,23 @@ static void vtxProtoUpdatePowerMetadata(uint16_t maxPower) default: if (maxPower >= 800) { // Max power 800mW: Use 25, 100, 200, 500, 800 table - vtxState.metadata.powerTablePtr = trampPowerTable_5G8_800; - vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_800; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; + constructPowerTable(trampPowerTable_5G8_800, sizeof(trampPowerTable_5G8_800)/sizeof(trampPowerTable_5G8_800[0])); } else if (maxPower >= 600) { // Max power 600mW: Use 25, 100, 200, 400, 600 table - vtxState.metadata.powerTablePtr = trampPowerTable_5G8_600; - vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_600; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; + constructPowerTable(trampPowerTable_5G8_600, sizeof(trampPowerTable_5G8_600)/sizeof(trampPowerTable_5G8_600[0])); } else if (maxPower >= 400) { // Max power 400mW: Use 25, 100, 200, 400 table - vtxState.metadata.powerTablePtr = trampPowerTable_5G8_400; - vtxState.metadata.powerTableCount = 4; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_400; - impl_vtxDevice.capability.powerCount = 4; + constructPowerTable(trampPowerTable_5G8_400, sizeof(trampPowerTable_5G8_400)/sizeof(trampPowerTable_5G8_400[0])); } else if (maxPower >= 200) { // Max power 200mW: Use 25, 100, 200 table - vtxState.metadata.powerTablePtr = trampPowerTable_5G8_200; - vtxState.metadata.powerTableCount = 3; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_200; - impl_vtxDevice.capability.powerCount = 3; + constructPowerTable(trampPowerTable_5G8_200, sizeof(trampPowerTable_5G8_200)/sizeof(trampPowerTable_5G8_200[0])); } else { // Default to standard TRAMP 600mW VTX - vtxState.metadata.powerTablePtr = trampPowerTable_5G8_600; - vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; - - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_600; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT; + constructPowerTable(trampPowerTable_5G8_600, sizeof(trampPowerTable_5G8_600)/sizeof(trampPowerTable_5G8_600[0])); } break; } diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 1f44201853c..eb1c35b54dd 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -18,6 +18,7 @@ #pragma once #include +#include "config/parameter_group.h" // 5.8 GHz #define VTX_TRAMP_5G8_BAND_COUNT 5 @@ -39,4 +40,23 @@ #define VTX_TRAMP_1G3_MIN_FREQUENCY_MHZ 1000 #define VTX_TRAMP_1G3_MAX_FREQUENCY_MHZ 1399 +// Defines a 'zero' config value. +#define VTX_TRAMP_NULL_PW_CONFIG -1 + +#define VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS VTX_TRAMP_5G8_MAX_POWER_COUNT +#define VTX_TRAMP_PW_OVERRIDE_VER 1 + +#define VTX_TRAMP_PL_OR_CMD "tramp_pl_override" +#define VTX_TRAMP_CONFIG_DUMP_CMD "tramp_dump_config" + +typedef struct { + int vtxPwOverrideMw; +} vtxTrampPwOverride_t; + +PG_DECLARE_ARRAY(vtxTrampPwOverride_t, VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS, vtxTrampPwOverride); + bool vtxTrampInit(void); + +typedef void (*consolePrintf_t)(const char*, ...); + +void dumpLiveVtxTrampConfig(consolePrintf_t consolePrint); From 98e627c6e4a434647a8a159e929f2106f88f14a2 Mon Sep 17 00:00:00 2001 From: SomeTestStuffDoesntWork Date: Mon, 29 Dec 2025 12:16:31 -0500 Subject: [PATCH 2/2] Performed a cleanup pass on all comments and variable names. Also added better range checks to PL commands. Hooked the new configuration into settings.yaml. Added preprocessor conditionals to static functions for tramp PL configuration to ensure that when the tramp option excludes the commands from the build the static processing functions are not left unused. Added tramp_pl_table command to ease updating the configuration in the cases where no default hard coded PL table values are to be reused. If less than the max number of PL entries are provided, remaining entries are filled with that of the last passed PL to ensure that the default values are not used. Tested on F405, worked without issue. --- src/main/fc/cli.c | 89 +++++++++++++++++++++++++++------------ src/main/fc/settings.yaml | 12 ++++++ src/main/io/vtx_tramp.c | 80 ++++++++++++++--------------------- src/main/io/vtx_tramp.h | 4 +- 4 files changed, 108 insertions(+), 77 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 84ca01b7a0d..20b37fbaf43 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3221,74 +3221,106 @@ static void cliTimerOutputMode(char *cmdline) } +#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL) + static void printTrampConfig(char* cmdline) { - (void) cmdline; + UNUSED(cmdline); dumpLiveVtxTrampConfig(cliPrintf); } +static void printTrampOverride(uint8_t dumpMask, const vtxTrampPwOverride_t* trampOverrideConfig, const vtxTrampPwOverride_t* defaultTrampOverrideConfig) +{ + const char* dumpFormat = VTX_TRAMP_PL_OR_CMD " %d %d"; + for (uint32_t currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) + { + const vtxTrampPwOverride_t* currentConfig = &trampOverrideConfig[currentPwLvl]; + bool equalsDefault = false; + if (defaultTrampOverrideConfig) { + const vtxTrampPwOverride_t* defaultConfig = &defaultTrampOverrideConfig[currentPwLvl]; + equalsDefault = currentConfig->vtxPwOverrideMw == defaultConfig->vtxPwOverrideMw; + cliDefaultPrintLinef(dumpMask, equalsDefault, dumpFormat, currentPwLvl + 1, defaultConfig->vtxPwOverrideMw); + } + + cliDumpPrintLinef(dumpMask, equalsDefault, dumpFormat, currentPwLvl + 1, currentConfig->vtxPwOverrideMw); + } +} + static void cliTrampPwOr(char *cmdline) { - static const char parseErrString[] = "ERROR. Cannot parse tramp PL override.\n"; if(isEmpty(cmdline)) { - // Nothing sent, so fail and bail. - cliPrint(parseErrString); + printTrampOverride(DUMP_ALL, vtxTrampPwOverride(0), NULL); return; } // Command structure is: - const char* current_cmd = cmdline++; + const char* currentCmd = cmdline++; - // Get the PL index. - int powerIdx = fastA2I(current_cmd); + int powerIdx = fastA2I(currentCmd); if(powerIdx > VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS || powerIdx < 1) { - // Invalid range, fail and bail - cliPrint(parseErrString); + cliPrint("ERROR. Cannot parse command.\n"); return; } // Convert PL Index to array indexable value powerIdx--; - // Get the PL value. int reqPlMw = VTX_TRAMP_NULL_PW_CONFIG; - current_cmd = nextArg(current_cmd); - if(current_cmd) + currentCmd = nextArg(currentCmd); + if(currentCmd) { - reqPlMw = fastA2I(current_cmd); + reqPlMw = fastA2I(currentCmd); } // If invalid, set to null (user may request that the VTX table is used as-is or it is not implemented) - if(reqPlMw < 0) + if(reqPlMw < 0 || reqPlMw > VTX_TRAMP_PL_MAX_MW) { reqPlMw = VTX_TRAMP_NULL_PW_CONFIG; } - cliPrintf("Tramp Override stat - PL IDX: %d; PL (mw): %d", powerIdx + 1, reqPlMw); - vtxTrampPwOverride_t* powerConfig = vtxTrampPwOverrideMutable(powerIdx); powerConfig->vtxPwOverrideMw = reqPlMw; } -static void printTrampOverride(uint8_t dumpMask, const vtxTrampPwOverride_t* trampOverrideConfig, const vtxTrampPwOverride_t* defaultTrampOverrideConfig) +static void cliTrampPwTable(char* cmdline) { - const char* dumpFormat = VTX_TRAMP_PL_OR_CMD " %d %d"; - for (uint32_t currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) + if(isEmpty(cmdline)) { - const vtxTrampPwOverride_t* current_config = &trampOverrideConfig[currentPwLvl]; - bool equalsDefault = false; - if (defaultTrampOverrideConfig) { - const vtxTrampPwOverride_t* default_config = &defaultTrampOverrideConfig[currentPwLvl]; - equalsDefault = current_config->vtxPwOverrideMw == default_config->vtxPwOverrideMw; - cliDefaultPrintLinef(dumpMask, equalsDefault, dumpFormat, currentPwLvl + 1, default_config->vtxPwOverrideMw); + printTrampOverride(DUMP_ALL, vtxTrampPwOverride(0), NULL); + return; + } + + // Command structure is: ... + const char* currentCmd = cmdline++; + int currentPlMw = 0; + int currentPlIdx = 0; + vtxTrampPwOverride_t* powerConfig = NULL; + while(currentCmd != NULL && currentPlIdx < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS) + { + currentPlMw = fastA2I(currentCmd); + if(currentPlMw < 0 || currentPlMw > VTX_TRAMP_PL_MAX_MW) + { + currentPlMw = VTX_TRAMP_NULL_PW_CONFIG; } + powerConfig = vtxTrampPwOverrideMutable(currentPlIdx); + powerConfig->vtxPwOverrideMw = currentPlMw; + currentPlIdx++; + currentCmd = nextArg(currentCmd); + } - cliDumpPrintLinef(dumpMask, equalsDefault, dumpFormat, currentPwLvl + 1, current_config->vtxPwOverrideMw); + // If not all table entries were passed, then fill the remaining with the last sent power. This will clear out any remaining + // default values picked up by the tramp PL table generation. + for(int currentEmptyPlIdx = currentPlIdx; currentEmptyPlIdx < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentEmptyPlIdx++) + { + powerConfig = vtxTrampPwOverrideMutable(currentEmptyPlIdx); + powerConfig->vtxPwOverrideMw = currentPlMw; } } +#endif + static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) { uint32_t mask = featureConfig->enabledFeatures; @@ -4600,7 +4632,7 @@ static void printConfig(const char *cmdline, bool doDiff) printOsdLayout(dumpMask, &osdLayoutsConfig_Copy, osdLayoutsConfig(), -1, -1); #endif -#ifdef USE_VTX_TRAMP +#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL) cliPrintHashLine("IRC Tramp VTX Table Overrides"); printTrampOverride(dumpMask, vtxTrampPwOverride_CopyArray, vtxTrampPwOverride(0)); #endif @@ -5016,8 +5048,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), -#ifdef USE_VTX_TRAMP +#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL) CLI_COMMAND_DEF(VTX_TRAMP_PL_OR_CMD, "Set the power level override of an IRC tramp VTX table", NULL, cliTrampPwOr), + CLI_COMMAND_DEF(VTX_TRAMP_PL_CMD, "Set all entries in the power level table", NULL, cliTrampPwTable), CLI_COMMAND_DEF(VTX_TRAMP_CONFIG_DUMP_CMD, "Dump the live operating tramp config", NULL, printTrampConfig), #endif }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 01ca6149bfb..29136f2ee32 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4501,3 +4501,15 @@ groups: field: noWayHomeAction table: geozone_rth_no_way_home type: uint8_t + - name: PG_TRAMP_TABLE_O_RIDE + type: vtxTrampPwOverride_t + headers: ["io/vtx_tramp.h"] + condition: USE_VTX_TRAMP && USE_VTX_CONTROL + members: + - name: vtx_pw_override_mw + type: int16_t + description: "Overrides default tramp VTX power level with user defined value in mw" + default_value: -1 + field: vtxPwOverrideMw + min: -1 + max: UINT16_MAX \ No newline at end of file diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index dc714a9d30b..509af57383d 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -59,10 +59,10 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(vtxTrampPwOverride_t, VTX_TRAMP_MAX_SUPPORTED_PW void pgResetFn_vtxTrampPwOverride(vtxTrampPwOverride_t* table) { - for (int current_pw_lvl = 0; current_pw_lvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; current_pw_lvl++) + for (int currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) { - RESET_CONFIG(vtxTrampPwOverride_t, &table[current_pw_lvl], - .vtxPwOverrideMw = -1 + RESET_CONFIG(vtxTrampPwOverride_t, &table[currentPwLvl], + .vtxPwOverrideMw = VTX_TRAMP_NULL_PW_CONFIG ); } } @@ -576,11 +576,11 @@ static vtxDevice_t impl_vtxDevice = { }; // Backwards compatable mutable data structures to allow for configuration to be added. -static uint16_t mutableTablePowers[VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS]; -#define MAX_VTX_PWR_NAME_CHARS 6 -static char mutableTablePowerNames[VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS + 1][MAX_VTX_PWR_NAME_CHARS]; +static uint16_t mutablePowerTable[VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS]; +#define MAX_VTX_POWER_PLACES_DECIMAL 6 +static char mutablePowerTableNames[VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS + 1][MAX_VTX_POWER_PLACES_DECIMAL + 1]; -// Default table power levels +// Default power level tables const uint16_t trampPowerTable_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 200, 200 }; const uint16_t trampPowerTable_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 400, 400 }; const uint16_t trampPowerTable_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT] = { 25, 100, 200, 400, 600 }; @@ -588,75 +588,59 @@ const uint16_t trampPowerTable_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT] = const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 800 }; const uint16_t trampPowerTable_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 2000 }; -// Dump the VTX operating params to the console. Used for configuration validation to ensure that the VTX is being commanded as -// expected. Used to debug VTX issues such as inconsistent power levels (IE command 1 mw for first power, 2 mw for second, etc), -// frequency issues (commanded != requested), etc. This does require that the CLI serial interface is exposed, but better that than -// exposing the VTX driver internals to the CLI. void dumpLiveVtxTrampConfig(consolePrintf_t consolePrint) { - // Dump configuration - consolePrint("Configured power levels: %d\n", impl_vtxDevice.capability.powerCount); + consolePrint("PLs Configured: %d\n", impl_vtxDevice.capability.powerCount); for(uint8_t current_pl = 0; current_pl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; current_pl++) { - consolePrint("PL %d: %d mw\n", current_pl + 1, mutableTablePowers[current_pl]); + consolePrint("PL %d: %d mw\n", current_pl + 1, mutablePowerTable[current_pl]); } - consolePrint("Actual VTX Freq: %u\n", vtxState.state.freq); - consolePrint("Actual VTX Power: %u\n", vtxState.state.power); + const char actual[] = "Act"; + const char req[] = "Req"; + const char pwr[] = "Pwr"; + const char freq[] = "Freq"; + consolePrint("%s %s: %u\n", actual, freq, vtxState.state.freq); + consolePrint("%s %s: %u\n", actual, pwr, vtxState.state.power); - consolePrint("Requested VTX Freq: %u\n", vtxState.request.freq); - consolePrint("Requested VTX Power: %u\n", vtxState.request.power); - consolePrint("Requested VTX Power IDX: %u\n", vtxState.request.powerIndex); + consolePrint("%s %s: %u\n", req, freq, vtxState.request.freq); + consolePrint("%s %s: %u\n", req, pwr, vtxState.request.power); + consolePrint("%s %s IDX: %u\n", req, pwr, vtxState.request.powerIndex); } -// Construct the power table. Takes into account any configured override power values. -// baseTable - Pointer to the base table configuration -// tableCount - Entries in the table configuration -static void constructPowerTable(const uint16_t* baseTable, const uint16_t tableCount) +static void constructPowerTable(const uint16_t* defaultTable, const uint16_t defaultTableSize) { - // Update the 0th power index - strcpy(mutableTablePowerNames[0], "---"); + // Update the 0th power index to the common start entry string + strcpy(mutablePowerTableNames[0], "---"); // Now construct each table - const vtxTrampPwOverride_t* pwr_config; + const vtxTrampPwOverride_t* pwrConfig; uint16_t currentPwLvl = 0; for(currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) { - pwr_config = vtxTrampPwOverride(currentPwLvl); - if(pwr_config->vtxPwOverrideMw >= 0) + pwrConfig = vtxTrampPwOverride(currentPwLvl); + // If user defined table entry contains valid data, use it in place of the default + if(pwrConfig->vtxPwOverrideMw >= 0) { - // Only override if power is configured to a valid value. If negative, this value is not configured and - // will be set to the default table for the power level. - mutableTablePowers[currentPwLvl] = (uint16_t) pwr_config->vtxPwOverrideMw; + mutablePowerTable[currentPwLvl] = (uint16_t) pwrConfig->vtxPwOverrideMw; } - - else if(currentPwLvl < tableCount) + // Otherwise, use the default's entry. + else if(currentPwLvl < defaultTableSize) { - // Not configured, so use the default. - mutableTablePowers[currentPwLvl] = *(baseTable + currentPwLvl); + mutablePowerTable[currentPwLvl] = *(defaultTable + currentPwLvl); } else { - // No default value and no configured value. Nothing to write, so terminate from the loop. break; } - // Update the "stringified" power. Always add 1 to the working power level since 0 is reserved. - sprintf(mutableTablePowerNames[currentPwLvl + 1], "%u", mutableTablePowers[currentPwLvl]); + sprintf(mutablePowerTableNames[currentPwLvl + 1], "%u", mutablePowerTable[currentPwLvl]); } - // Update the stored params. - vtxState.metadata.powerTablePtr = mutableTablePowers; - - // NOTE: If loop broke before max (IE 2 overrides and three default, < 5, idx = 3) the last loop index will be the total - // quantity configured. Set that here. vtxState.metadata.powerTableCount = currentPwLvl; - - impl_vtxDevice.capability.powerNames = (char**) mutableTablePowerNames; + vtxState.metadata.powerTablePtr = mutablePowerTable; impl_vtxDevice.capability.powerCount = currentPwLvl; - - // NOTE: If less power levels than max supported are passed, then the upper values in the config are left as-is. - // The result is that they are not used. + impl_vtxDevice.capability.powerNames = (char**) mutablePowerTableNames; } static void vtxProtoUpdatePowerMetadata(uint16_t maxPower) diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index eb1c35b54dd..618f85cf9e9 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -40,14 +40,16 @@ #define VTX_TRAMP_1G3_MIN_FREQUENCY_MHZ 1000 #define VTX_TRAMP_1G3_MAX_FREQUENCY_MHZ 1399 -// Defines a 'zero' config value. +// Defines an unconfigured power level #define VTX_TRAMP_NULL_PW_CONFIG -1 #define VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS VTX_TRAMP_5G8_MAX_POWER_COUNT +#define VTX_TRAMP_PL_MAX_MW UINT16_MAX #define VTX_TRAMP_PW_OVERRIDE_VER 1 #define VTX_TRAMP_PL_OR_CMD "tramp_pl_override" #define VTX_TRAMP_CONFIG_DUMP_CMD "tramp_dump_config" +#define VTX_TRAMP_PL_CMD "tramp_pl_table" typedef struct { int vtxPwOverrideMw;