diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index a0c78f8c4..29b8af83c 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/src/main/cms/cms_menu_vtx_smartaudio.c @@ -70,6 +70,7 @@ uint16_t saCmsDeviceFreq = 0; uint8_t saCmsDeviceStatus = 0; uint8_t saCmsPower; +uint8_t saCmsPit; uint8_t saCmsPitFMode; // Undef(0), In-Range(1) or Out-Range(2) uint8_t saCmsFselMode; // Channel(0) or User defined(1) @@ -86,7 +87,7 @@ void saCmsUpdate(void) // XXX Take care of pit mode update somewhere??? if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { // This is a first valid response to GET_SETTINGS. - saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; + saCmsOpmodel = saDevice.willBootIntoPitMode ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0; @@ -125,6 +126,7 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self); static long saCmsConfigBandByGvar(displayPort_t *, const void *self); static long saCmsConfigChanByGvar(displayPort_t *, const void *self); static long saCmsConfigPowerByGvar(displayPort_t *, const void *self); +static long saCmsConfigPitByGvar(displayPort_t *, const void *self); void saUpdateStatusString(void) { @@ -144,6 +146,8 @@ if (saDevice.version == 2) { saCmsPitFMode = 1; else saCmsPitFMode = 0; +} else if (saDevice.version == 3) { + saCmsPitFMode = 1;//V2.1 only supports PIR } const vtxDevice_t *device = vtxCommonDevice(); @@ -169,18 +173,26 @@ if (saDevice.version == 2) { saCmsStatusString[9] = ' '; if (saDevice.mode & SA_MODE_GET_PITMODE) { + saCmsPit = 2; saCmsStatusString[10] = 'P'; - if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { - saCmsStatusString[11] = 'I'; - } else { + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) { saCmsStatusString[11] = 'O'; + } else { + saCmsStatusString[11] = 'I'; } saCmsStatusString[12] = 'R'; saCmsStatusString[13] = 0; } else { - int index = (saDevice.version == 2) ? saDevice.power : saDacToPowerIndex(saDevice.power); - tfp_sprintf(&saCmsStatusString[10], "%s", vtxCommonLookupPowerName(vtxCommonDevice(), index + 1)); + saCmsPit = 1; + uint8_t powerIndex = 0; + bool powerFound = vtxCommonGetPowerIndex(vtxCommonDevice(), &powerIndex); + tfp_sprintf(&saCmsStatusString[10], "%s", powerFound ? vtxCommonLookupPowerName(vtxCommonDevice(), powerIndex) : "???"); } +#if defined(USE_VTX_TABLE) + if (vtxCommonDevice()->capability.bandCount == 0 || vtxCommonDevice()->capability.powerCount == 0) { + strncpy(saCmsStatusString, "PLEASE CONFIGURE VTXTABLE", sizeof(saCmsStatusString)); + } +#endif } void saCmsResetOpmodel() @@ -239,6 +251,30 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self) return 0; } +static long saCmsConfigPitByGvar(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + dprintf(("saCmsConfigPitByGvar: saCmsPit %d\r\n", saCmsPit)); + + if (saDevice.version == 0) { + // Bounce back; not online yet + saCmsPit = 0; + return 0; + } + + if (saCmsPit == 0) {//trying to go back to undef state; bounce back + saCmsPit = 1; + return 0; + } + if (saDevice.power != saCmsPower) {//we can't change both power and pit mode at once + saCmsPower = saDevice.power; + } + + return 0; +} + static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); @@ -256,9 +292,14 @@ static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self) return 0; } + if (saCmsPit > 0 && saCmsPit != 1 ) { + saCmsPit = 1; + } + if (saCmsOpmodel == SACMS_OPMODEL_FREE && !saDeferred) { vtxSettingsConfigMutable()->power = saCmsPower; } + dprintf(("saCmsConfigPowerByGvar: power index is now %d\r\n", saCmsPower)); return 0; } @@ -273,6 +314,12 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self) saCmsPitFMode = 0; return 0; } + if (saDevice.version >= 3) { + // V2.1 device only supports PIR mode. and setting any flag immediately enables pit mode. + //therefore: bounce back. + saCmsPitFMode = 1; + return 0; + } dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode)); @@ -318,6 +365,9 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) // out-range receivers from getting blinded. saCmsPitFMode = 0; saCmsConfigPitFModeByGvar(pDisp, self); + if(saDevice.version >= 3) { + saSetMode(SA_MODE_SET_IN_RANGE_PITMODE | ((saDevice.mode & SA_MODE_GET_PITMODE) ? 0 : SA_MODE_CLR_PITMODE)); + } // Direct frequency mode is not available in RACE opmodel saCmsFselModeNew = 0; @@ -335,9 +385,10 @@ static const char * const saCmsDeviceStatusNames[] = { "OFFL", "ONL V1", "ONL V2", + "ONLV21", }; -static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames }; +static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 3, saCmsDeviceStatusNames }; static const OSD_Entry saCmsMenuStatsEntries[] = { { "- SA STATS -", OME_Label, NULL, NULL, 0 }, @@ -404,7 +455,14 @@ static const char * const saCmsPitFModeNames[] = { "POR" }; +static const char * const saCmsPitNames[] = { + "---", + "OFF", + "ON ", +}; + static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames }; +static OSD_TAB_t saCmsEntPit = {&saCmsPit , 2, saCmsPitNames}; static long sacms_SetupTopMenu(void); // Forward @@ -448,12 +506,6 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) newSettings.band = saCmsBand; newSettings.channel = saCmsChan; newSettings.freq = vtxCommonLookupFrequency(vtxCommonDevice(), saCmsBand, saCmsChan); - // If in pit mode, cancel it. - - if (saCmsPitFMode == 0) - saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); - else - saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); } else { // Freestyle model // Setup band and freq / user freq @@ -468,6 +520,9 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) } } + if(newSettings.power == saCmsPower && saCmsPit > 0) { + vtxCommonSetPitMode(vtxCommonDevice(), saCmsPit == 2); + } newSettings.power = saCmsPower; if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) { @@ -631,7 +686,8 @@ static const OSD_Entry saCmsMenuFreqModeEntries[] = { { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, { "FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsUserFreqGetString, &saCmsMenuUserFreq, OPTSTRING }, - { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, DYNAMIC }, + { "PIT", OME_TAB, saCmsConfigPitByGvar, &saCmsEntPit, DYNAMIC }, { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, @@ -647,7 +703,8 @@ static const OSD_Entry saCmsMenuChanModeEntries[] = { "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 }, { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreqRef, DYNAMIC }, - { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, DYNAMIC }, + { "PIT", OME_TAB, saCmsConfigPitByGvar, &saCmsEntPit, DYNAMIC }, { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 8e6ea30a7..24f46fe75 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -207,8 +207,8 @@ const char *vtxCommonLookupPowerName(const vtxDevice_t *vtxDevice, int index) uint16_t vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index) { - if (vtxDevice) { - return vtxDevice->powerValues[index]; + if (vtxDevice && index > 0) { + return vtxDevice->powerValues[index - 1]; } else { return 0; } diff --git a/src/main/drivers/vtx_table.c b/src/main/drivers/vtx_table.c index 9eed732ae..d6a30265c 100644 --- a/src/main/drivers/vtx_table.c +++ b/src/main/drivers/vtx_table.c @@ -69,7 +69,7 @@ void vtxTableInit(void) } vtxTableChannelNames[0] = "-"; - for (int level = 0; level < config->powerLevels; level++) { + for (int level = 0; level < VTX_TABLE_MAX_POWER_LEVELS; level++) { vtxTablePowerValues[level] = config->powerValues[level]; vtxTablePowerLabels[level + 1] = config->powerLabels[level]; } diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 803801ab0..002933ff2 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -40,6 +40,7 @@ #include "drivers/time.h" #include "drivers/vtx_common.h" +#include "drivers/vtx_table.h" #include "io/serial.h" #include "io/vtx.h" @@ -54,9 +55,6 @@ #define SMARTAUDIO_POLLING_INTERVAL 150 // Minimum time between state polling #define SMARTAUDIO_POLLING_WINDOW 1000 // Time window after command polling for state change -//#define USE_SMARTAUDIO_DPRINTF -//#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1 - #ifdef USE_SMARTAUDIO_DPRINTF serialPort_t *debugSerialPort = NULL; #endif // USE_SMARTAUDIO_DPRINTF @@ -65,12 +63,6 @@ static serialPort_t *smartAudioSerialPort = NULL; smartAudioDevice_t saDevice; -#if defined(USE_CMS) || defined(USE_VTX_COMMON) -const char * saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = { - "---", "25 ", "200", "500", "800", -}; -#endif - #ifdef USE_VTX_COMMON static const vtxVTable_t saVTable; // Forward static vtxDevice_t vtxSmartAudio = { @@ -117,23 +109,12 @@ smartAudioStat_t saStat = { .badcode = 0, }; -saPowerTable_t saPowerTable[VTX_SMARTAUDIO_POWER_COUNT] = { - { 25, 7, 0 }, - { 200, 16, 1 }, - { 500, 25, 2 }, - { 800, 40, 3 }, -}; - -uint16_t saPowerValues[VTX_SMARTAUDIO_POWER_COUNT] = { - 25, 200, 500, 800 -}; - // Last received device ('hard') states smartAudioDevice_t saDevice = { .version = 0, .channel = -1, - .power = -1, + .power = 0, .mode = 0, .freq = 0, .orfreq = 0, @@ -146,6 +127,13 @@ static smartAudioDevice_t saDevicePrev = { // XXX Possible compliance problem here. Need LOCK/UNLOCK menu? static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable? +static uint8_t saSupportedNumPowerLevels = VTX_SMARTAUDIO_POWER_COUNT; +static uint16_t saSupportedPowerValues[VTX_SMARTAUDIO_POWER_COUNT]; +#if !defined(USE_VTX_TABLE) +static char saSupportedPowerLabels[VTX_SMARTAUDIO_POWER_COUNT + 1][4] = {"---", "25 ", "200", "500", "800"}; +static char *saSupportedPowerLabelPointerArray[VTX_SMARTAUDIO_POWER_COUNT + 1]; +#endif + // XXX Should be configurable by user? bool saDeferred = true; // saCms variable? @@ -194,21 +182,13 @@ static void saPrintSettings(void) dprintf((" channel: %d ", saDevice.channel)); dprintf(("freq: %d ", saDevice.freq)); dprintf(("power: %d ", saDevice.power)); + dprintf(("powerval: %d ",saDevice.power > 0 ? vtxSmartAudio.powerValues[saDevice.power - 1] : -1)); dprintf(("pitfreq: %d ", saDevice.orfreq)); + dprintf(("device %s boot into pitmode ", saDevice.willBootIntoPitMode ? "will" : "will not")); dprintf(("\r\n")); } #endif -int saDacToPowerIndex(int dac) -{ - for (int idx = 3 ; idx >= 0 ; idx--) { - if (saPowerTable[idx].valueV1 <= dac) { - return idx; - } - } - return 0; -} - // // Autobauding // @@ -290,19 +270,77 @@ static void saProcessResponse(uint8_t *buf, int len) case SA_CMD_GET_SETTINGS_V21: // Version 2.1 Get Settings case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings + dprintf(("received settings\r\n")); if (len < 7) { break; } - // XXX(fujin): For now handle V21 saDevice.version as '2'. // From spec: "Bit 7-3 is holding the Smart audio version where 0 is V1, 1 is V2, 2 is V2.1" - // saDevice.version = buf[0]; - saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; + // 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]; - saDevice.power = buf[3]; + if(saDevice.version != 3) { + saDevice.power = buf[3]; + } saDevice.mode = buf[4]; saDevice.freq = (buf[5] << 8)|buf[6]; - // XXX(fujin): Receive additional SA2.1 fields here for dBm based power level. + + // read pir and por flags to detect if the device will boot into pitmode. + // note that "quit pitmode without unsetting the pitmode flag" clears pir and por flags but the device will still boot into pitmode. + // therefore we ignore the pir and por flags while the device is not in pitmode + // actually, this is the whole reason the variable saDevice.willBootIntoPitMode exists. + // otherwise we could use saDevice.mode directly + if (saDevice.mode & SA_MODE_GET_PITMODE) { + bool newBootMode = (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE); + if (newBootMode != saDevice.willBootIntoPitMode) { + dprintf(("saProcessResponse: willBootIntoPitMode is now %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false")); + } + saDevice.willBootIntoPitMode = newBootMode; + } + + if(saDevice.version == 3) { + //read dbm based power levels + //aaaaaand promptly forget them. todo: write them into vtxtable pg and load it + if(len < 10) {//current power level in dbm field missing or power level length field missing or zero power levels reported + dprintf(("processResponse: V2.1 vtx didn't report any power levels\r\n")); + break; + } + saSupportedNumPowerLevels = constrain((int8_t)buf[8], 0, VTX_TABLE_MAX_POWER_LEVELS); + //SmartAudio seems to report buf[8] + 1 power levels, but one of them is zero. + //zero is indeed a valid power level to set the vtx to, but it activates pit mode. + //crucially, after sending 0 dbm, the vtx does NOT report its power level to be 0 dbm. + //instead, it reports whatever value was set previously and it reports to be in pit mode. + //for this reason, zero shouldn't be used as a normal power level in betaflight. +#ifdef USE_SMARTAUDIO_DPRINTF + if ( len < ( 9 + saSupportedNumPowerLevels )) { + dprintf(("processResponse: V2.1 vtx expected %d power levels but packet too short\r\n", saSupportedNumPowerLevels)); + break; + } + if (len > (10 + saSupportedNumPowerLevels )) { + dprintf(("processResponse: V2.1 %d extra bytes found!\r\n", len - (10 + saSupportedNumPowerLevels))); + } +#endif + for ( int8_t i = 0; i < saSupportedNumPowerLevels; i++ ) { + saSupportedPowerValues[i] = buf[9 + i + 1];//+ 1 to skip the first power level, as mentioned above + } + + dprintf(("processResponse: %d power values: %d, %d, %d, %d\r\n", + 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]) { +#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)); + } +#endif + saDevice.power = i + 1; + } + } + } DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode); DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel); @@ -336,7 +374,9 @@ static void saProcessResponse(uint8_t *buf, int len) break; case SA_CMD_SET_MODE: // Set Mode - dprintf(("saProcessResponse: SET_MODE 0x%x\r\n", buf[2])); + dprintf(("saProcessResponse: SET_MODE 0x%x (pir %s, por %s, pitdsbl %s, %s)\r\n", + buf[2], (buf[2] & 1) ? "on" : "off", (buf[2] & 2) ? "on" : "off", (buf[3] & 4) ? "on" : "off", + (buf[4] & 8) ? "unlocked" : "locked")); break; default: @@ -557,6 +597,7 @@ static void saGetSettings(void) { static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F}; + dprintf(("smartAudioGetSettings\r\n")); saQueueCmd(bufGetSettings, 5); } @@ -630,6 +671,7 @@ static void saDevSetBandAndChannel(uint8_t band, uint8_t channel) buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel); buf[5] = CRC8(buf, 5); + dprintf(("saDevSetBandAndChannel band %d channel %d value sent 0x%x", band, channel, buf[4])); saQueueCmd(buf, 6); } @@ -644,38 +686,27 @@ void saSetMode(int mode) static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; buf[4] = (mode & 0x3f)|saLockMode; + if (saDevice.version>=3 && (mode & SA_MODE_CLR_PITMODE) && + ((mode & SA_MODE_SET_IN_RANGE_PITMODE) || (mode & SA_MODE_SET_OUT_RANGE_PITMODE))) { + saDevice.willBootIntoPitMode = true;//quit pitmode without unsetting flag. + //the response will just say pit=off but the device will still go into pitmode on reboot. + //therefore we have to memorize this change here. + } + dprintf(("saSetMode(0x%x): pir=%s por=%s pitdsbl=%s %s\r\n", mode, (mode & 1) ? "on " : "off", (mode & 2) ? "on " : "off", + (mode & 4)? "on " : "off", (mode & 8) ? "locked" : "unlocked")); buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } -static void saDevSetPowerByIndex(uint8_t index) -{ - static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 }; - - dprintf(("saSetPowerByIndex: index %d\r\n", index)); - - if (saDevice.version == 0) { - // Unknown or yet unknown version. - return; - } - - if (index >= VTX_SMARTAUDIO_POWER_COUNT) { - return; - } - - buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; - buf[5] = CRC8(buf, 5); - saQueueCmd(buf, 6); -} - -void saSetPowerByIndex(uint8_t index) -{ - saDevSetPowerByIndex(index); -} bool vtxSmartAudioInit(void) { +#if !defined(USE_VTX_TABLE) + for (int8_t i = 0; i < VTX_SMARTAUDIO_POWER_COUNT + 1; i++) { + saSupportedPowerLabelPointerArray[i] = saSupportedPowerLabels[i]; + } +#endif #ifdef USE_SMARTAUDIO_DPRINTF // Setup debugSerialPort @@ -702,15 +733,31 @@ bool vtxSmartAudioInit(void) return false; } + for(int8_t i = 0; i < VTX_SMARTAUDIO_POWER_COUNT; i++) { + saSupportedPowerValues[i] = 1; + } + +#ifdef USE_VTX_TABLE + vtxSmartAudio.capability.bandCount = vtxTableBandCount; + vtxSmartAudio.capability.channelCount = vtxTableChannelCount; + vtxSmartAudio.capability.powerCount = vtxTablePowerLevels; + vtxSmartAudio.frequencyTable = (uint16_t *)vtxTableFrequency; + vtxSmartAudio.bandNames = vtxTableBandNames; + vtxSmartAudio.bandLetters = vtxTableBandLetters; + vtxSmartAudio.channelNames = vtxTableChannelNames; + vtxSmartAudio.powerNames = vtxTablePowerLabels; + vtxSmartAudio.powerValues = vtxTablePowerValues; +#else vtxSmartAudio.capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT; vtxSmartAudio.capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT; - vtxSmartAudio.capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT; + vtxSmartAudio.capability.powerCount = saSupportedNumPowerLevels, vtxSmartAudio.frequencyTable = vtxStringFrequencyTable(); vtxSmartAudio.bandNames = vtxStringBandNames(); vtxSmartAudio.bandLetters = vtxStringBandLetters(); vtxSmartAudio.channelNames = vtxStringChannelNames(); - vtxSmartAudio.powerNames = saPowerNames; - vtxSmartAudio.powerValues = saPowerValues; + vtxSmartAudio.powerNames = (const char**)saSupportedPowerLabelPointerArray; + vtxSmartAudio.powerValues = saSupportedPowerValues; +#endif vtxCommonSetDevice(&vtxSmartAudio); @@ -753,6 +800,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) case SA_INITPHASE_WAIT_SETTINGS: // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ, // and put the device into user frequency mode with uninitialized freq. + // Also don't send it to V2.1 for the same reason. if (saDevice.version) { if (saDevice.version == 2) { saDoDevSetFreq(SA_FREQ_GETPIT); @@ -760,6 +808,34 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) } else { initPhase = SA_INITPHASE_DONE; } + +#if !defined(USE_VTX_TABLE) + if (saDevice.version == 1) {//this is kind of ugly. use fixed tables and set a pointer to them instead? + saSupportedPowerValues[0] = 7; + saSupportedPowerValues[1] = 16; + saSupportedPowerValues[2] = 25; + saSupportedPowerValues[3] = 40; + }else if(saDevice.version == 2) { + saSupportedPowerValues[0] = 0; + saSupportedPowerValues[1] = 1; + saSupportedPowerValues[2] = 2; + saSupportedPowerValues[3] = 3; + } + + if (saDevice.version >= 2) {//V2.0 and V1.0 use defaults set at declaration + vtxSmartAudio.capability.powerCount = constrain(saSupportedNumPowerLevels, 0, VTX_SMARTAUDIO_POWER_COUNT); + for(int8_t i = 0; i < saSupportedNumPowerLevels; i++) { + tfp_sprintf(saSupportedPowerLabels[i + 1], "%3d", constrain(saSupportedPowerValues[i], 0, 999)); + //ideally we would convert dbm to mW here + } + } +#endif + + if (saDevice.version >= 2 ) { + //did the device boot up in pit mode on its own? + saDevice.willBootIntoPitMode = (saDevice.mode & SA_MODE_GET_PITMODE) ? true : false; + dprintf(("sainit: willBootIntoPitMode is %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false")); + } } break; @@ -795,7 +871,6 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) saSendQueue(); } } - #ifdef USE_VTX_COMMON // Interface to common VTX API @@ -820,37 +895,70 @@ static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - UNUSED(vtxDevice); - if (index == 0) { - // SmartAudio doesn't support power off. + static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 }; + + if (!vtxSAIsReady(vtxDevice)) { return; } - saSetPowerByIndex(index - 1); + if (index <= 0 || index > vtxSmartAudio.capability.powerCount) { + dprintf(("saSetPowerByIndex: cannot get power level %d, only levels 1 through %d supported", index, vtxSmartAudio.capability.powerCount)); + return; + } + + buf[4] = vtxSmartAudio.powerValues[index - 1];//use vtxcommonlookuppowervalue instead? + dprintf(("saSetPowerByIndex: index %d, value %d\r\n", index, buf[4])); + if (saDevice.version == 3) { + buf[4] |= 128;//set MSB to indicate set power by dbm + } + buf[5] = CRC8(buf, 5); + saQueueCmd(buf, 6); } static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { - if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version >= 2))) { return; } - if (onoff) { - // SmartAudio can not turn pit mode on by software. + if (onoff && saDevice.version < 3) { + // Smart Audio prior to V2.1 can not turn pit mode on by software. return; } - uint8_t newmode = SA_MODE_CLR_PITMODE; - - if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { - newmode |= SA_MODE_SET_IN_RANGE_PITMODE; + if (onoff && saDevice.version>=3 && !saDevice.willBootIntoPitMode) { + // enable pitmode using SET_POWER command with 0 dbm. + // This enables pitmode without causing the device to boot into pitmode next power-up + static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 }; + buf[4] = 0 | 128; + buf[5] = CRC8(buf,5); + saQueueCmd(buf,6); + dprintf(("vtxSASetPitMode: set power to 0 dbm\r\n")); + return; } + if (!onoff && saDevice.version>=3 && !saDevice.willBootIntoPitMode) { + saSetMode(SA_MODE_CLR_PITMODE); + dprintf(("sasetpidmode: clear pitmode permanently")); + return; + } + + uint8_t newMode = onoff ? 0 : SA_MODE_CLR_PITMODE; + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) { - newmode |= SA_MODE_SET_OUT_RANGE_PITMODE; + newMode |= SA_MODE_SET_OUT_RANGE_PITMODE; } - saSetMode(newmode); + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE || !(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; + } + dprintf(("vtxsasetpitmode %s with stored mode 0x%x por %s, pir %s, newmode 0x%x\r\n", onoff ? "on" : "off", saDevice.mode, + (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) ? "on" : "off", + (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) ? "on" : "off" , newMode)); + + + saSetMode(newMode); return; } @@ -883,13 +991,13 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) return false; } - *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1; + *pIndex = saDevice.power;//power levels are 1-based to match vtxtables with one more label than value return true; } static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version >= 2))) { return false; } diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 7c7383663..d5fb33f24 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -25,6 +25,13 @@ #include "platform.h" +//#define USE_SMARTAUDIO_DPRINTF +#ifdef USE_SMARTAUDIO_DPRINTF +#include "io/serial.h" +#include "common/printf.h" +#include "common/printf_serial.h" +#endif + #define VTX_SMARTAUDIO_MIN_BAND 1 #define VTX_SMARTAUDIO_MAX_BAND 5 #define VTX_SMARTAUDIO_MIN_CHANNEL 1 @@ -69,14 +76,9 @@ typedef struct smartAudioDevice_s { int8_t mode; uint16_t freq; uint16_t orfreq; + bool willBootIntoPitMode; } smartAudioDevice_t; -typedef struct saPowerTable_s { - int rfpower; - int16_t valueV1; - int16_t valueV2; -} saPowerTable_t; - typedef struct smartAudioStat_s { uint16_t pktsent; uint16_t pktrcvd; @@ -93,17 +95,16 @@ extern smartAudioStat_t saStat; extern uint16_t sa_smartbaud; extern bool saDeferred; -int saDacToPowerIndex(int dac); void saSetBandAndChannel(uint8_t band, uint8_t channel); void saSetMode(int mode); -void saSetPowerByIndex(uint8_t index); void saSetFreq(uint16_t freq); void saSetPitFreq(uint16_t freq); bool vtxSmartAudioInit(void); #ifdef USE_SMARTAUDIO_DPRINTF +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 extern serialPort_t *debugSerialPort; -#define dprintf(x) if (debugSerialPort) printf x +#define dprintf(x) if (debugSerialPort) tfp_printf x #else #define dprintf(x) #endif // USE_SMARTAUDIO_DPRINTF