1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00

Tidy of VTX code

This commit is contained in:
Martin Budden 2017-09-24 08:04:31 +01:00
parent b9b10ef88e
commit 19230a436f
10 changed files with 122 additions and 80 deletions

View file

@ -16,9 +16,13 @@
*/ */
// Get target build configuration #include <stdbool.h>
#include <stdint.h>
#include "platform.h" #include "platform.h"
#if defined(VTX_CONTROL) && defined(VTX_COMMON)
#include "common/maths.h" #include "common/maths.h"
#include "config/config_eeprom.h" #include "config/config_eeprom.h"
@ -38,7 +42,6 @@
#include "io/vtx_control.h" #include "io/vtx_control.h"
#if defined(VTX_CONTROL) && defined(VTX_COMMON)
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
@ -176,7 +179,6 @@ void vtxCyclePower(const uint8_t powerStep)
void handleVTXControlButton(void) void handleVTXControlButton(void)
{ {
#if defined(VTX_RTC6705) && defined(BUTTON_A_PIN) #if defined(VTX_RTC6705) && defined(BUTTON_A_PIN)
bool buttonHeld;
bool buttonWasPressed = false; bool buttonWasPressed = false;
uint32_t start = millis(); uint32_t start = millis();
uint32_t ledToggleAt = start; uint32_t ledToggleAt = start;
@ -184,6 +186,7 @@ void handleVTXControlButton(void)
uint8_t flashesDone = 0; uint8_t flashesDone = 0;
uint8_t actionCounter = 0; uint8_t actionCounter = 0;
bool buttonHeld;
while ((buttonHeld = buttonAPressed())) { while ((buttonHeld = buttonAPressed())) {
uint32_t end = millis(); uint32_t end = millis();
@ -232,21 +235,20 @@ void handleVTXControlButton(void)
LED1_OFF; LED1_OFF;
switch (actionCounter) { switch (actionCounter) {
case 4: case 4:
vtxCycleBandOrChannel(0, +1); vtxCycleBandOrChannel(0, +1);
break; break;
case 3: case 3:
vtxCycleBandOrChannel(+1, 0); vtxCycleBandOrChannel(+1, 0);
break; break;
case 2: case 2:
vtxCyclePower(+1); vtxCyclePower(+1);
break; break;
case 1: case 1:
saveConfigAndNotify(); saveConfigAndNotify();
break; break;
} }
#endif #endif
} }
#endif #endif

View file

@ -17,6 +17,12 @@
#pragma once #pragma once
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "config/parameter_group.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10 #define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10

View file

@ -25,7 +25,9 @@
#include "platform.h" #include "platform.h"
#if defined(VTX_RTC6705) && defined(VTX_CONTROL) #if defined(VTX_RTC6705) && defined(VTX_CONTROL)
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h"
#include "cms/cms.h" #include "cms/cms.h"
#include "cms/cms_types.h" #include "cms/cms_types.h"
@ -46,8 +48,6 @@
#include "io/vtx_rtc6705.h" #include "io/vtx_rtc6705.h"
#include "io/vtx_string.h" #include "io/vtx_string.h"
#include "build/debug.h"
bool canUpdateVTX(void); bool canUpdateVTX(void);
PG_REGISTER_WITH_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config, PG_VTX_RTC6705_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config, PG_VTX_RTC6705_CONFIG, 0);

View file

@ -20,6 +20,8 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include "platform.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
typedef struct vtxRTC6705Config_s { typedef struct vtxRTC6705Config_s {

View file

@ -25,10 +25,10 @@
#include "platform.h" #include "platform.h"
#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#include "cms/cms.h" #include "cms/cms.h"
#include "cms/cms_types.h" #include "cms/cms_types.h"
#include "cms/cms_menu_vtx_smartaudio.h" #include "cms/cms_menu_vtx_smartaudio.h"
@ -194,8 +194,9 @@ static void saPrintSettings(void)
int saDacToPowerIndex(int dac) int saDacToPowerIndex(int dac)
{ {
for (int idx = 3 ; idx >= 0 ; idx--) { for (int idx = 3 ; idx >= 0 ; idx--) {
if (saPowerTable[idx].valueV1 <= dac) if (saPowerTable[idx].valueV1 <= dac) {
return idx; return idx;
}
} }
return 0; return 0;
} }
@ -214,9 +215,10 @@ static int sa_baudstep = 50;
static void saAutobaud(void) static void saAutobaud(void)
{ {
if (saStat.pktsent < 10) if (saStat.pktsent < 10) {
// Not enough samples collected // Not enough samples collected
return; return;
}
#if 0 #if 0
dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
@ -277,8 +279,9 @@ static void saProcessResponse(uint8_t *buf, int len)
switch (resp) { switch (resp) {
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
if (len < 7) if (len < 7) {
break; break;
}
saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
saDevice.channel = buf[2]; saDevice.channel = buf[2];
@ -301,10 +304,11 @@ static void saProcessResponse(uint8_t *buf, int len)
break; break;
case SA_CMD_SET_FREQ: // Set Frequency case SA_CMD_SET_FREQ: // Set Frequency
if (len < 5) if (len < 5) {
break; break;
}
uint16_t freq = (buf[2] << 8)|buf[3]; const uint16_t freq = (buf[2] << 8)|buf[3];
if (freq & SA_FREQ_GETPIT) { if (freq & SA_FREQ_GETPIT) {
saDevice.orfreq = freq & SA_FREQ_MASK; saDevice.orfreq = freq & SA_FREQ_MASK;
@ -328,8 +332,9 @@ static void saProcessResponse(uint8_t *buf, int len)
} }
// Debug // Debug
if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
saPrintSettings(); saPrintSettings();
}
saDevicePrev = saDevice; saDevicePrev = saDevice;
#ifdef VTX_COMMON #ifdef VTX_COMMON
@ -427,12 +432,11 @@ static void saReceiveFramer(uint8_t c)
static void saSendFrame(uint8_t *buf, int len) static void saSendFrame(uint8_t *buf, int len)
{ {
int i;
serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
for (i = 0 ; i < len ; i++) for (int i = 0 ; i < len ; i++) {
serialWrite(smartAudioSerialPort, buf[i]); serialWrite(smartAudioSerialPort, buf[i]);
}
serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
@ -467,10 +471,9 @@ static void saResendCmd(void)
static void saSendCmd(uint8_t *buf, int len) static void saSendCmd(uint8_t *buf, int len)
{ {
int i; for (int i = 0 ; i < len ; i++) {
for (i = 0 ; i < len ; i++)
sa_osbuf[i] = buf[i]; sa_osbuf[i] = buf[i];
}
sa_oslen = len; sa_oslen = len;
sa_outstanding = (buf[2] >> 1); sa_outstanding = (buf[2] >> 1);
@ -513,8 +516,9 @@ static bool saQueueFull(void)
static void saQueueCmd(uint8_t *buf, int len) static void saQueueCmd(uint8_t *buf, int len)
{ {
if (saQueueFull()) if (saQueueFull()) {
return; return;
}
sa_queue[sa_qhead].buf = buf; sa_queue[sa_qhead].buf = buf;
sa_queue[sa_qhead].len = len; sa_queue[sa_qhead].len = len;
@ -523,8 +527,9 @@ static void saQueueCmd(uint8_t *buf, int len)
static void saSendQueue(void) static void saSendQueue(void)
{ {
if (saQueueEmpty()) if (saQueueEmpty()) {
return; return;
}
saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len); saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
sa_qtail = (sa_qtail + 1) % SA_QSIZE; sa_qtail = (sa_qtail + 1) % SA_QSIZE;
@ -601,8 +606,9 @@ void saSetPowerByIndex(uint8_t index)
return; return;
} }
if (index > 3) if (index > 3) {
return; return;
}
buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2;
buf[5] = CRC8(buf, 5); buf[5] = CRC8(buf, 5);
@ -646,8 +652,9 @@ void vtxSAProcess(uint32_t now)
{ {
static char initPhase = 0; static char initPhase = 0;
if (smartAudioSerialPort == NULL) if (smartAudioSerialPort == NULL) {
return; return;
}
while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
uint8_t c = serialRead(smartAudioSerialPort); uint8_t c = serialRead(smartAudioSerialPort);
@ -673,8 +680,7 @@ void vtxSAProcess(uint32_t now)
break; break;
} }
if ((sa_outstanding != SA_CMD_NONE) if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
&& (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));
// XXX Todo: Resend termination and possible offline transition // XXX Todo: Resend termination and possible offline transition
@ -694,8 +700,9 @@ void vtxSAProcess(uint32_t now)
// Testing VTX_COMMON API // Testing VTX_COMMON API
{ {
static uint32_t lastMonitorUs = 0; static uint32_t lastMonitorUs = 0;
if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) {
return; return;
}
static uint8_t monBand; static uint8_t monBand;
static uint8_t monChan; static uint8_t monChan;
@ -725,8 +732,9 @@ bool vtxSAIsReady(void)
void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) void vtxSASetBandAndChannel(uint8_t band, uint8_t channel)
{ {
if (band && channel) if (band && channel) {
saSetBandAndChannel(band - 1, channel - 1); saSetBandAndChannel(band - 1, channel - 1);
}
} }
void vtxSASetPowerByIndex(uint8_t index) void vtxSASetPowerByIndex(uint8_t index)
@ -741,8 +749,9 @@ void vtxSASetPowerByIndex(uint8_t index)
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.
@ -751,11 +760,13 @@ void vtxSASetPitMode(uint8_t onoff)
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);
@ -764,8 +775,9 @@ void vtxSASetPitMode(uint8_t onoff)
bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
{ {
if (!vtxSAIsReady()) if (!vtxSAIsReady()) {
return false; return false;
}
*pBand = (saDevice.channel / 8) + 1; *pBand = (saDevice.channel / 8) + 1;
*pChannel = (saDevice.channel % 8) + 1; *pChannel = (saDevice.channel % 8) + 1;
@ -774,8 +786,9 @@ bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
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;
@ -783,8 +796,9 @@ bool vtxSAGetPowerIndex(uint8_t *pIndex)
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;

View file

@ -1,5 +1,10 @@
#pragma once #pragma once
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
// opmode flags, GET side // opmode flags, GET side
#define SA_MODE_GET_FREQ_BY_FREQ 1 #define SA_MODE_GET_FREQ_BY_FREQ 1
#define SA_MODE_GET_PITMODE 2 #define SA_MODE_GET_PITMODE 2

View file

@ -19,10 +19,9 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <ctype.h>
#include <string.h>
#include "platform.h" #include "platform.h"
#include "build/debug.h" #include "build/debug.h"
#if defined(VTX_COMMON) #if defined(VTX_COMMON)
@ -53,13 +52,10 @@ const char * const vtx58ChannelNames[] = {
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel) bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
{ {
int8_t band;
uint8_t channel;
// Use reverse lookup order so that 5880Mhz // Use reverse lookup order so that 5880Mhz
// get Raceband 7 instead of Fatshark 8. // get Raceband 7 instead of Fatshark 8.
for (band = 4 ; band >= 0 ; band--) { for (int band = 4 ; band >= 0 ; band--) {
for (channel = 0 ; channel < 8 ; channel++) { for (int channel = 0 ; channel < 8 ; channel++) {
if (vtx58frequencyTable[band][channel] == freq) { if (vtx58frequencyTable[band][channel] == freq) {
*pBand = band + 1; *pBand = band + 1;
*pChannel = channel + 1; *pChannel = channel + 1;

View file

@ -2,6 +2,8 @@
#include <stdint.h> #include <stdint.h>
#include "platform.h"
#if defined(VTX_COMMON) #if defined(VTX_COMMON)
extern const uint16_t vtx58frequencyTable[5][8]; extern const uint16_t vtx58frequencyTable[5][8];

View file

@ -107,16 +107,18 @@ static uint8_t trampChecksum(uint8_t *trampBuf)
{ {
uint8_t cksum = 0; uint8_t cksum = 0;
for (int i = 1 ; i < 14 ; i++) for (int i = 1 ; i < 14 ; i++) {
cksum += trampBuf[i]; cksum += trampBuf[i];
}
return cksum; return cksum;
} }
void trampCmdU16(uint8_t cmd, uint16_t param) void trampCmdU16(uint8_t cmd, uint16_t param)
{ {
if (!trampSerialPort) if (!trampSerialPort) {
return; return;
}
memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer)); memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
trampReqBuffer[0] = 15; trampReqBuffer[0] = 15;
@ -130,8 +132,9 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
void trampSetFreq(uint16_t freq) void trampSetFreq(uint16_t freq)
{ {
trampConfFreq = freq; trampConfFreq = freq;
if (trampConfFreq != trampCurFreq) if (trampConfFreq != trampCurFreq) {
trampFreqRetries = TRAMP_MAX_RETRIES; trampFreqRetries = TRAMP_MAX_RETRIES;
}
} }
void trampSendFreq(uint16_t freq) void trampSendFreq(uint16_t freq)
@ -147,8 +150,9 @@ void trampSetBandAndChannel(uint8_t band, uint8_t channel)
void trampSetRFPower(uint16_t level) void trampSetRFPower(uint16_t level)
{ {
trampConfPower = level; trampConfPower = level;
if (trampConfPower != trampPower) if (trampConfPower != trampPower) {
trampPowerRetries = TRAMP_MAX_RETRIES; trampPowerRetries = TRAMP_MAX_RETRIES;
}
} }
void trampSendRFPower(uint16_t level) void trampSendRFPower(uint16_t level)
@ -159,8 +163,9 @@ void trampSendRFPower(uint16_t level)
// return false if error // return false if error
bool trampCommitChanges(void) bool trampCommitChanges(void)
{ {
if (trampStatus != TRAMP_STATUS_ONLINE) if (trampStatus != TRAMP_STATUS_ONLINE) {
return false; return false;
}
trampStatus = TRAMP_STATUS_SET_FREQ_PW; trampStatus = TRAMP_STATUS_SET_FREQ_PW;
return true; return true;
@ -174,12 +179,12 @@ void trampSetPitMode(uint8_t onoff)
// returns completed response code // returns completed response code
char trampHandleResponse(void) char trampHandleResponse(void)
{ {
uint8_t respCode = trampRespBuffer[1]; const uint8_t respCode = trampRespBuffer[1];
switch (respCode) { switch (respCode) {
case 'r': case 'r':
{ {
uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
if (min_freq != 0) { if (min_freq != 0) {
trampRFFreqMin = min_freq; trampRFFreqMin = min_freq;
trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
@ -193,7 +198,7 @@ char trampHandleResponse(void)
case 'v': case 'v':
{ {
uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
if (freq != 0) { if (freq != 0) {
trampCurFreq = freq; trampCurFreq = freq;
trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
@ -212,7 +217,7 @@ char trampHandleResponse(void)
case 's': case 's':
{ {
uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
if (temp != 0) { if (temp != 0) {
trampTemperature = temp; trampTemperature = temp;
return 's'; return 's';
@ -241,10 +246,11 @@ static void trampResetReceiver(void)
static bool trampIsValidResponseCode(uint8_t code) static bool trampIsValidResponseCode(uint8_t code)
{ {
if (code == 'r' || code == 'v' || code == 's') if (code == 'r' || code == 'v' || code == 's') {
return true; return true;
else } else {
return false; return false;
}
} }
// returns completed response code or 0 // returns completed response code or 0
@ -252,11 +258,12 @@ static char trampReceive(uint32_t currentTimeUs)
{ {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
if (!trampSerialPort) if (!trampSerialPort) {
return 0; return 0;
}
while (serialRxBytesWaiting(trampSerialPort)) { while (serialRxBytesWaiting(trampSerialPort)) {
uint8_t c = serialRead(trampSerialPort); const uint8_t c = serialRead(trampSerialPort);
trampRespBuffer[trampReceivePos++] = c; trampRespBuffer[trampReceivePos++] = c;
switch (trampReceiveState) { switch (trampReceiveState) {
@ -290,6 +297,7 @@ static char trampReceive(uint32_t currentTimeUs)
default: default:
trampResetReceiver(); trampResetReceiver();
break;
} }
} }
@ -326,10 +334,11 @@ void vtxTrampProcess(uint32_t currentTimeUs)
static uint16_t debugPowReqCounter = 0; static uint16_t debugPowReqCounter = 0;
#endif #endif
if (trampStatus == TRAMP_STATUS_BAD_DEVICE) if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
return; return;
}
char replyCode = trampReceive(currentTimeUs); const char replyCode = trampReceive(currentTimeUs);
#ifdef TRAMP_DEBUG #ifdef TRAMP_DEBUG
debug[0] = trampStatus; debug[0] = trampStatus;
@ -337,13 +346,15 @@ void vtxTrampProcess(uint32_t currentTimeUs)
switch (replyCode) { switch (replyCode) {
case 'r': case 'r':
if (trampStatus <= TRAMP_STATUS_OFFLINE) if (trampStatus <= TRAMP_STATUS_OFFLINE) {
trampStatus = TRAMP_STATUS_ONLINE; trampStatus = TRAMP_STATUS_ONLINE;
}
break; break;
case 'v': case 'v':
if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) {
trampStatus = TRAMP_STATUS_SET_FREQ_PW; trampStatus = TRAMP_STATUS_SET_FREQ_PW;
}
break; break;
} }
@ -353,14 +364,15 @@ void vtxTrampProcess(uint32_t currentTimeUs)
case TRAMP_STATUS_ONLINE: case TRAMP_STATUS_ONLINE:
if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
if (trampStatus == TRAMP_STATUS_OFFLINE) if (trampStatus == TRAMP_STATUS_OFFLINE) {
trampQueryR(); trampQueryR();
else { } else {
static unsigned int cnt = 0; static unsigned int cnt = 0;
if (((cnt++) & 1) == 0) if (((cnt++) & 1) == 0) {
trampQueryV(); trampQueryV();
else } else {
trampQueryS(); trampQueryS();
}
} }
lastQueryTimeUs = currentTimeUs; lastQueryTimeUs = currentTimeUs;
@ -377,8 +389,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
debugFreqReqCounter++; debugFreqReqCounter++;
#endif #endif
done = false; done = false;
} } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
trampSendRFPower(trampConfPower); trampSendRFPower(trampConfPower);
trampPowerRetries--; trampPowerRetries--;
#ifdef TRAMP_DEBUG #ifdef TRAMP_DEBUG
@ -392,8 +403,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
// delay next status query by 300ms // delay next status query by 300ms
lastQueryTimeUs = currentTimeUs + 300 * 1000; lastQueryTimeUs = currentTimeUs + 300 * 1000;
} } else {
else {
// everything has been done, let's return to original state // everything has been done, let's return to original state
trampStatus = TRAMP_STATUS_ONLINE; trampStatus = TRAMP_STATUS_ONLINE;
// reset configuration value in case it failed (no more retries) // reset configuration value in case it failed (no more retries)
@ -464,8 +474,9 @@ void vtxTrampSetPitMode(uint8_t onoff)
bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
{ {
if (!vtxTrampIsReady()) if (!vtxTrampIsReady()) {
return false; return false;
}
*pBand = trampBand; *pBand = trampBand;
*pChannel = trampChannel; *pChannel = trampChannel;
@ -474,8 +485,9 @@ bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
bool vtxTrampGetPowerIndex(uint8_t *pIndex) bool vtxTrampGetPowerIndex(uint8_t *pIndex)
{ {
if (!vtxTrampIsReady()) if (!vtxTrampIsReady()) {
return false; return false;
}
if (trampConfiguredPower > 0) { if (trampConfiguredPower > 0) {
for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
@ -491,8 +503,9 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
bool vtxTrampGetPitMode(uint8_t *pOnOff) bool vtxTrampGetPitMode(uint8_t *pOnOff)
{ {
if (!vtxTrampIsReady()) if (!vtxTrampIsReady()) {
return false; return false;
}
*pOnOff = trampPitMode; *pOnOff = trampPitMode;
return true; return true;

View file

@ -1,5 +1,7 @@
#pragma once #pragma once
#include <stdint.h>
#define VTX_TRAMP_POWER_COUNT 5 #define VTX_TRAMP_POWER_COUNT 5
extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT]; extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT];
extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1]; extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1];