From dc26a83bcedce5415651041d29b569ccf5d8be1f Mon Sep 17 00:00:00 2001 From: functionpointer Date: Tue, 28 May 2019 16:56:23 +0200 Subject: [PATCH] [SmartAudio] Fixed power logic for SA2.0 and 1.0 The SmartAudio implementation now uses indices of vtxTable in saDevice.power. This requires an index lookup in saProcessResponse, which was previously only present for SA2.1 devices. --- src/main/io/vtx_smartaudio.c | 35 +++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 002933ff2..a3acb1f15 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -279,9 +279,7 @@ static void saProcessResponse(uint8_t *buf, int len) // saDevice.version = 0 means unknown, 1 means Smart audio V1, 2 means Smart audio V2 and 3 means Smart audio V2.1 saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : ((buf[0] == SA_CMD_GET_SETTINGS_V2) ? 2 : 3); saDevice.channel = buf[2]; - if(saDevice.version != 3) { - saDevice.power = buf[3]; - } + uint8_t rawpowervalue = buf[3]; saDevice.mode = buf[4]; saDevice.freq = (buf[5] << 8)|buf[6]; @@ -328,17 +326,22 @@ static void saProcessResponse(uint8_t *buf, int len) vtxSmartAudio.capability.powerCount, vtxSmartAudio.powerValues[0], vtxSmartAudio.powerValues[1], vtxSmartAudio.powerValues[2], vtxSmartAudio.powerValues[3])); //dprintf(("processResponse: V2.1 received vtx power value %d\r\n",buf[7])); - - saDevice.power = 0;//set to unknown power level if the reported one doesnt match any of the known ones - for (int8_t i = 0; i < vtxSmartAudio.capability.powerCount; i++) { - if(buf[7] == vtxSmartAudio.powerValues[i]) { + rawpowervalue = buf[7]; + } #ifdef USE_SMARTAUDIO_DPRINTF - if (saDevice.power != i + 1) { - dprintf(("processResponse: V2.1 power changed from index %d to index %d\r\n", saDevice.power, i + 1)); - } + int8_t prevpower = saDevice.power; #endif - saDevice.power = i + 1; + saDevice.power = 0;//set to unknown power level if the reported one doesnt match any of the known ones + dprintf(("processResponse: rawpowervalue is %d, legacy power is %d\r\n", rawpowervalue, buf[3])); + for (int8_t i = 0; i < vtxSmartAudio.capability.powerCount; i++) { + if(rawpowervalue == vtxSmartAudio.powerValues[i]) { +#ifdef USE_SMARTAUDIO_DPRINTF + if (prevpower != i + 1) { + dprintf(("processResponse: power changed from index %d to index %d\r\n", prevpower, i + 1)); } +#endif + saDevice.power = i + 1; + } } @@ -713,8 +716,8 @@ bool vtxSmartAudioInit(void) debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, NULL, 115200, MODE_RXTX, 0); if (debugSerialPort) { setPrintfSerialPort(debugSerialPort); - dprintf(("smartAudioInit: OK\r\n")); } + dprintf(("smartAudioInit: OK\r\n")); #endif serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); @@ -856,17 +859,17 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) if ((sa_outstanding != SA_CMD_NONE) && (nowMs - sa_lastTransmissionMs > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out - // dprintf(("process: resending 0x%x\r\n", sa_outstanding)); + dprintf(("process: resending 0x%x\r\n", sa_outstanding)); // XXX Todo: Resend termination and possible offline transition saResendCmd(); lastCommandSentMs = nowMs; } else if (!saQueueEmpty()) { // Command pending. Send it. - // dprintf(("process: sending queue\r\n")); + dprintf(("process: sending queue\r\n")); saSendQueue(); lastCommandSentMs = nowMs; } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) { - //dprintf(("process: sending status change polling\r\n")); + dprintf(("process: sending status change polling\r\n")); saGetSettings(); saSendQueue(); } @@ -949,7 +952,7 @@ static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) newMode |= SA_MODE_SET_OUT_RANGE_PITMODE; } - if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE || !(saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)) { + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE || (onoff && !(saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE))) { // ensure when turning on pit mode that pit mode gets actually enabled newMode |= SA_MODE_SET_IN_RANGE_PITMODE; }