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..20b37fbaf43 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,106 @@ static void cliTimerOutputMode(char *cmdline) } +#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL) + +static void printTrampConfig(char* 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) +{ + if(isEmpty(cmdline)) + { + printTrampOverride(DUMP_ALL, vtxTrampPwOverride(0), NULL); + return; + } + + // Command structure is: + const char* currentCmd = cmdline++; + + int powerIdx = fastA2I(currentCmd); + if(powerIdx > VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS || powerIdx < 1) + { + cliPrint("ERROR. Cannot parse command.\n"); + return; + } + + // Convert PL Index to array indexable value + powerIdx--; + + int reqPlMw = VTX_TRAMP_NULL_PW_CONFIG; + currentCmd = nextArg(currentCmd); + if(currentCmd) + { + 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 || reqPlMw > VTX_TRAMP_PL_MAX_MW) + { + reqPlMw = VTX_TRAMP_NULL_PW_CONFIG; + } + + vtxTrampPwOverride_t* powerConfig = vtxTrampPwOverrideMutable(powerIdx); + powerConfig->vtxPwOverrideMw = reqPlMw; +} + +static void cliTrampPwTable(char* cmdline) +{ + if(isEmpty(cmdline)) + { + 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); + } + + // 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; @@ -4531,6 +4632,11 @@ static void printConfig(const char *cmdline, bool doDiff) printOsdLayout(dumpMask, &osdLayoutsConfig_Copy, osdLayoutsConfig(), -1, -1); #endif +#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL) + 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 +5048,11 @@ 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), +#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 }; static void cliHelp(char *cmdline) 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 540c9c9f222..509af57383d 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 currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) + { + RESET_CONFIG(vtxTrampPwOverride_t, &table[currentPwLvl], + .vtxPwOverrideMw = VTX_TRAMP_NULL_PW_CONFIG + ); + } +} + typedef enum { VTX_STATE_RESET = 0, VTX_STATE_OFFILE = 1, // Not detected @@ -560,41 +575,83 @@ 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 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 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 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 }; -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" }; +void dumpLiveVtxTrampConfig(consolePrintf_t consolePrint) +{ + 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, mutablePowerTable[current_pl]); + } + + 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("%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); +} + +static void constructPowerTable(const uint16_t* defaultTable, const uint16_t defaultTableSize) +{ + // Update the 0th power index to the common start entry string + strcpy(mutablePowerTableNames[0], "---"); + + // Now construct each table + const vtxTrampPwOverride_t* pwrConfig; + uint16_t currentPwLvl = 0; + for(currentPwLvl = 0; currentPwLvl < VTX_TRAMP_MAX_SUPPORTED_PW_LEVELS; currentPwLvl++) + { + pwrConfig = vtxTrampPwOverride(currentPwLvl); + // If user defined table entry contains valid data, use it in place of the default + if(pwrConfig->vtxPwOverrideMw >= 0) + { + mutablePowerTable[currentPwLvl] = (uint16_t) pwrConfig->vtxPwOverrideMw; + } + // Otherwise, use the default's entry. + else if(currentPwLvl < defaultTableSize) + { + mutablePowerTable[currentPwLvl] = *(defaultTable + currentPwLvl); + } + else + { + break; + } + + sprintf(mutablePowerTableNames[currentPwLvl + 1], "%u", mutablePowerTable[currentPwLvl]); + } + + vtxState.metadata.powerTableCount = currentPwLvl; + vtxState.metadata.powerTablePtr = mutablePowerTable; + impl_vtxDevice.capability.powerCount = currentPwLvl; + impl_vtxDevice.capability.powerNames = (char**) mutablePowerTableNames; +} 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 +661,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..618f85cf9e9 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,25 @@ #define VTX_TRAMP_1G3_MIN_FREQUENCY_MHZ 1000 #define VTX_TRAMP_1G3_MAX_FREQUENCY_MHZ 1399 +// 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; +} 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);