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

Refactor initialization code

Browse source
This commit is contained in:
jflyper 2017-11-10 12:03:19 +09:00
parent 364afcbf25
commit 9e48fb5f11
1 changed files with 30 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -697,10 +697,14 @@ bool vtxSmartAudioInit(void)
return true; return true;
} }
#define SA_INITPHASE_START 0
#define SA_INITPHASE_WAIT_SETTINGS 1 // SA_CMD_GET_SETTINGS was sent and waiting for reply.
#define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply.
#define SA_INITPHASE_DONE 3
void vtxSAProcess(timeUs_t now) void vtxSAProcess(timeUs_t now)
{ {
static char initPhase = 0; static char initPhase = SA_INITPHASE_START;
static bool initSettingsDoneFlag = false;
if (smartAudioSerialPort == NULL) { if (smartAudioSerialPort == NULL) {
return; return;
@ -715,21 +719,37 @@ void vtxSAProcess(timeUs_t now)
saAutobaud(); saAutobaud();
switch (initPhase) { switch (initPhase) {
case 0: case SA_INITPHASE_START:
saGetSettings(); saGetSettings();
saSendQueue(); //saSendQueue();
++initPhase; initPhase = SA_INITPHASE_WAIT_SETTINGS;
return; break;
case 1: 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.
if (saDevice.version == 2) if (saDevice.version) {
saDoDevSetFreq(SA_FREQ_GETPIT); if (saDevice.version == 2) {
++initPhase; saDoDevSetFreq(SA_FREQ_GETPIT);
initPhase = SA_INITPHASE_WAIT_PITFREQ;
} else {
initPhase = SA_INITPHASE_DONE;
}
}
break;
case SA_INITPHASE_WAIT_PITFREQ:
if (saDevice.orfreq) {
initPhase = SA_INITPHASE_DONE;
}
break;
case SA_INITPHASE_DONE:
break; break;
} }
// Command queue control
if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
// Last command timed out // Last command timed out
// dprintf(("process: resending 0x%x\r\n", sa_outstanding)); // dprintf(("process: resending 0x%x\r\n", sa_outstanding));
@ -746,17 +766,6 @@ void vtxSAProcess(timeUs_t now)
saSendQueue(); saSendQueue();
} }
// once device is ready enter vtx settings
if (!initSettingsDoneFlag) {
if (saDevice.version != 0) {
initSettingsDoneFlag = true;
} else if (now - sa_lastTransmission >= 100) {
// device is not ready; repeat query
saGetSettings();
saSendQueue();
}
}
#ifdef SMARTAUDIO_TEST_VTX_COMMON #ifdef SMARTAUDIO_TEST_VTX_COMMON
// Testing VTX_COMMON API // Testing VTX_COMMON API
{ {