Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

SmartAudio conversion to VTX table and V2.1 implementation

Browse source
This commit is contained in:
functionpointer 2019-05-15 16:22:05 +02:00
parent 1541466dac
commit e976ea13a7
5 changed files with 273 additions and 107 deletions
Download patch file Download diff file Expand all files Collapse all files

87
src/main/cms/cms_menu_vtx_smartaudio.c
View file

@ -70,6 +70,7 @@ uint16_t saCmsDeviceFreq = 0;
uint8_t saCmsDeviceStatus = 0; uint8_t saCmsDeviceStatus = 0;
uint8_t saCmsPower; uint8_t saCmsPower;
uint8_t saCmsPit;
uint8_t saCmsPitFMode; // Undef(0), In-Range(1) or Out-Range(2) uint8_t saCmsPitFMode; // Undef(0), In-Range(1) or Out-Range(2)
uint8_t saCmsFselMode; // Channel(0) or User defined(1) uint8_t saCmsFselMode; // Channel(0) or User defined(1)
@ -86,7 +87,7 @@ void saCmsUpdate(void)
// XXX Take care of pit mode update somewhere??? // XXX Take care of pit mode update somewhere???
if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) {
// This is a first valid response to GET_SETTINGS. // 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; 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 saCmsConfigBandByGvar(displayPort_t *, const void *self);
static long saCmsConfigChanByGvar(displayPort_t *, const void *self); static long saCmsConfigChanByGvar(displayPort_t *, const void *self);
static long saCmsConfigPowerByGvar(displayPort_t *, const void *self); static long saCmsConfigPowerByGvar(displayPort_t *, const void *self);
static long saCmsConfigPitByGvar(displayPort_t *, const void *self);
void saUpdateStatusString(void) void saUpdateStatusString(void)
{ {
@ -144,6 +146,8 @@ if (saDevice.version == 2) {
saCmsPitFMode = 1; saCmsPitFMode = 1;
else else
saCmsPitFMode = 0; saCmsPitFMode = 0;
} else if (saDevice.version == 3) {
saCmsPitFMode = 1;//V2.1 only supports PIR
} }
const vtxDevice_t *device = vtxCommonDevice(); const vtxDevice_t *device = vtxCommonDevice();
@ -169,18 +173,26 @@ if (saDevice.version == 2) {
saCmsStatusString[9] = ' '; saCmsStatusString[9] = ' ';
if (saDevice.mode & SA_MODE_GET_PITMODE) { if (saDevice.mode & SA_MODE_GET_PITMODE) {
saCmsPit = 2;
saCmsStatusString[10] = 'P'; saCmsStatusString[10] = 'P';
if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) {
saCmsStatusString[11] = 'I';
} else {
saCmsStatusString[11] = 'O'; saCmsStatusString[11] = 'O';
} else {
saCmsStatusString[11] = 'I';
} }
saCmsStatusString[12] = 'R'; saCmsStatusString[12] = 'R';
saCmsStatusString[13] = 0; saCmsStatusString[13] = 0;
} else { } else {
int index = (saDevice.version == 2) ? saDevice.power : saDacToPowerIndex(saDevice.power); saCmsPit = 1;
tfp_sprintf(&saCmsStatusString[10], "%s", vtxCommonLookupPowerName(vtxCommonDevice(), index + 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() void saCmsResetOpmodel()
@ -239,6 +251,30 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self)
return 0; 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) static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
{ {
UNUSED(pDisp); UNUSED(pDisp);
@ -256,9 +292,14 @@ static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
return 0; return 0;
} }
if (saCmsPit > 0 && saCmsPit != 1 ) {
saCmsPit = 1;
}
if (saCmsOpmodel == SACMS_OPMODEL_FREE && !saDeferred) { if (saCmsOpmodel == SACMS_OPMODEL_FREE && !saDeferred) {
vtxSettingsConfigMutable()->power = saCmsPower; vtxSettingsConfigMutable()->power = saCmsPower;
} }
dprintf(("saCmsConfigPowerByGvar: power index is now %d\r\n", saCmsPower));
return 0; return 0;
} }
@ -273,6 +314,12 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
saCmsPitFMode = 0; saCmsPitFMode = 0;
return 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)); 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. // out-range receivers from getting blinded.
saCmsPitFMode = 0; saCmsPitFMode = 0;
saCmsConfigPitFModeByGvar(pDisp, self); 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 // Direct frequency mode is not available in RACE opmodel
saCmsFselModeNew = 0; saCmsFselModeNew = 0;
@ -335,9 +385,10 @@ static const char * const saCmsDeviceStatusNames[] = {
"OFFL", "OFFL",
"ONL V1", "ONL V1",
"ONL V2", "ONL V2",
"ONLV21",
}; };
static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames }; static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 3, saCmsDeviceStatusNames };
static const OSD_Entry saCmsMenuStatsEntries[] = { static const OSD_Entry saCmsMenuStatsEntries[] = {
{ "- SA STATS -", OME_Label, NULL, NULL, 0 }, { "- SA STATS -", OME_Label, NULL, NULL, 0 },
@ -404,7 +455,14 @@ static const char * const saCmsPitFModeNames[] = {
"POR" "POR"
}; };
static const char * const saCmsPitNames[] = {
"---",
"OFF",
"ON ",
};
static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames }; static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames };
static OSD_TAB_t saCmsEntPit = {&saCmsPit , 2, saCmsPitNames};
static long sacms_SetupTopMenu(void); // Forward static long sacms_SetupTopMenu(void); // Forward
@ -448,12 +506,6 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self)
newSettings.band = saCmsBand; newSettings.band = saCmsBand;
newSettings.channel = saCmsChan; newSettings.channel = saCmsChan;
newSettings.freq = vtxCommonLookupFrequency(vtxCommonDevice(), saCmsBand, 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 { } else {
// Freestyle model // Freestyle model
// Setup band and freq / user freq // 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; newSettings.power = saCmsPower;
if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) { if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) {
@ -631,7 +686,8 @@ static const OSD_Entry saCmsMenuFreqModeEntries[] = {
{ "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, { "", OME_Label, NULL, saCmsStatusString, DYNAMIC },
{ "FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsUserFreqGetString, &saCmsMenuUserFreq, OPTSTRING }, { "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 }, { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 },
{ "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 },
@ -647,7 +703,8 @@ static const OSD_Entry saCmsMenuChanModeEntries[] =
{ "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 }, { "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 },
{ "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 }, { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 },
{ "(FREQ)", OME_UINT16, NULL, &saCmsEntFreqRef, DYNAMIC }, { "(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 }, { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 },
{ "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 },

4
src/main/drivers/vtx_common.c
View file

@ -207,8 +207,8 @@ const char *vtxCommonLookupPowerName(const vtxDevice_t *vtxDevice, int index)
uint16_t vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index) uint16_t vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index)
{ {
if (vtxDevice) { if (vtxDevice && index > 0) {
return vtxDevice->powerValues[index]; return vtxDevice->powerValues[index - 1];
} else { } else {
return 0; return 0;
} }

2
src/main/drivers/vtx_table.c
View file

@ -69,7 +69,7 @@ void vtxTableInit(void)
} }
vtxTableChannelNames[0] = "-"; 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]; vtxTablePowerValues[level] = config->powerValues[level];
vtxTablePowerLabels[level + 1] = config->powerLabels[level]; vtxTablePowerLabels[level + 1] = config->powerLabels[level];
} }

266
src/main/io/vtx_smartaudio.c
View file

@ -40,6 +40,7 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
#include "drivers/vtx_table.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/vtx.h" #include "io/vtx.h"
@ -54,9 +55,6 @@
#define SMARTAUDIO_POLLING_INTERVAL 150 // Minimum time between state polling #define SMARTAUDIO_POLLING_INTERVAL 150 // Minimum time between state polling
#define SMARTAUDIO_POLLING_WINDOW 1000 // Time window after command polling for state change #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 #ifdef USE_SMARTAUDIO_DPRINTF
serialPort_t *debugSerialPort = NULL; serialPort_t *debugSerialPort = NULL;
#endif // USE_SMARTAUDIO_DPRINTF #endif // USE_SMARTAUDIO_DPRINTF
@ -65,12 +63,6 @@ static serialPort_t *smartAudioSerialPort = NULL;
smartAudioDevice_t saDevice; 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 #ifdef USE_VTX_COMMON
static const vtxVTable_t saVTable; // Forward static const vtxVTable_t saVTable; // Forward
static vtxDevice_t vtxSmartAudio = { static vtxDevice_t vtxSmartAudio = {
@ -117,23 +109,12 @@ smartAudioStat_t saStat = {
.badcode = 0, .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 // Last received device ('hard') states
smartAudioDevice_t saDevice = { smartAudioDevice_t saDevice = {
.version = 0, .version = 0,
.channel = -1, .channel = -1,
.power = -1, .power = 0,
.mode = 0, .mode = 0,
.freq = 0, .freq = 0,
.orfreq = 0, .orfreq = 0,
@ -146,6 +127,13 @@ static smartAudioDevice_t saDevicePrev = {
// XXX Possible compliance problem here. Need LOCK/UNLOCK menu? // XXX Possible compliance problem here. Need LOCK/UNLOCK menu?
static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable? 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? // XXX Should be configurable by user?
bool saDeferred = true; // saCms variable? bool saDeferred = true; // saCms variable?
@ -194,21 +182,13 @@ static void saPrintSettings(void)
dprintf((" channel: %d ", saDevice.channel)); dprintf((" channel: %d ", saDevice.channel));
dprintf(("freq: %d ", saDevice.freq)); dprintf(("freq: %d ", saDevice.freq));
dprintf(("power: %d ", saDevice.power)); dprintf(("power: %d ", saDevice.power));
dprintf(("powerval: %d ",saDevice.power > 0 ? vtxSmartAudio.powerValues[saDevice.power - 1] : -1));
dprintf(("pitfreq: %d ", saDevice.orfreq)); dprintf(("pitfreq: %d ", saDevice.orfreq));
dprintf(("device %s boot into pitmode ", saDevice.willBootIntoPitMode ? "will" : "will not"));
dprintf(("\r\n")); dprintf(("\r\n"));
} }
#endif #endif
int saDacToPowerIndex(int dac)
{
for (int idx = 3 ; idx >= 0 ; idx--) {
if (saPowerTable[idx].valueV1 <= dac) {
return idx;
}
}
return 0;
}
// //
// Autobauding // 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_V21: // Version 2.1 Get Settings
case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
case SA_CMD_GET_SETTINGS: // Version 1 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings
dprintf(("received settings\r\n"));
if (len < 7) { if (len < 7) {
break; 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" // 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 = 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 : 2; saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : ((buf[0] == SA_CMD_GET_SETTINGS_V2) ? 2 : 3);
saDevice.channel = buf[2]; saDevice.channel = buf[2];
if(saDevice.version != 3) {
saDevice.power = buf[3]; saDevice.power = buf[3];
}
saDevice.mode = buf[4]; saDevice.mode = buf[4];
saDevice.freq = (buf[5] << 8)|buf[6]; 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, 0, saDevice.version * 100 + saDevice.mode);
DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel); DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel);
@ -336,7 +374,9 @@ static void saProcessResponse(uint8_t *buf, int len)
break; break;
case SA_CMD_SET_MODE: // Set Mode 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; break;
default: default:
@ -557,6 +597,7 @@ static void saGetSettings(void)
{ {
static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F}; static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
dprintf(("smartAudioGetSettings\r\n"));
saQueueCmd(bufGetSettings, 5); 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[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
buf[5] = CRC8(buf, 5); buf[5] = CRC8(buf, 5);
dprintf(("saDevSetBandAndChannel band %d channel %d value sent 0x%x", band, channel, buf[4]));
saQueueCmd(buf, 6); saQueueCmd(buf, 6);
} }
@ -644,38 +686,27 @@ void saSetMode(int mode)
static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
buf[4] = (mode & 0x3f)|saLockMode; 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); buf[5] = CRC8(buf, 5);
saQueueCmd(buf, 6); 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) 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 #ifdef USE_SMARTAUDIO_DPRINTF
// Setup debugSerialPort // Setup debugSerialPort
@ -702,15 +733,31 @@ bool vtxSmartAudioInit(void)
return false; 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.bandCount = VTX_SMARTAUDIO_BAND_COUNT;
vtxSmartAudio.capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT; vtxSmartAudio.capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT;
vtxSmartAudio.capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT; vtxSmartAudio.capability.powerCount = saSupportedNumPowerLevels,
vtxSmartAudio.frequencyTable = vtxStringFrequencyTable(); vtxSmartAudio.frequencyTable = vtxStringFrequencyTable();
vtxSmartAudio.bandNames = vtxStringBandNames(); vtxSmartAudio.bandNames = vtxStringBandNames();
vtxSmartAudio.bandLetters = vtxStringBandLetters(); vtxSmartAudio.bandLetters = vtxStringBandLetters();
vtxSmartAudio.channelNames = vtxStringChannelNames(); vtxSmartAudio.channelNames = vtxStringChannelNames();
vtxSmartAudio.powerNames = saPowerNames; vtxSmartAudio.powerNames = (const char**)saSupportedPowerLabelPointerArray;
vtxSmartAudio.powerValues = saPowerValues; vtxSmartAudio.powerValues = saSupportedPowerValues;
#endif
vtxCommonSetDevice(&vtxSmartAudio); vtxCommonSetDevice(&vtxSmartAudio);
@ -753,6 +800,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
case SA_INITPHASE_WAIT_SETTINGS: case SA_INITPHASE_WAIT_SETTINGS:
// Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ, // 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. // 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) {
if (saDevice.version == 2) { if (saDevice.version == 2) {
saDoDevSetFreq(SA_FREQ_GETPIT); saDoDevSetFreq(SA_FREQ_GETPIT);
@ -760,6 +808,34 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
} else { } else {
initPhase = SA_INITPHASE_DONE; 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; break;
@ -795,7 +871,6 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
saSendQueue(); saSendQueue();
} }
} }
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
// Interface to common VTX API // 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) static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{ {
UNUSED(vtxDevice); static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
if (index == 0) {
// SmartAudio doesn't support power off. if (!vtxSAIsReady(vtxDevice)) {
return; 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) static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
{ {
if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { if (!(vtxSAIsReady(vtxDevice) && (saDevice.version >= 2))) {
return; return;
} }
if (onoff) { if (onoff && saDevice.version < 3) {
// SmartAudio can not turn pit mode on by software. // Smart Audio prior to V2.1 can not turn pit mode on by software.
return; return;
} }
uint8_t newmode = SA_MODE_CLR_PITMODE; if (onoff && saDevice.version>=3 && !saDevice.willBootIntoPitMode) {
// enable pitmode using SET_POWER command with 0 dbm.
if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { // This enables pitmode without causing the device to boot into pitmode next power-up
newmode |= SA_MODE_SET_IN_RANGE_PITMODE; 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) { 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; return;
} }
@ -883,13 +991,13 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
return false; 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; return true;
} }
static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{ {
if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { if (!(vtxSAIsReady(vtxDevice) && (saDevice.version >= 2))) {
return false; return false;
} }

19
src/main/io/vtx_smartaudio.h
View file

@ -25,6 +25,13 @@
#include "platform.h" #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_MIN_BAND 1
#define VTX_SMARTAUDIO_MAX_BAND 5 #define VTX_SMARTAUDIO_MAX_BAND 5
#define VTX_SMARTAUDIO_MIN_CHANNEL 1 #define VTX_SMARTAUDIO_MIN_CHANNEL 1
@ -69,14 +76,9 @@ typedef struct smartAudioDevice_s {
int8_t mode; int8_t mode;
uint16_t freq; uint16_t freq;
uint16_t orfreq; uint16_t orfreq;
bool willBootIntoPitMode;
} smartAudioDevice_t; } smartAudioDevice_t;
typedef struct saPowerTable_s {
int rfpower;
int16_t valueV1;
int16_t valueV2;
} saPowerTable_t;
typedef struct smartAudioStat_s { typedef struct smartAudioStat_s {
uint16_t pktsent; uint16_t pktsent;
uint16_t pktrcvd; uint16_t pktrcvd;
@ -93,17 +95,16 @@ extern smartAudioStat_t saStat;
extern uint16_t sa_smartbaud; extern uint16_t sa_smartbaud;
extern bool saDeferred; extern bool saDeferred;
int saDacToPowerIndex(int dac);
void saSetBandAndChannel(uint8_t band, uint8_t channel); void saSetBandAndChannel(uint8_t band, uint8_t channel);
void saSetMode(int mode); void saSetMode(int mode);
void saSetPowerByIndex(uint8_t index);
void saSetFreq(uint16_t freq); void saSetFreq(uint16_t freq);
void saSetPitFreq(uint16_t freq); void saSetPitFreq(uint16_t freq);
bool vtxSmartAudioInit(void); bool vtxSmartAudioInit(void);
#ifdef USE_SMARTAUDIO_DPRINTF #ifdef USE_SMARTAUDIO_DPRINTF
#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
extern serialPort_t *debugSerialPort; extern serialPort_t *debugSerialPort;
#define dprintf(x) if (debugSerialPort) printf x #define dprintf(x) if (debugSerialPort) tfp_printf x
#else #else
#define dprintf(x) #define dprintf(x)
#endif // USE_SMARTAUDIO_DPRINTF #endif // USE_SMARTAUDIO_DPRINTF