From a04491f0e23ae565d4608d272359fd2bdec59811 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 10 Sep 2024 18:01:12 +0100 Subject: [PATCH 01/13] Rewrite MSP VTX support for proper integration. --- src/main/fc/fc_msp.c | 16 +- src/main/fc/fc_tasks.c | 3 - src/main/io/vtx_msp.c | 722 +++++++------------------ src/main/io/vtx_msp.h | 36 +- src/main/programming/logic_condition.c | 15 +- 5 files changed, 220 insertions(+), 572 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e7a7a4aa3a6..7993dae6eef 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2686,10 +2686,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (sbufBytesRemaining(src) > 1) { uint8_t newPower = sbufReadU8(src); - uint8_t currentPower = 0; - vtxCommonGetPowerIndex(vtxDevice, ¤tPower); - if (newPower != currentPower) { - vtxCommonSetPowerByIndex(vtxDevice, newPower); + if (vtxSettingsConfig()->power != newPower) { vtxSettingsConfigMutable()->power = newPower; } @@ -2715,9 +2712,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency if (sbufBytesRemaining(src) >= 4) { uint8_t newBand = sbufReadU8(src); + if (vtxSettingsConfig()->band != newBand) { + vtxSettingsConfigMutable()->band = newBand; + } + const uint8_t newChannel = sbufReadU8(src); - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; + if (vtxSettingsConfig()->channel != newChannel) { + vtxSettingsConfigMutable()->channel = newChannel; + } } /* if (sbufBytesRemaining(src) >= 4) { @@ -3684,7 +3686,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) while (bytesCount < 80) //whole response should be less 155 bytes at worst. { bool blink1; - uint16_t lastChar; + uint16_t lastChar = 0; count = 0; while ( true ) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index afb880db526..fbe119c272b 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -120,9 +120,6 @@ void taskHandleSerial(timeUs_t currentTimeUs) #ifdef USE_MSP_OSD // Capture MSP Displayport messages to determine if VTX is connected mspOsdSerialProcess(mspFcProcessCommand); -#ifdef USE_VTX_MSP - mspVtxSerialProcess(mspFcProcessCommand); -#endif #endif } diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 696918e5705..13a3247eb14 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -17,157 +17,115 @@ * * If not, see . */ +/* Created by geoffsim */ -/* Created by phobos- */ - -#include #include -#include -#include -#include #include +#include #include "platform.h" #if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) -#include "build/debug.h" - -//#include "cms/cms_menu_vtx_msp.h" -#include "common/crc.h" #include "common/log.h" -#include "config/feature.h" - +#include "common/crc.h" #include "drivers/vtx_common.h" -//#include "drivers/vtx_table.h" - -#include "fc/runtime_config.h" -#include "flight/failsafe.h" - -#include "io/serial.h" -#include "io/vtx_msp.h" -#include "io/vtx_control.h" -#include "io/vtx_string.h" -#include "io/vtx_smartaudio.h" -#include "io/vtx.h" -#include "io/displayport_msp_osd.h" - #include "msp/msp_protocol.h" -#include "msp/msp_serial.h" -#include "msp/msp.h" - -//#include "pg/vtx_table.h" -#include "fc/settings.h" - -#include "rx/crsf.h" -//#include "rx/crsf_protocol.h" #include "rx/rx.h" - -#include "telemetry/msp_shared.h" - -//static uint16_t mspConfFreq = 0; -static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT; -static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT; -//static uint16_t mspConfPower = 0; -static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT; -static uint8_t mspConfPitMode = 0; -static bool mspVtxConfigChanged = false; -static timeUs_t mspVtxLastTimeUs = 0; -static bool prevLowPowerDisarmedState = false; - -static const vtxVTable_t mspVTable; // forward -static vtxDevice_t vtxMsp = { - .vTable = &mspVTable, - .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS, - .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS, - .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS, - .capability.bandNames = (char **)vtx58BandNames, - .capability.channelNames = (char **)vtx58ChannelNames, - .capability.powerNames = (char**)saPowerNames - +#include "rx/crsf.h" +#include "telemetry/crsf.h" +#include "vtx.h" +#include "displayport_msp_osd.h" +#include "vtx_string.h" +#include "vtx_msp.h" + +#define VTX_MSP_MIN_BAND (1) +#define VTX_MSP_MAX_BAND (VTX_MSP_MIN_BAND + VTX_MSP_BAND_COUNT - 1) +#define VTX_MSP_MIN_CHANNEL (1) +#define VTX_MSP_MAX_CHANNEL (VTX_MSP_MIN_CHANNEL + VTX_MSP_CHANNEL_COUNT -1) + +#define VTX_UPDATE_REQ_NONE 0x00 +#define VTX_UPDATE_REQ_FREQUENCY 0x01 +#define VTX_UPDATE_REQ_POWER 0x02 +#define VTX_UPDATE_REQ_PIT_MODE 0x04 + +typedef struct { + bool ready; + uint8_t timeouts; + uint8_t updateReqMask; + bool crsfTelemetryEnabled; + + struct { + uint8_t band; + uint8_t channel; + uint16_t freq; + uint8_t power; + uint8_t powerIndex; + uint8_t pitMode; + } request; +; +} vtxProtoState_t; + +const char * const vtxMspBandNames[VTX_MSP_BAND_COUNT + 1] = { + "-----", "A 2.4", "B 2.4", "E 2.4", "F 2.4", "R 2.4" }; -STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE; -static uint8_t mspVtxPortIdentifier = 255; +const char * vtxMspBandLetters = "-ABEFR"; -#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms - -static bool isCrsfPortConfig(const serialPortConfig_t *portConfig) -{ - return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF; -} - -static bool isLowPowerDisarmed(void) -{ - return (!ARMING_FLAG(ARMED) && !failsafeIsActive() && - (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS || - (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))); -} - -bool isVtxConfigValid(const vtxConfig_t *cfg) -{ - LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n"); - for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) { - - if (cfg->vtxChannelActivationConditions[i].band || - (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) || - cfg->vtxChannelActivationConditions[i].auxChannelIndex || - cfg->vtxChannelActivationConditions[i].channel) { - return true; - } - } - - LOG_DEBUG(VTX, "msp Invalid Config!\r\n"); - return false; -} +const char * const vtxMspChannelNames[VTX_MSP_CHANNEL_COUNT + 1] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8" +}; +const char * const vtxMspPowerNames[VTX_MSP_POWER_COUNT + 1] = { + "---", "25", "200", "500", "MAX" +}; -void setMspVtxDeviceStatusReady(const int descriptor) -{ - LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n"); - UNUSED(descriptor); +const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT] = { + 25, 200, 500, 1000 +}; - mspVtxStatus = MSP_VTX_STATUS_READY; - mspVtxConfigChanged = true; -} +static serialPortIdentifier_e mspVtxPortIdentifier; +static vtxProtoState_t vtxState; +static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *); +static bool vtxMspIsReady(const vtxDevice_t *); -void prepareMspFrame(uint8_t *mspFrame) +static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) { - LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n"); -/* -HDZERO parsing - fc_band_rx = msp_rx_buf[1]; - fc_channel_rx = msp_rx_buf[2]; - fc_pwr_rx = msp_rx_buf[3]; - fc_pit_rx = msp_rx_buf[4]; - fc_lp_rx = msp_rx_buf[8]; -*/ - - uint8_t pitmode = 0; - vtxCommonGetPitMode(&vtxMsp, &pitmode); - - mspFrame[0] = VTXDEV_MSP, - mspFrame[1] = vtxSettingsConfig()->band; - mspFrame[2] = vtxSettingsConfig()->channel; - mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based - mspFrame[4] = pitmode; - mspFrame[5] = 0; // Freq_L - mspFrame[6] = 0; // Freq_H - mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0; - mspFrame[8] = isLowPowerDisarmed(); - mspFrame[9] = 0; // Pitmode freq Low - mspFrame[10] = 0; // pitmod freq High + LOG_DEBUG(VTX, "msp prepareMspFrame\r\n"); + //UNUSED(vtxDevice); + + // Send an MSP_VTX_V2 frame to the VTX + + mspFrame[0] = vtxMspGetDeviceType(vtxDevice); + mspFrame[1] = vtxState.request.band; + mspFrame[2] = vtxState.request.channel; + mspFrame[3] = vtxState.request.powerIndex; + mspFrame[4] = vtxState.request.pitMode; + mspFrame[5] = 0; // Freq_L + mspFrame[6] = 0; // Freq_H + mspFrame[7] = vtxMspIsReady(vtxDevice); + mspFrame[8] = vtxSettingsConfig()->lowPowerDisarm; + mspFrame[9] = 0; // pitmode freq Low + mspFrame[10] = 0; // pitmode freq High mspFrame[11] = 0; // 1 if using vtx table - mspFrame[12] = 0; // vtx table bands or 0 - mspFrame[13] = 0; // vtx table channels or 0 - mspFrame[14] = 0; // vtx table power levels or 0 -} + mspFrame[12] = 6; // bands or 0 + mspFrame[13] = 8; // channels or 0 + mspFrame[14] = 5; // power levels or 0 + + LOG_DEBUG(VTX, "msp device [%d]\r\n", mspFrame[0]); + LOG_DEBUG(VTX, "msp band [%d]\r\n", mspFrame[1]); + LOG_DEBUG(VTX, "msp channel [%d]\r\n", mspFrame[2]); + LOG_DEBUG(VTX, "msp power [%d]\r\n", mspFrame[3]); + LOG_DEBUG(VTX, "msp freq [%d]\r\n", ((uint16_t)mspFrame[6] << 8) + mspFrame[5]); + LOG_DEBUG(VTX, "msp pitmode [%d]\r\n", mspFrame[4]); + LOG_DEBUG(VTX, "msp isready [%d]\r\n", mspFrame[7]); + LOG_DEBUG(VTX, "msp lowPower [%d]\r\n", mspFrame[8]); +} static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) { - - LOG_DEBUG(VTX, "msp CrsfPush\r\n"); + LOG_DEBUG(VTX, "msp mspCrsfPush\r\n"); #ifndef USE_TELEMETRY_CRSF UNUSED(mspCommand); UNUSED(mspFrame); @@ -201,285 +159,157 @@ static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length sbufSwitchToReader(dst, crsfFrame); - crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry + crsfRxSendTelemetryData(); // give the FC a chance to send outstanding telemetry crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); crsfRxSendTelemetryData(); #endif } -static uint16_t packetCounter = 0; - -static bool isVtxConfigChanged(void) -{ - if(mspVtxStatus == MSP_VTX_STATUS_READY) { - if (mspVtxConfigChanged == true) - return true; - - if (isLowPowerDisarmed() != prevLowPowerDisarmedState) { - LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n"); - mspVtxConfigChanged = true; - prevLowPowerDisarmedState = isLowPowerDisarmed(); - } - - if (mspConfPowerIndex != vtxSettingsConfig()->power) { - LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n"); - mspVtxConfigChanged = true; - } - - if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) { - LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n"); - mspVtxConfigChanged = true; - } - - return mspVtxConfigChanged; - } - - return false; -} - static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { + LOG_DEBUG(VTX, "msp vtxMspProcess\r\n"); UNUSED(vtxDevice); - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); - uint8_t frame[15]; + uint8_t mspFrame[15]; - switch (mspVtxStatus) { - case MSP_VTX_STATUS_OFFLINE: - LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n"); - // wait for MSP communication from the VTX -#ifdef USE_CMS - //mspCmsUpdateStatusString(); -#endif - break; - case MSP_VTX_STATUS_READY: - LOG_DEBUG(VTX, "msp MspProcess: READY\r\n"); - // send an update if stuff has changed with 200ms period - if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) { - - LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n"); - prepareMspFrame(frame); - - if (isCrsfPortConfig(portConfig)) { - mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame)); - } else { - mspPort_t *port = getMspOsdPort(); - if(port != NULL && port->port) { - LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n"); - int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE); - if (sent <= 0) { - break; - } - } + mspPort_t *mspPort = getMspOsdPort(); + unsigned lastActivity = (currentTimeUs/1000) - mspPort->lastActivityMs; + if (lastActivity > VTX_MSP_TIMEOUT) { + if (vtxState.timeouts++ > 3) { + if (vtxState.ready) { + vtxState.ready = false; } - packetCounter++; - mspVtxLastTimeUs = currentTimeUs; - mspVtxConfigChanged = false; - -#ifdef USE_CMS - //mspCmsUpdateStatusString(); -#endif } - break; - default: - mspVtxStatus = MSP_VTX_STATUS_OFFLINE; - break; + } else { // active + if (!vtxState.ready) { + vtxState.ready = true; + } } -#if 0 - DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter); - DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig)); - DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed()); -#if defined(USE_MSP_OVER_TELEMETRY) - DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier)); -#else - DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier)); -#endif -#endif + if (vtxState.ready) { + if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) { + prepareMspFrame(vtxDevice, mspFrame); + if (vtxState.crsfTelemetryEnabled) { // keep ELRS LUA up to date ? + mspCrsfPush(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame)); + } + + int sent = mspSerialPushPort(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame), mspPort, MSP_V2_NATIVE); + if (sent > 0) { + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + } + } + } } static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) { - //LOG_DEBUG(VTX, "msp GetDeviceType\r\n"); + LOG_DEBUG(VTX, "msp vtxMspGetDeviceType\r\n"); UNUSED(vtxDevice); + return VTXDEV_MSP; } static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) { - //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N"); - return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY; + LOG_DEBUG(VTX, "msp vtxMspIsReady\r\n"); + return vtxDevice != NULL && mspVtxPortIdentifier >=0 && vtxState.ready; } static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n"); + LOG_DEBUG(VTX, "msp vtxMspSetBandAndChannel\r\n"); UNUSED(vtxDevice); - if (band != mspConfBand || channel != mspConfChannel) { - LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n"); - mspVtxConfigChanged = true; + + if (band < VTX_MSP_MIN_BAND || band > VTX_MSP_MAX_BAND || channel < VTX_MSP_MIN_CHANNEL || channel > VTX_MSP_MAX_CHANNEL) { + return; } - mspConfBand = band; - mspConfChannel = channel; + + vtxState.request.band = band; + vtxState.request.channel = channel; + vtxState.request.freq = vtx58_Bandchan2Freq(band, channel); + vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY; } static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n"); + LOG_DEBUG(VTX, "msp vtxMspSetPowerByIndex\r\n"); UNUSED(vtxDevice); - if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS)) - { - if (index != mspConfPowerIndex) - { - LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n"); - mspVtxConfigChanged = true; - } - mspConfPowerIndex = index; - } + vtxState.request.power = vtxMspPowerTable[index - 1]; + vtxState.request.powerIndex = index; + vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; } -static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) { - LOG_DEBUG(VTX, "msp SetPitMode\r\n"); + LOG_DEBUG(VTX, "msp vtxMspSetPitMode\r\n"); UNUSED(vtxDevice); - if (onoff != mspConfPitMode) { - LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n"); - mspVtxConfigChanged = true; - } - mspConfPitMode = onoff; -} -#if 0 -static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) -{ - UNUSED(vtxDevice); - if (freq != mspConfFreq) { - mspVtxConfigChanged = true; - } - mspConfFreq = freq; + vtxState.request.pitMode = onOff; + vtxState.updateReqMask |= VTX_UPDATE_REQ_PIT_MODE; } -#endif - - - static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (!vtxMspIsReady(vtxDevice)) { - return false; - } - - *pBand = vtxSettingsConfig()->band; - *pChannel = vtxSettingsConfig()->channel; - - //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + LOG_DEBUG(VTX, "msp vtxMspGetBandAndChannel\r\n"); + UNUSED(vtxDevice); + *pBand = vtxState.request.band; + *pChannel = vtxState.request.channel; return true; } static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxMspIsReady(vtxDevice)) { - return false; - } - - uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; - // Special case, power not set - if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) { - *pIndex = 0; - //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); - return true; - } - - *pIndex = power; + LOG_DEBUG(VTX, "msp vtxMspGetPowerIndex\r\n"); + UNUSED(vtxDevice); - //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); + *pIndex = vtxState.request.powerIndex; return true; } -static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - LOG_DEBUG(VTX, "msp GetFreq\r\n"); - if (!vtxMspIsReady(vtxDevice)) { - return false; - } + LOG_DEBUG(VTX, "msp vtxMspGetPitMode\r\n"); + UNUSED(vtxDevice); - *pFreq = 5800; + *pOnOff = vtxState.request.pitMode; return true; } -static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) +static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) { - LOG_DEBUG(VTX, "msp GetPower\r\n"); - uint8_t powerIndex; - - if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) { - return false; - } - + LOG_DEBUG(VTX, "msp vtxMspGetFreq\r\n"); + UNUSED(vtxDevice); - *pIndex = powerIndex; - *pPowerMw = *pIndex; + *pFreq = vtxState.request.freq; return true; } -static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo) +static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) { - LOG_DEBUG(VTX, "msp GetOsdInfo\r\n"); - uint8_t powerIndex; - uint16_t powerMw; - uint16_t freq; - uint8_t band, channel; - - if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) { - return false; - } - - if (!vtxMspGetFreq(vtxDevice, &freq)) { - return false; - } - - if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) { - return false; - } + LOG_DEBUG(VTX, "msp vtxMspGetPower\r\n"); + UNUSED(vtxDevice); - pOsdInfo->band = band; - pOsdInfo->channel = channel; - pOsdInfo->frequency = freq; - pOsdInfo->powerIndex = powerIndex; - pOsdInfo->powerMilliwatt = powerMw; - pOsdInfo->bandLetter = vtx58BandNames[band][0]; - pOsdInfo->bandName = vtx58BandNames[band]; - pOsdInfo->channelName = vtx58ChannelNames[channel]; - pOsdInfo->powerIndexLetter = '0' + powerIndex; + *pIndex = vtxState.request.powerIndex; + *pPowerMw = vtxState.request.power; return true; } - -bool vtxMspInit(void) +static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *pOsdInfo) { - LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); - // don't bother setting up this device if we don't have MSP vtx enabled - // Port is shared with MSP_OSD - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); - if (!portConfig) { - return false; - } - - mspVtxPortIdentifier = portConfig->identifier; - - // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called. -#if defined(USE_VTX_COMMON) - vtxCommonSetDevice(&vtxMsp); -#endif - - mspConfBand = vtxSettingsConfig()->band; - mspConfChannel = vtxSettingsConfig()->channel; - mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based - vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode); + LOG_DEBUG(VTX, "msp vtxMspGetOsdInfo\r\n"); + UNUSED(vtxDevice); - vtxInit(); + pOsdInfo->band = vtxState.request.band; + pOsdInfo->channel = vtxState.request.channel; + pOsdInfo->frequency = vtxState.request.freq; + pOsdInfo->powerIndex = vtxState.request.powerIndex; + pOsdInfo->powerMilliwatt = vtxState.request.power; + pOsdInfo->bandName = vtxMspBandNames[vtxState.request.band]; + pOsdInfo->bandLetter = vtxMspBandLetters[vtxState.request.band]; + pOsdInfo->channelName = vtxMspChannelNames[vtxState.request.channel]; + pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex; return true; } @@ -491,211 +321,51 @@ static const vtxVTable_t mspVTable = { .setBandAndChannel = vtxMspSetBandAndChannel, .setPowerByIndex = vtxMspSetPowerByIndex, .setPitMode = vtxMspSetPitMode, - //.setFrequency = vtxMspSetFreq, .getBandAndChannel = vtxMspGetBandAndChannel, .getPowerIndex = vtxMspGetPowerIndex, + .getPitMode = vtxMspGetPitMode, .getFrequency = vtxMspGetFreq, - //.getStatus = vtxMspGetStatus, .getPower = vtxMspGetPower, - //.serializeCustomDeviceStatus = NULL, - .getOsdInfo = vtxMspGetOsdInfo, + .getOsdInfo = vtxMspGetOsdInfo }; -static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) -{ - //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n"); - UNUSED(mspPostProcessFn); - - sbuf_t *dst = &reply->buf; - sbuf_t *src = &cmd->buf; - - const unsigned int dataSize = sbufBytesRemaining(src); - UNUSED(dst); - UNUSED(src); - - // Start initializing the reply message - reply->cmd = cmd->cmd; - reply->result = MSP_RESULT_ACK; - - vtxDevice_t *vtxDevice = vtxCommonDevice(); - if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) { - LOG_DEBUG(VTX, "msp wrong vtx\r\n"); - return MSP_RESULT_ERROR; - } - - switch (cmd->cmd) - { - case MSP_VTXTABLE_BAND: - { - LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n"); - uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice); - if (deviceType == VTXDEV_MSP) - { - /* - char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1]; - memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1); - uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS]; - const uint8_t band = sbufReadU8(src); - const uint8_t bandNameLength = sbufReadU8(src); - for (int i = 0; i < bandNameLength; i++) { - const char nameChar = sbufReadU8(src); - if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) { - bandName[i] = toupper(nameChar); - } - } - const char bandLetter = toupper(sbufReadU8(src)); - const bool isFactoryBand = (bool)sbufReadU8(src); - const uint8_t channelCount = sbufReadU8(src); - for (int i = 0; i < channelCount; i++) - { - const uint16_t frequency = sbufReadU16(src); - if (i < vtxTableConfig()->channels) - { - frequencies[i] = frequency; - } - } - */ - - setMspVtxDeviceStatusReady(1); - } - break; - } - case MSP_VTXTABLE_POWERLEVEL: - { - LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n"); - - /* - char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1]; - memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1); - const uint8_t powerLevel = sbufReadU8(src); - const uint16_t powerValue = sbufReadU16(src); - const uint8_t powerLevelLabelLength = sbufReadU8(src); - for (int i = 0; i < powerLevelLabelLength; i++) - { - const char labelChar = sbufReadU8(src); - if (i < VTX_TABLE_POWER_LABEL_LENGTH) - { - powerLevelLabel[i] = toupper(labelChar); - } - } - */ - setMspVtxDeviceStatusReady(1); - } - break; - case MSP_VTX_CONFIG: - { - LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n"); - LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n"); - uint8_t pitmode = 0; - vtxCommonGetPitMode(vtxDevice, &pitmode); - - // VTXDEV_MSP, - sbufWriteU8(dst, VTXDEV_MSP); - // band; - sbufWriteU8(dst, vtxSettingsConfig()->band); - // channel; - sbufWriteU8(dst, vtxSettingsConfig()->channel); - // power; // index based - sbufWriteU8(dst, vtxSettingsConfig()->power); - // pit mode; - // Freq_L - sbufWriteU8(dst, 0); - // Freq_H - sbufWriteU8(dst, 0); - // vtx status - sbufWriteU8(dst, 1); - // lowPowerDisarm - - sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); - // Pitmode freq Low - sbufWriteU8(dst, 0); - // pitmod freq High - sbufWriteU8(dst, 0); - // 1 if using vtx table - sbufWriteU8(dst, 0); - // vtx table bands or 0 - sbufWriteU8(dst, 0); - // vtx table channels or 0 - sbufWriteU8(dst, 0); - - setMspVtxDeviceStatusReady(1); - break; - } - case MSP_SET_VTX_CONFIG: - LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n"); - if (dataSize == 15) - { - if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) - { - for (int i = 0; i < 15; ++i) - { - uint8_t data = sbufReadU8(src); - switch (i) - { - case 1: - vtxSettingsConfigMutable()->band = data; - break; - case 2: - vtxSettingsConfigMutable()->channel = data; - break; - case 3: - vtxSettingsConfigMutable()->power = data; - break; - case 4: - vtxCommonSetPitMode(vtxDevice, data); - break; - case 7: - // vtx ready - break; - case 8: - vtxSettingsConfigMutable()->lowPowerDisarm = data; - break; - } - } - } - - setMspVtxDeviceStatusReady(1); - break; - } - LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n"); - return MSP_RESULT_ERROR; - default: - // debug[1]++; - // debug[2] = cmd->cmd; - if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) { - LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd); - } - reply->result = MSP_RESULT_ERROR; - break; - } - - // Process DONT_REPLY flag - if (cmd->flags & MSP_FLAG_DONT_REPLY) - { - reply->result = MSP_RESULT_NO_REPLY; - } - - return reply->result; -} +static vtxDevice_t vtxMsp = { + .vTable = &mspVTable, + .capability.bandCount = VTX_MSP_MAX_BAND, + .capability.channelCount = VTX_MSP_MAX_CHANNEL, + .capability.powerCount = VTX_MSP_POWER_COUNT, + .capability.bandNames = (char **)vtxMspBandNames, + .capability.channelNames = (char **)vtxMspChannelNames, + .capability.powerNames = (char **)vtxMspPowerNames +}; -void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) +bool vtxMspInit(void) { - UNUSED(mspProcessCommandFn); - // Check if VTX is ready - /* - if (mspVtxStatus != MSP_VTX_STATUS_READY) { - LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n"); - return; + LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); + const serialPortConfig_t *portConfig; + + // Shares MSP_OSD port + portConfig = findSerialPortConfig(FUNCTION_VTX_MSP); + if (!portConfig) { + return false; } - */ - mspPort_t *port = getMspOsdPort(); + portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); - if(port) { - mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand); - } + vtxState.ready = false; + vtxState.timeouts = 0; + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + vtxState.crsfTelemetryEnabled = crsfRxIsActive() && checkCrsfTelemetryState(); + vtxState.request.band = vtxSettingsConfig()->band; + vtxState.request.channel = vtxSettingsConfig()->channel; + vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel); + vtxState.request.power = vtxSettingsConfig()->power; + uint8_t pitmode = 0; + vtxCommonGetPitMode(&vtxMsp, &pitmode); + vtxState.request.pitMode = pitmode; + vtxCommonSetDevice(&vtxMsp); + return true; } - #endif \ No newline at end of file diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h index 30ca245fed3..ceb2d5bea17 100644 --- a/src/main/io/vtx_msp.h +++ b/src/main/io/vtx_msp.h @@ -17,37 +17,17 @@ * * If not, see . */ +/* Created by geoffsim */ -#pragma once +#ifndef _VTX_MSP_H +#define _VTX_MSP_H -#include - -#include "build/build_config.h" - -#include "msp/msp_protocol.h" -#include "msp/msp_serial.h" - -typedef enum { - // Offline - device hasn't responded yet - MSP_VTX_STATUS_OFFLINE = 0, - MSP_VTX_STATUS_READY, -} mspVtxStatus_e; - -typedef struct mspPowerTable_s { - int mW; // rfpower - int16_t dbi; // valueV1 -} mspPowerTable_t; - -#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands -#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels -#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT -#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1 -#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8 -#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3 +#define VTX_MSP_TIMEOUT 250 // ms +#define VTX_MSP_BAND_COUNT 5 +#define VTX_MSP_CHANNEL_COUNT 8 +#define VTX_MSP_POWER_COUNT 4 bool vtxMspInit(void); -void setMspVtxDeviceStatusReady(const int descriptor); -void prepareMspFrame(uint8_t *mspFrame); -void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); +#endif // _VTX_MSP_H diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 594db21417c..b078e25f751 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -296,14 +296,13 @@ static int logicConditionCompute( #endif case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: #if defined(USE_VTX_CONTROL) -#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); - vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) + uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; + if ( newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_MAX_POWER); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newpower; + vtxSettingsConfigMutable()->power = newpower; + return newpower; } else { return false; } From fd71bed9a2dbc2b38a4f1bfa066a2d0f57c1b4a2 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Thu, 12 Sep 2024 16:52:37 +0100 Subject: [PATCH 02/13] Restrict band/channel/power in logic conditions to the specific device capabilities. Add one to the power to allow HDZERO 0mW option. --- src/main/programming/logic_condition.c | 38 ++++++++++++-------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index b078e25f751..f02cdc1fa76 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -294,12 +294,13 @@ static int logicConditionCompute( return true; break; #endif - case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: + #if defined(USE_VTX_CONTROL) -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) + case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - if ( newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_MAX_POWER); + if (newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + // HDZERO VTX max power+1 is 0mW. + newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newpower; vtxSettingsConfigMutable()->power = newpower; return newpower; @@ -307,30 +308,25 @@ static int logicConditionCompute( return false; } break; -#else - return false; -#endif case LOGIC_CONDITION_SET_VTX_BAND: - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND); - vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + uint8_t newband = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + if (newband != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newband = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newband; + vtxSettingsConfigMutable()->band = newband; + return newband; } else { return false; } break; case LOGIC_CONDITION_SET_VTX_CHANNEL: - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL); - vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + uint8_t newchannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + if (newchannel != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newchannel = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.channelCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newchannel; + vtxSettingsConfigMutable()->band = newchannel; + return newchannel; } else { return false; } From 77a0f20970c67d502c4f265b139e9190b0ea4079 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Thu, 12 Sep 2024 18:27:31 +0100 Subject: [PATCH 03/13] Move crsf include to the correct header. Include uint definitions. --- src/main/programming/logic_condition.c | 1 + src/main/rx/crsf.h | 1 + src/main/telemetry/crsf.h | 1 - 3 files changed, 2 insertions(+), 1 deletion(-) mode change 100755 => 100644 src/main/telemetry/crsf.h diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f02cdc1fa76..bdc008af8a8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -22,6 +22,7 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include #include #include "config/config_reset.h" diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 69777a0390c..da7aa9024e2 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -133,5 +133,6 @@ struct rxConfig_s; struct rxRuntimeConfig_s; bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); bool crsfRxIsActive(void); +bool checkCrsfTelemetryState(void); void crsfBind(void); diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h old mode 100755 new mode 100644 index 6ffa4df3588..f738c71bf28 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -24,7 +24,6 @@ #define CRSF_MSP_TX_BUF_SIZE 128 void initCrsfTelemetry(void); -bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); void crsfScheduleMspResponse(void); From 05fa7e6dee895dbcf8194f58a9514e58327289ec Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 13 Sep 2024 13:06:38 +0100 Subject: [PATCH 04/13] Fix SITL compile issues. Move telemetry function definition back to correct place. --- src/main/io/vtx_msp.c | 2 +- src/main/programming/logic_condition.c | 12 ++++++++---- src/main/rx/crsf.h | 1 - src/main/telemetry/crsf.h | 1 + 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 13a3247eb14..7868fed6717 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -355,7 +355,7 @@ bool vtxMspInit(void) vtxState.ready = false; vtxState.timeouts = 0; vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; - vtxState.crsfTelemetryEnabled = crsfRxIsActive() && checkCrsfTelemetryState(); + vtxState.crsfTelemetryEnabled = crsfRxIsActive(); vtxState.request.band = vtxSettingsConfig()->band; vtxState.request.channel = vtxSettingsConfig()->channel; vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index bdc008af8a8..98ae76f8710 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -22,7 +22,6 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ -#include #include #include "config/config_reset.h" @@ -298,6 +297,7 @@ static int logicConditionCompute( #if defined(USE_VTX_CONTROL) case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: + { uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; if (newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { // HDZERO VTX max power+1 is 0mW. @@ -309,8 +309,9 @@ static int logicConditionCompute( return false; } break; - + } case LOGIC_CONDITION_SET_VTX_BAND: + { uint8_t newband = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; if (newband != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newband = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); @@ -321,17 +322,20 @@ static int logicConditionCompute( return false; } break; + } case LOGIC_CONDITION_SET_VTX_CHANNEL: + { uint8_t newchannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; if (newchannel != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newchannel = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.channelCount); + newchannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newchannel; - vtxSettingsConfigMutable()->band = newchannel; + vtxSettingsConfigMutable()->channel = newchannel; return newchannel; } else { return false; } break; + } #endif case LOGIC_CONDITION_INVERT_ROLL: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL); diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index da7aa9024e2..69777a0390c 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -133,6 +133,5 @@ struct rxConfig_s; struct rxRuntimeConfig_s; bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); bool crsfRxIsActive(void); -bool checkCrsfTelemetryState(void); void crsfBind(void); diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index f738c71bf28..6ffa4df3588 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -24,6 +24,7 @@ #define CRSF_MSP_TX_BUF_SIZE 128 void initCrsfTelemetry(void); +bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); void crsfScheduleMspResponse(void); From 73857e22b8793aa5405cc4d5480b99800aa81de4 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sat, 14 Sep 2024 18:56:04 +0100 Subject: [PATCH 05/13] Remove debug code. Simplify initialisation. --- src/main/io/vtx_msp.c | 22 +--------------------- 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 7868fed6717..9271f2e5eca 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -92,9 +92,6 @@ static bool vtxMspIsReady(const vtxDevice_t *); static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) { - LOG_DEBUG(VTX, "msp prepareMspFrame\r\n"); - //UNUSED(vtxDevice); - // Send an MSP_VTX_V2 frame to the VTX mspFrame[0] = vtxMspGetDeviceType(vtxDevice); @@ -125,7 +122,6 @@ static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) { - LOG_DEBUG(VTX, "msp mspCrsfPush\r\n"); #ifndef USE_TELEMETRY_CRSF UNUSED(mspCommand); UNUSED(mspFrame); @@ -167,7 +163,6 @@ static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { - LOG_DEBUG(VTX, "msp vtxMspProcess\r\n"); UNUSED(vtxDevice); uint8_t mspFrame[15]; @@ -203,21 +198,17 @@ static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) { - LOG_DEBUG(VTX, "msp vtxMspGetDeviceType\r\n"); UNUSED(vtxDevice); - return VTXDEV_MSP; } static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) { - LOG_DEBUG(VTX, "msp vtxMspIsReady\r\n"); return vtxDevice != NULL && mspVtxPortIdentifier >=0 && vtxState.ready; } static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - LOG_DEBUG(VTX, "msp vtxMspSetBandAndChannel\r\n"); UNUSED(vtxDevice); if (band < VTX_MSP_MIN_BAND || band > VTX_MSP_MAX_BAND || channel < VTX_MSP_MIN_CHANNEL || channel > VTX_MSP_MAX_CHANNEL) { @@ -232,7 +223,6 @@ static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_ static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - LOG_DEBUG(VTX, "msp vtxMspSetPowerByIndex\r\n"); UNUSED(vtxDevice); vtxState.request.power = vtxMspPowerTable[index - 1]; @@ -242,7 +232,6 @@ static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) { - LOG_DEBUG(VTX, "msp vtxMspSetPitMode\r\n"); UNUSED(vtxDevice); vtxState.request.pitMode = onOff; @@ -251,7 +240,6 @@ static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - LOG_DEBUG(VTX, "msp vtxMspGetBandAndChannel\r\n"); UNUSED(vtxDevice); *pBand = vtxState.request.band; @@ -261,7 +249,6 @@ static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - LOG_DEBUG(VTX, "msp vtxMspGetPowerIndex\r\n"); UNUSED(vtxDevice); *pIndex = vtxState.request.powerIndex; @@ -270,7 +257,6 @@ static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - LOG_DEBUG(VTX, "msp vtxMspGetPitMode\r\n"); UNUSED(vtxDevice); *pOnOff = vtxState.request.pitMode; @@ -279,7 +265,6 @@ static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) { - LOG_DEBUG(VTX, "msp vtxMspGetFreq\r\n"); UNUSED(vtxDevice); *pFreq = vtxState.request.freq; @@ -288,7 +273,6 @@ static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) { - LOG_DEBUG(VTX, "msp vtxMspGetPower\r\n"); UNUSED(vtxDevice); *pIndex = vtxState.request.powerIndex; @@ -298,7 +282,6 @@ static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16 static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *pOsdInfo) { - LOG_DEBUG(VTX, "msp vtxMspGetOsdInfo\r\n"); UNUSED(vtxDevice); pOsdInfo->band = vtxState.request.band; @@ -341,7 +324,6 @@ static vtxDevice_t vtxMsp = { bool vtxMspInit(void) { - LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); const serialPortConfig_t *portConfig; // Shares MSP_OSD port @@ -360,9 +342,7 @@ bool vtxMspInit(void) vtxState.request.channel = vtxSettingsConfig()->channel; vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel); vtxState.request.power = vtxSettingsConfig()->power; - uint8_t pitmode = 0; - vtxCommonGetPitMode(&vtxMsp, &pitmode); - vtxState.request.pitMode = pitmode; + vtxState.request.pitMode = 0; vtxCommonSetDevice(&vtxMsp); return true; From 2f0d50362d84c3ce37f0b09068e3abe1e5b16bff Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 17 Sep 2024 13:29:11 +0100 Subject: [PATCH 06/13] Remove direct call to band and channel setting. --- src/main/fc/fc_msp.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7993dae6eef..08b7b336ae7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2675,13 +2675,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *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; - - if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { - vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel); + if (vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; } - - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; } if (sbufBytesRemaining(src) > 1) { From 3009d7d86c3469f8a8fc9f18c4df069ff588bf1a Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 17 Sep 2024 13:30:28 +0100 Subject: [PATCH 07/13] Ensure VTX logic switches take priority over FC/VTX menu settings. --- src/main/programming/logic_condition.c | 53 ++++++++++++++------------ 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 98ae76f8710..9ca587ad9c8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -298,42 +298,45 @@ static int logicConditionCompute( #if defined(USE_VTX_CONTROL) case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: { - uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - if (newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - // HDZERO VTX max power+1 is 0mW. - newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newpower; - vtxSettingsConfigMutable()->power = newpower; - return newpower; - } else { - return false; + if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + uint8_t power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; + if (power != operandA || power != vtxSettingsConfig()->power) { + // HDZERO VTX max power+1 is 0mW. + power = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = power; + vtxSettingsConfigMutable()->power = power; + return power; + } } + return false; break; } case LOGIC_CONDITION_SET_VTX_BAND: { - uint8_t newband = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - if (newband != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newband = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newband; - vtxSettingsConfigMutable()->band = newband; - return newband; - } else { - return false; + if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + uint8_t band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + if (band != operandA || band != vtxSettingsConfig()->band) { + band = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = band; + vtxSettingsConfigMutable()->band = band; + return band; + } } + return false; break; } case LOGIC_CONDITION_SET_VTX_CHANNEL: { - uint8_t newchannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - if (newchannel != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newchannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newchannel; - vtxSettingsConfigMutable()->channel = newchannel; - return newchannel; - } else { - return false; + if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + uint8_t channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + if (channel != operandA || channel != vtxSettingsConfig()->channel) { + channel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = channel; + vtxSettingsConfigMutable()->channel = channel; + return channel; + } } + return false; break; } #endif From 79026ed027bb75a105ca28fb8b37f4db806fabf4 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 20 Sep 2024 15:31:30 +0100 Subject: [PATCH 08/13] Include MSP VTX with Smart Audio and Tramp defines. --- src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_adjustments.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index fbe119c272b..97cf005a5b6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -406,7 +406,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW)); #endif #ifdef USE_VTX_CONTROL -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 37c338fedd8..27b88800841 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -576,7 +576,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); break; -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) case ADJUSTMENT_VTX_POWER_LEVEL: { vtxDeviceCapability_t vtxDeviceCapability; From defd28358411c3a58183c5a52a8b8aef72992f9f Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 20 Sep 2024 15:34:07 +0100 Subject: [PATCH 09/13] Allow VTX to specify number of power levels available. --- src/main/fc/fc_msp.c | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 08b7b336ae7..0601cd89a78 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2699,9 +2699,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); } - // ////////////////////////////////////////////////////////// - // this code is taken from BF, it's hack for HDZERO VTX MSP frame - // API version 1.42 - this parameter kept separate since clients may already be supplying + // API version 1.42 - extension for pitmode frequency if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); //skip pitModeFreq } @@ -2719,13 +2717,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } } - /* if (sbufBytesRemaining(src) >= 4) { - sbufRead8(src); // freq_l - sbufRead8(src); // freq_h - sbufRead8(src); // band count - sbufRead8(src); // channel count - }*/ - // ////////////////////////////////////////////////////////// + if (sbufBytesRemaining(src) >= 5) { + sbufReadU16(src); // freq + sbufReadU8(src); // band count + sbufReadU8(src); // channel count + + uint8_t newPowerCount = sbufReadU8(src); + if (newPowerCount > 0 && newPowerCount < (vtxDevice->capability.powerCount)) { + vtxDevice->capability.powerCount = newPowerCount; + } + } } } } From f4abd1c3b7e7ad72259e6c753f34bda1024ddf76 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 20 Sep 2024 15:36:58 +0100 Subject: [PATCH 10/13] Allow FC Logic to specifiy a VTX power level of 0. --- src/main/programming/logic_condition.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 9ca587ad9c8..6501ab527c5 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -301,8 +301,7 @@ static int logicConditionCompute( if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { uint8_t power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; if (power != operandA || power != vtxSettingsConfig()->power) { - // HDZERO VTX max power+1 is 0mW. - power = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); + power = constrain(operandA, 0, vtxDeviceCapability.powerCount); // Allow a power level of 0 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = power; vtxSettingsConfigMutable()->power = power; return power; From c7d55874b88047eade4430929fad1cf5cc0bc278 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Thu, 26 Sep 2024 13:46:03 +0100 Subject: [PATCH 11/13] Allow min power of 0 (will need configurator change at some point). Set band/channel/power max values sent from VTX. Optimise VTX logic condition code. --- src/main/drivers/vtx_common.h | 4 +-- src/main/fc/fc_msp.c | 5 ++- src/main/io/vtx_msp.c | 25 +++++---------- src/main/programming/logic_condition.c | 42 +++++++++++--------------- 4 files changed, 32 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 80b957c5d13..83e608dc49b 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -36,11 +36,11 @@ #define VTX_SETTINGS_POWER_COUNT 8 #define VTX_SETTINGS_DEFAULT_POWER 1 -#define VTX_SETTINGS_MIN_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 -#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1) +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER) #else diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 69552d03173..bfa57bee833 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2708,8 +2708,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } } - if (sbufBytesRemaining(src) >= 5) { + if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); // freq + } + + if (sbufBytesRemaining(src) >= 3) { sbufReadU8(src); // band count sbufReadU8(src); // channel count diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 9271f2e5eca..84746e0c9f3 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -77,11 +77,11 @@ const char * const vtxMspChannelNames[VTX_MSP_CHANNEL_COUNT + 1] = { }; const char * const vtxMspPowerNames[VTX_MSP_POWER_COUNT + 1] = { - "---", "25", "200", "500", "MAX" + "0", "25", "200", "500", "MAX" }; -const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT] = { - 25, 200, 500, 1000 +const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT + 1] = { + 0, 25, 200, 500, 1000 }; static serialPortIdentifier_e mspVtxPortIdentifier; @@ -89,11 +89,11 @@ static vtxProtoState_t vtxState; static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *); static bool vtxMspIsReady(const vtxDevice_t *); +static vtxDevice_t vtxMsp; static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) { // Send an MSP_VTX_V2 frame to the VTX - mspFrame[0] = vtxMspGetDeviceType(vtxDevice); mspFrame[1] = vtxState.request.band; mspFrame[2] = vtxState.request.channel; @@ -106,18 +106,9 @@ static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) mspFrame[9] = 0; // pitmode freq Low mspFrame[10] = 0; // pitmode freq High mspFrame[11] = 0; // 1 if using vtx table - mspFrame[12] = 6; // bands or 0 - mspFrame[13] = 8; // channels or 0 - mspFrame[14] = 5; // power levels or 0 - - LOG_DEBUG(VTX, "msp device [%d]\r\n", mspFrame[0]); - LOG_DEBUG(VTX, "msp band [%d]\r\n", mspFrame[1]); - LOG_DEBUG(VTX, "msp channel [%d]\r\n", mspFrame[2]); - LOG_DEBUG(VTX, "msp power [%d]\r\n", mspFrame[3]); - LOG_DEBUG(VTX, "msp freq [%d]\r\n", ((uint16_t)mspFrame[6] << 8) + mspFrame[5]); - LOG_DEBUG(VTX, "msp pitmode [%d]\r\n", mspFrame[4]); - LOG_DEBUG(VTX, "msp isready [%d]\r\n", mspFrame[7]); - LOG_DEBUG(VTX, "msp lowPower [%d]\r\n", mspFrame[8]); + mspFrame[12] = vtxMsp.capability.bandCount; // bands or 0 + mspFrame[13] = vtxMsp.capability.channelCount; // channels or 0 + mspFrame[14] = vtxMsp.capability.powerCount; // power levels or 0 } static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) @@ -225,7 +216,7 @@ static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { UNUSED(vtxDevice); - vtxState.request.power = vtxMspPowerTable[index - 1]; + vtxState.request.power = vtxMspPowerTable[index]; vtxState.request.powerIndex = index; vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; } diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 6501ab527c5..e99ab92cfe9 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -298,42 +298,36 @@ static int logicConditionCompute( #if defined(USE_VTX_CONTROL) case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: { - if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - uint8_t power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - if (power != operandA || power != vtxSettingsConfig()->power) { - power = constrain(operandA, 0, vtxDeviceCapability.powerCount); // Allow a power level of 0 - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = power; - vtxSettingsConfigMutable()->power = power; - return power; - } + uint8_t newPower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; + if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower; + vtxSettingsConfigMutable()->power = newPower; + return newPower; } return false; break; } case LOGIC_CONDITION_SET_VTX_BAND: { - if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - uint8_t band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - if (band != operandA || band != vtxSettingsConfig()->band) { - band = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = band; - vtxSettingsConfigMutable()->band = band; - return band; - } + uint8_t newBand = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + if ((newBand != operandA || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand; + vtxSettingsConfigMutable()->band = newBand; + return newBand; } return false; break; } case LOGIC_CONDITION_SET_VTX_CHANNEL: { - if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - uint8_t channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - if (channel != operandA || channel != vtxSettingsConfig()->channel) { - channel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = channel; - vtxSettingsConfigMutable()->channel = channel; - return channel; - } + uint8_t newChannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + if ((newChannel != operandA || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel; + vtxSettingsConfigMutable()->channel = newChannel; + return newChannel; } return false; break; From b41bcdc1b02ea870b6a0380acb1d9f4e70389b38 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 27 Sep 2024 14:15:37 +0100 Subject: [PATCH 12/13] Ensure VTX logic settings override any external changes. --- src/main/programming/logic_condition.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index e99ab92cfe9..697091ff7a0 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -302,6 +302,9 @@ static int logicConditionCompute( if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower; + if (newPower != vtxSettingsConfig()->power) { + vtxCommonSetPowerByIndex(vtxCommonDevice(), newPower); // Force setting if modified elsewhere + } vtxSettingsConfigMutable()->power = newPower; return newPower; } @@ -314,6 +317,9 @@ static int logicConditionCompute( if ((newBand != operandA || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand; + if (newBand != vtxSettingsConfig()->band) { + vtxCommonSetPowerByIndex(vtxCommonDevice(), newBand); + } vtxSettingsConfigMutable()->band = newBand; return newBand; } @@ -326,6 +332,9 @@ static int logicConditionCompute( if ((newChannel != operandA || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel; + if (newChannel != vtxSettingsConfig()->channel) { + vtxCommonSetPowerByIndex(vtxCommonDevice(), newChannel); + } vtxSettingsConfigMutable()->channel = newChannel; return newChannel; } From ac6a1de0885412c3c119e8fc92edeafc132b55f4 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Mon, 7 Oct 2024 16:56:18 +0100 Subject: [PATCH 13/13] Fix cut/paste error in band and channel logic override. --- src/main/programming/logic_condition.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 697091ff7a0..1c19889fe47 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -318,7 +318,7 @@ static int logicConditionCompute( newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand; if (newBand != vtxSettingsConfig()->band) { - vtxCommonSetPowerByIndex(vtxCommonDevice(), newBand); + vtxCommonSetBandAndChannel(vtxCommonDevice(), newBand, vtxSettingsConfig()->channel); } vtxSettingsConfigMutable()->band = newBand; return newBand; @@ -333,7 +333,7 @@ static int logicConditionCompute( newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel; if (newChannel != vtxSettingsConfig()->channel) { - vtxCommonSetPowerByIndex(vtxCommonDevice(), newChannel); + vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxSettingsConfig()->band, newChannel); } vtxSettingsConfigMutable()->channel = newChannel; return newChannel;