Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

Poll state change after command several times, not continuous

Browse source
This commit is contained in:
jflyper 2017-11-10 17:55:32 +09:00
parent a03b586eb8
commit ed68e57d48
1 changed files with 88 additions and 98 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -55,6 +55,12 @@
#include "io/vtx_smartaudio.h" #include "io/vtx_smartaudio.h"
#include "io/vtx_string.h" #include "io/vtx_string.h"
// Timing parameters
// Note that vtxSAProcess() is normally called at 200ms interval
#define SMARTAUDIO_CMD_TIMEOUT 120 // Time until the command is considered lost
#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_DPRINTF //#define SMARTAUDIO_DPRINTF
//#define SMARTAUDIO_DEBUG_MONITOR //#define SMARTAUDIO_DEBUG_MONITOR
@ -219,8 +225,6 @@ uint16_t sa_smartbaud = SMARTBAUD_MIN;
static int sa_adjdir = 1; // -1=going down, 1=going up static int sa_adjdir = 1; // -1=going down, 1=going up
static int sa_baudstep = 50; static int sa_baudstep = 50;
#define SMARTAUDIO_CMD_TIMEOUT 120
static void saAutobaud(void) static void saAutobaud(void)
{ {
if (saStat.pktsent < 10) { if (saStat.pktsent < 10) {
@ -262,7 +266,7 @@ static void saAutobaud(void)
// Transport level variables // Transport level variables
static timeUs_t sa_lastTransmission = 0; static timeUs_t sa_lastTransmissionMs = 0;
static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
static int sa_oslen; // And associate length static int sa_oslen; // And associate length
@ -448,7 +452,7 @@ static void saSendFrame(uint8_t *buf, int len)
serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
sa_lastTransmission = millis(); sa_lastTransmissionMs = millis();
saStat.pktsent++; saStat.pktsent++;
} }
@ -702,8 +706,10 @@ bool vtxSmartAudioInit(void)
#define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply. #define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply.
#define SA_INITPHASE_DONE 3 #define SA_INITPHASE_DONE 3
void vtxSAProcess(timeUs_t now) void vtxSAProcess(timeUs_t currentTimeUs)
{ {
UNUSED(currentTimeUs);
static char initPhase = SA_INITPHASE_START; static char initPhase = SA_INITPHASE_START;
if (smartAudioSerialPort == NULL) { if (smartAudioSerialPort == NULL) {
@ -750,41 +756,25 @@ void vtxSAProcess(timeUs_t now)
// Command queue control // Command queue control
if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { timeMs_t nowMs = millis(); // Don't substitute with "currentTimeUs / 1000"; sa_lastTransmissionMs is based on millis().
static timeMs_t lastCommandSentMs = 0; // Last non-GET_SETTINGS sent
if ((sa_outstanding != SA_CMD_NONE) && (nowMs - sa_lastTransmissionMs > 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));
// XXX Todo: Resend termination and possible offline transition // XXX Todo: Resend termination and possible offline transition
saResendCmd(); saResendCmd();
lastCommandSentMs = nowMs;
} else if (!saQueueEmpty()) { } else if (!saQueueEmpty()) {
// Command pending. Send it. // Command pending. Send it.
// dprintf(("process: sending queue\r\n")); // dprintf(("process: sending queue\r\n"));
saSendQueue(); saSendQueue();
} else if (saDevice.version != 0 && now - sa_lastTransmission >= 1000) { lastCommandSentMs = nowMs;
// Heart beat for autobauding } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) {
//dprintf(("process: sending heartbeat\r\n")); //dprintf(("process: sending status change polling\r\n"));
saGetSettings(); saGetSettings();
saSendQueue(); saSendQueue();
} }
#ifdef SMARTAUDIO_TEST_VTX_COMMON
// Testing VTX_COMMON API
{
static uint32_t lastMonitorUs = 0;
if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) {
return;
}
static uint8_t monBand;
static uint8_t monChan;
static uint8_t monPower;
vtxCommonGetBandAndChannel(&monBand, &monChan);
vtxCommonGetPowerIndex(&monPower);
debug[0] = monBand;
debug[1] = monChan;
debug[2] = monPower;
}
#endif
} }
#ifdef VTX_COMMON #ifdef VTX_COMMON
@ -792,123 +782,123 @@ void vtxSAProcess(timeUs_t now)
vtxDevType_e vtxSAGetDeviceType(void) vtxDevType_e vtxSAGetDeviceType(void)
{ {
return VTXDEV_SMARTAUDIO; return VTXDEV_SMARTAUDIO;
} }
bool vtxSAIsReady(void) bool vtxSAIsReady(void)
{ {
return !(saDevice.version == 0); return !(saDevice.version == 0);
} }
void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) void vtxSASetBandAndChannel(uint8_t band, uint8_t channel)
{ {
if (saValidateBandAndChannel(band, channel)) { if (saValidateBandAndChannel(band, channel)) {
saSetBandAndChannel(band - 1, channel - 1); saSetBandAndChannel(band - 1, channel - 1);
} }
} }
void vtxSASetPowerByIndex(uint8_t index) void vtxSASetPowerByIndex(uint8_t index)
{ {
if (index == 0) { if (index == 0) {
// SmartAudio doesn't support power off. // SmartAudio doesn't support power off.
return; return;
} }
saSetPowerByIndex(index - 1); saSetPowerByIndex(index - 1);
} }
void vtxSASetPitMode(uint8_t onoff) void vtxSASetPitMode(uint8_t onoff)
{ {
if (!(vtxSAIsReady() && (saDevice.version == 2))) { if (!(vtxSAIsReady() && (saDevice.version == 2))) {
return; return;
} }
if (onoff) { if (onoff) {
// SmartAudio can not turn pit mode on by software. // SmartAudio can not turn pit mode on by software.
return; return;
} }
uint8_t newmode = SA_MODE_CLR_PITMODE; uint8_t newmode = SA_MODE_CLR_PITMODE;
if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
newmode |= SA_MODE_SET_IN_RANGE_PITMODE; newmode |= SA_MODE_SET_IN_RANGE_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); saSetMode(newmode);
return; return;
} }
void vtxSASetFreq(uint16_t freq) void vtxSASetFreq(uint16_t freq)
{ {
if (saValidateFreq(freq)) { if (saValidateFreq(freq)) {
saSetMode(0); //need to be in FREE mode to set freq saSetMode(0); //need to be in FREE mode to set freq
saSetFreq(freq); saSetFreq(freq);
} }
} }
bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
{ {
if (!vtxSAIsReady()) { if (!vtxSAIsReady()) {
return false; return false;
} }
// if in user-freq mode then report band as zero // if in user-freq mode then report band as zero
*pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 : *pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 :
(SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1); (SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1);
*pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1; *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1;
return true; return true;
} }
bool vtxSAGetPowerIndex(uint8_t *pIndex) bool vtxSAGetPowerIndex(uint8_t *pIndex)
{ {
if (!vtxSAIsReady()) { if (!vtxSAIsReady()) {
return false; return false;
} }
*pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1; *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
return true; return true;
} }
bool vtxSAGetPitMode(uint8_t *pOnOff) bool vtxSAGetPitMode(uint8_t *pOnOff)
{ {
if (!(vtxSAIsReady() && (saDevice.version == 2))) { if (!(vtxSAIsReady() && (saDevice.version == 2))) {
return false; return false;
} }
*pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0; *pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
return true; return true;
} }
bool vtxSAGetFreq(uint16_t *pFreq) bool vtxSAGetFreq(uint16_t *pFreq)
{ {
if (!vtxSAIsReady()) { if (!vtxSAIsReady()) {
return false; return false;
} }
// if not in user-freq mode then convert band/chan to frequency // if not in user-freq mode then convert band/chan to frequency
*pFreq = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? saDevice.freq : *pFreq = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? saDevice.freq :
vtx58_Bandchan2Freq(SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1, vtx58_Bandchan2Freq(SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1,
SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1); SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1);
return true; return true;
} }
static const vtxVTable_t saVTable = { static const vtxVTable_t saVTable = {
.process = vtxSAProcess, .process = vtxSAProcess,
.getDeviceType = vtxSAGetDeviceType, .getDeviceType = vtxSAGetDeviceType,
.isReady = vtxSAIsReady, .isReady = vtxSAIsReady,
.setBandAndChannel = vtxSASetBandAndChannel, .setBandAndChannel = vtxSASetBandAndChannel,
.setPowerByIndex = vtxSASetPowerByIndex, .setPowerByIndex = vtxSASetPowerByIndex,
.setPitMode = vtxSASetPitMode, .setPitMode = vtxSASetPitMode,
.setFrequency = vtxSASetFreq, .setFrequency = vtxSASetFreq,
.getBandAndChannel = vtxSAGetBandAndChannel, .getBandAndChannel = vtxSAGetBandAndChannel,
.getPowerIndex = vtxSAGetPowerIndex, .getPowerIndex = vtxSAGetPowerIndex,
.getPitMode = vtxSAGetPitMode, .getPitMode = vtxSAGetPitMode,
.getFrequency = vtxSAGetFreq, .getFrequency = vtxSAGetFreq,
}; };
#endif // VTX_COMMON #endif // VTX_COMMON