rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SmartAudio conversion to VTX table and V2.1 implementation

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
Show Stats 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 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 },

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)
{
if (vtxDevice) {
return vtxDevice->powerValues[index];
if (vtxDevice && index > 0) {
return vtxDevice->powerValues[index - 1];
} else {
return 0;
}

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

@ -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];
}

268
src/main/io/vtx_smartaudio.c
View File

@ -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;
}

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

@ -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