Fix issue entering pitmode.
Make as many variables as possible private. Re-work settings application into state machine. Remove legacy interfaces. Update OSD interface following implementation rework. Review updates.
This commit is contained in:
parent
6a92bf39b4
commit
367cc3a513
|
@ -97,4 +97,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
||||||
"GYRO_SAMPLE",
|
"GYRO_SAMPLE",
|
||||||
"RX_TIMING",
|
"RX_TIMING",
|
||||||
"D_LPF",
|
"D_LPF",
|
||||||
|
"VTX_TRAMP",
|
||||||
};
|
};
|
||||||
|
|
|
@ -113,6 +113,7 @@ typedef enum {
|
||||||
DEBUG_GYRO_SAMPLE,
|
DEBUG_GYRO_SAMPLE,
|
||||||
DEBUG_RX_TIMING,
|
DEBUG_RX_TIMING,
|
||||||
DEBUG_D_LPF,
|
DEBUG_D_LPF,
|
||||||
|
DEBUG_VTX_TRAMP,
|
||||||
DEBUG_COUNT
|
DEBUG_COUNT
|
||||||
} debugType_e;
|
} debugType_e;
|
||||||
|
|
||||||
|
|
|
@ -45,6 +45,9 @@ char trampCmsStatusString[31] = "- -- ---- ----";
|
||||||
// m bc ffff tppp
|
// m bc ffff tppp
|
||||||
// 01234567890123
|
// 01234567890123
|
||||||
|
|
||||||
|
static int16_t trampCmsTemp;
|
||||||
|
static OSD_INT16_t trampCmsEntTemp = { &trampCmsTemp, -100, 300, 0 };
|
||||||
|
|
||||||
void trampCmsUpdateStatusString(void)
|
void trampCmsUpdateStatusString(void)
|
||||||
{
|
{
|
||||||
vtxDevice_t *vtxDevice = vtxCommonDevice();
|
vtxDevice_t *vtxDevice = vtxCommonDevice();
|
||||||
|
@ -67,16 +70,23 @@ void trampCmsUpdateStatusString(void)
|
||||||
}
|
}
|
||||||
trampCmsStatusString[4] = ' ';
|
trampCmsStatusString[4] = ' ';
|
||||||
|
|
||||||
if (trampCurFreq)
|
uint16_t freq;
|
||||||
tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq);
|
if (!vtxCommonGetFrequency(vtxDevice, &freq) || (freq == 0)) {
|
||||||
else
|
|
||||||
tfp_sprintf(&trampCmsStatusString[5], "----");
|
tfp_sprintf(&trampCmsStatusString[5], "----");
|
||||||
|
} else {
|
||||||
|
tfp_sprintf(&trampCmsStatusString[5], "%4d", freq);
|
||||||
|
}
|
||||||
|
|
||||||
if (trampPower) {
|
uint16_t actualPower = vtxTrampGetCurrentActualPower();
|
||||||
tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampPower == trampConfiguredPower) ? ' ' : '*', trampPower);
|
uint8_t powerIndex;
|
||||||
|
uint16_t powerValue;
|
||||||
|
if (actualPower > 0 && vtxCommonGetPowerIndex(vtxDevice, &powerIndex) && vtxCommonLookupPowerValue(vtxDevice, powerIndex, &powerValue)) {
|
||||||
|
tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (actualPower == powerValue) ? ' ' : '*', actualPower);
|
||||||
} else {
|
} else {
|
||||||
tfp_sprintf(&trampCmsStatusString[9], " ----");
|
tfp_sprintf(&trampCmsStatusString[9], " ----");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
trampCmsTemp = vtxTrampGetCurrentTemp();
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t trampCmsPitMode = 0;
|
uint8_t trampCmsPitMode = 0;
|
||||||
|
@ -105,11 +115,12 @@ static const void *trampCmsConfigBand(displayPort_t *pDisp, const void *self)
|
||||||
UNUSED(pDisp);
|
UNUSED(pDisp);
|
||||||
UNUSED(self);
|
UNUSED(self);
|
||||||
|
|
||||||
if (trampCmsBand == 0)
|
if (trampCmsBand == 0) {
|
||||||
// Bounce back
|
// Bounce back
|
||||||
trampCmsBand = 1;
|
trampCmsBand = 1;
|
||||||
else
|
} else {
|
||||||
trampCmsUpdateFreqRef();
|
trampCmsUpdateFreqRef();
|
||||||
|
}
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
@ -119,11 +130,12 @@ static const void *trampCmsConfigChan(displayPort_t *pDisp, const void *self)
|
||||||
UNUSED(pDisp);
|
UNUSED(pDisp);
|
||||||
UNUSED(self);
|
UNUSED(self);
|
||||||
|
|
||||||
if (trampCmsChan == 0)
|
if (trampCmsChan == 0) {
|
||||||
// Bounce back
|
// Bounce back
|
||||||
trampCmsChan = 1;
|
trampCmsChan = 1;
|
||||||
else
|
} else {
|
||||||
trampCmsUpdateFreqRef();
|
trampCmsUpdateFreqRef();
|
||||||
|
}
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
@ -133,14 +145,17 @@ static const void *trampCmsConfigPower(displayPort_t *pDisp, const void *self)
|
||||||
UNUSED(pDisp);
|
UNUSED(pDisp);
|
||||||
UNUSED(self);
|
UNUSED(self);
|
||||||
|
|
||||||
if (trampCmsPower == 0)
|
if (trampCmsPower == 0) {
|
||||||
// Bounce back
|
// Bounce back
|
||||||
trampCmsPower = 1;
|
trampCmsPower = 1;
|
||||||
|
}
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
static OSD_INT16_t trampCmsEntTemp = { &trampTemperature, -100, 300, 0 };
|
#define TRAMP_PIT_STATUS_NA (0)
|
||||||
|
#define TRAMP_PIT_STATUS_OFF (1)
|
||||||
|
#define TRAMP_PIT_STATUS_ON (2)
|
||||||
|
|
||||||
static const char * const trampCmsPitModeNames[] = {
|
static const char * const trampCmsPitModeNames[] = {
|
||||||
"---", "OFF", "ON "
|
"---", "OFF", "ON "
|
||||||
|
@ -153,11 +168,12 @@ static const void *trampCmsSetPitMode(displayPort_t *pDisp, const void *self)
|
||||||
UNUSED(pDisp);
|
UNUSED(pDisp);
|
||||||
UNUSED(self);
|
UNUSED(self);
|
||||||
|
|
||||||
if (trampCmsPitMode == 0) {
|
if (trampCmsPitMode == TRAMP_PIT_STATUS_NA) {
|
||||||
// Bouce back
|
// Bouce back
|
||||||
trampCmsPitMode = 1;
|
trampCmsPitMode = TRAMP_PIT_STATUS_OFF;
|
||||||
} else {
|
} else {
|
||||||
trampSetPitMode(trampCmsPitMode - 1);
|
vtxCommonSetPitMode(vtxCommonDevice(),
|
||||||
|
(trampCmsPitMode == TRAMP_PIT_STATUS_OFF) ? 0 : 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
|
@ -172,9 +188,6 @@ static const void *trampCmsCommence(displayPort_t *pDisp, const void *self)
|
||||||
vtxCommonSetBandAndChannel(device, trampCmsBand, trampCmsChan);
|
vtxCommonSetBandAndChannel(device, trampCmsBand, trampCmsChan);
|
||||||
vtxCommonSetPowerByIndex(device, trampCmsPower);
|
vtxCommonSetPowerByIndex(device, trampCmsPower);
|
||||||
|
|
||||||
// If it fails, the user should retry later
|
|
||||||
trampCommitChanges();
|
|
||||||
|
|
||||||
// update'vtx_' settings
|
// update'vtx_' settings
|
||||||
vtxSettingsConfigMutable()->band = trampCmsBand;
|
vtxSettingsConfigMutable()->band = trampCmsBand;
|
||||||
vtxSettingsConfigMutable()->channel = trampCmsChan;
|
vtxSettingsConfigMutable()->channel = trampCmsChan;
|
||||||
|
@ -189,6 +202,7 @@ static const void *trampCmsCommence(displayPort_t *pDisp, const void *self)
|
||||||
static bool trampCmsInitSettings(void)
|
static bool trampCmsInitSettings(void)
|
||||||
{
|
{
|
||||||
vtxDevice_t *device = vtxCommonDevice();
|
vtxDevice_t *device = vtxCommonDevice();
|
||||||
|
unsigned vtxStatus;
|
||||||
|
|
||||||
if (!device) {
|
if (!device) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -197,12 +211,14 @@ static bool trampCmsInitSettings(void)
|
||||||
vtxCommonGetBandAndChannel(device, &trampCmsBand, &trampCmsChan);
|
vtxCommonGetBandAndChannel(device, &trampCmsBand, &trampCmsChan);
|
||||||
|
|
||||||
trampCmsUpdateFreqRef();
|
trampCmsUpdateFreqRef();
|
||||||
trampCmsPitMode = trampPitMode + 1;
|
if (vtxCommonGetStatus(device, &vtxStatus)) {
|
||||||
|
trampCmsPitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? TRAMP_PIT_STATUS_ON : TRAMP_PIT_STATUS_OFF;
|
||||||
|
} else {
|
||||||
|
trampCmsPitMode = TRAMP_PIT_STATUS_NA;
|
||||||
|
}
|
||||||
|
|
||||||
if (trampConfiguredPower > 0) {
|
if (!vtxCommonGetPowerIndex(vtxCommonDevice(), &trampCmsPower)) {
|
||||||
if (!vtxCommonGetPowerIndex(vtxCommonDevice(), &trampCmsPower)) {
|
trampCmsPower = 1;
|
||||||
trampCmsPower = 1;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
trampCmsEntBand.val = &trampCmsBand;
|
trampCmsEntBand.val = &trampCmsBand;
|
||||||
|
|
|
@ -44,6 +44,18 @@
|
||||||
#include "io/vtx_control.h"
|
#include "io/vtx_control.h"
|
||||||
#include "io/vtx.h"
|
#include "io/vtx.h"
|
||||||
|
|
||||||
|
// Maximum number of requests sent to try a config change
|
||||||
|
// Some VTX fail to respond to every request (like Matek FCHUB-VTX) so
|
||||||
|
// we sometimes need multiple retries to get the VTX to respond.
|
||||||
|
#define TRAMP_MAX_RETRIES (20)
|
||||||
|
|
||||||
|
// Race lock - settings can't be changed
|
||||||
|
#define TRAMP_CONTROL_RACE_LOCK (0x01)
|
||||||
|
|
||||||
|
// Define periods between requests
|
||||||
|
#define TRAMP_MIN_REQUEST_PERIOD_US (200 * 1000) // 200ms
|
||||||
|
#define TRAMP_STATUS_REQUEST_PERIOD_US (1000 * 1000) // 1s
|
||||||
|
|
||||||
#if (defined(USE_CMS) || defined(USE_VTX_COMMON)) && !defined(USE_VTX_TABLE)
|
#if (defined(USE_CMS) || defined(USE_VTX_COMMON)) && !defined(USE_VTX_TABLE)
|
||||||
const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
|
const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
|
||||||
25, 100, 200, 400, 600
|
25, 100, 200, 400, 600
|
||||||
|
@ -61,52 +73,72 @@ static vtxDevice_t vtxTramp = {
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// Device serial port instance
|
||||||
static serialPort_t *trampSerialPort = NULL;
|
static serialPort_t *trampSerialPort = NULL;
|
||||||
|
|
||||||
|
// Serial transmit and receive buffers
|
||||||
static uint8_t trampReqBuffer[16];
|
static uint8_t trampReqBuffer[16];
|
||||||
static uint8_t trampRespBuffer[16];
|
static uint8_t trampRespBuffer[16];
|
||||||
|
|
||||||
|
// Module state machine
|
||||||
typedef enum {
|
typedef enum {
|
||||||
TRAMP_STATUS_BAD_DEVICE = -1,
|
// Offline - device hasn't responded yet
|
||||||
TRAMP_STATUS_OFFLINE = 0,
|
TRAMP_STATUS_OFFLINE = 0,
|
||||||
TRAMP_STATUS_ONLINE,
|
// Init - fetching current settings from device
|
||||||
TRAMP_STATUS_SET_FREQ_PW,
|
TRAMP_STATUS_INIT,
|
||||||
TRAMP_STATUS_CHECK_FREQ_PW
|
// Online - device is ready and being monitored - freq/power/pitmode
|
||||||
|
TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT,
|
||||||
|
// Online - device is ready and being monitored - temperature
|
||||||
|
TRAMP_STATUS_ONLINE_MONITOR_TEMP,
|
||||||
|
// Online - device is ready and config has just been updated
|
||||||
|
TRAMP_STATUS_ONLINE_CONFIG
|
||||||
} trampStatus_e;
|
} trampStatus_e;
|
||||||
|
|
||||||
trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
|
static trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
|
||||||
|
|
||||||
|
// Device limits, read from device during init
|
||||||
static uint32_t trampRFFreqMin = 0;
|
static uint32_t trampRFFreqMin = 0;
|
||||||
static uint32_t trampRFFreqMax = 0;
|
static uint32_t trampRFFreqMax = 0;
|
||||||
static uint32_t trampRFPowerMax;
|
static uint32_t trampRFPowerMax;
|
||||||
|
|
||||||
uint32_t trampCurFreq = 0;
|
// Device status, read from device periodically
|
||||||
static uint32_t trampLastFreq = 0;
|
static uint32_t trampCurFreq = 0;
|
||||||
uint16_t trampPower = 0; // Actual transmitting power
|
static uint16_t trampCurConfPower = 0; // Configured power
|
||||||
uint16_t trampConfiguredPower = 0; // Configured transmitting power
|
static uint16_t trampCurActPower = 0; // Actual power
|
||||||
static uint16_t trampLastPower = 0;
|
static uint8_t trampCurPitMode = 0;
|
||||||
int16_t trampTemperature = 0;
|
static int16_t trampCurTemp = 0;
|
||||||
uint8_t trampPitMode = 0;
|
static uint8_t trampCurControlMode = 0;
|
||||||
static uint8_t trampControlMode = 0;
|
|
||||||
|
|
||||||
#define TRAMP_CONTROL_RACE_LOCK 0x01
|
// Device configuration, desired state of device
|
||||||
|
static uint32_t trampConfFreq = 0;
|
||||||
|
static uint16_t trampConfPower = 0;
|
||||||
|
static uint8_t trampConfPitMode = 0;
|
||||||
|
|
||||||
// Maximum number of requests sent to try a config change
|
// Last device configuration, last desired state of device - used to reset
|
||||||
// Some VTX fail to respond to every request (like Matek FCHUB-VTX) so
|
// retry count
|
||||||
// we sometimes need multiple retries to get the VTX to respond.
|
static uint32_t trampLastConfFreq = 0;
|
||||||
#define TRAMP_MAX_RETRIES 20
|
static uint16_t trampLastConfPower = 0;
|
||||||
|
static uint8_t trampLastConfPitMode = 0;
|
||||||
|
|
||||||
uint32_t trampConfFreq = 0;
|
// Retry count
|
||||||
uint8_t trampFreqRetries = 0;
|
static uint8_t trampRetryCount = TRAMP_MAX_RETRIES;
|
||||||
|
|
||||||
uint16_t trampConfPower = 0;
|
// Receive state machine
|
||||||
uint8_t trampPowerRetries = 0;
|
typedef enum {
|
||||||
|
S_WAIT_LEN = 0, // Waiting for a packet len
|
||||||
|
S_WAIT_CODE, // Waiting for a response code
|
||||||
|
S_DATA, // Waiting for rest of the packet.
|
||||||
|
} trampReceiveState_e;
|
||||||
|
|
||||||
static void trampWriteBuf(uint8_t *buf)
|
static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
|
||||||
{
|
|
||||||
serialWriteBuf(trampSerialPort, buf, 16);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
// Receive buffer index
|
||||||
|
static int trampReceivePos = 0;
|
||||||
|
|
||||||
|
// Last action time
|
||||||
|
static timeUs_t trampLastTimeUs = 0;
|
||||||
|
|
||||||
|
// Calculate tramp protocol checksum of provided buffer
|
||||||
static uint8_t trampChecksum(uint8_t *trampBuf)
|
static uint8_t trampChecksum(uint8_t *trampBuf)
|
||||||
{
|
{
|
||||||
uint8_t cksum = 0;
|
uint8_t cksum = 0;
|
||||||
|
@ -118,16 +150,13 @@ static uint8_t trampChecksum(uint8_t *trampBuf)
|
||||||
return cksum;
|
return cksum;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool trampVtxControlEnabled(void)
|
// Check if race lock is enabled
|
||||||
{
|
|
||||||
return !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE);
|
|
||||||
}
|
|
||||||
|
|
||||||
static bool trampVtxRaceLockEnabled(void)
|
static bool trampVtxRaceLockEnabled(void)
|
||||||
{
|
{
|
||||||
return trampControlMode & TRAMP_CONTROL_RACE_LOCK;
|
return trampCurControlMode & TRAMP_CONTROL_RACE_LOCK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Send tramp protocol frame to device
|
||||||
static void trampSendU16(uint8_t cmd, uint16_t param)
|
static void trampSendU16(uint8_t cmd, uint16_t param)
|
||||||
{
|
{
|
||||||
if (!trampSerialPort) {
|
if (!trampSerialPort) {
|
||||||
|
@ -135,82 +164,25 @@ static void trampSendU16(uint8_t cmd, uint16_t param)
|
||||||
}
|
}
|
||||||
|
|
||||||
memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
|
memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
|
||||||
trampReqBuffer[0] = 15;
|
trampReqBuffer[0] = 0x0F;
|
||||||
trampReqBuffer[1] = cmd;
|
trampReqBuffer[1] = cmd;
|
||||||
trampReqBuffer[2] = param & 0xff;
|
trampReqBuffer[2] = param & 0xff;
|
||||||
trampReqBuffer[3] = (param >> 8) & 0xff;
|
trampReqBuffer[3] = (param >> 8) & 0xff;
|
||||||
trampReqBuffer[14] = trampChecksum(trampReqBuffer);
|
trampReqBuffer[14] = trampChecksum(trampReqBuffer);
|
||||||
trampWriteBuf(trampReqBuffer);
|
serialWriteBuf(trampSerialPort, trampReqBuffer, 16);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Send command to device
|
||||||
static void trampSendCommand(uint8_t cmd, uint16_t param)
|
static void trampSendCommand(uint8_t cmd, uint16_t param)
|
||||||
{
|
{
|
||||||
if (trampVtxControlEnabled()) {
|
// Is VTX control enabled?
|
||||||
|
if (!IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
|
||||||
|
// Yes, send command
|
||||||
trampSendU16(cmd, param);
|
trampSendU16(cmd, param);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool trampValidateFreq(uint16_t freq)
|
// Process response and return code if valid else 0
|
||||||
{
|
|
||||||
if (trampRFFreqMin != 0 && trampRFFreqMax != 0) {
|
|
||||||
return (freq >= trampRFFreqMin && freq <= trampRFFreqMax);
|
|
||||||
} else {
|
|
||||||
return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static void trampDevSetFreq(uint16_t freq)
|
|
||||||
{
|
|
||||||
trampConfFreq = freq;
|
|
||||||
if (trampConfFreq != trampCurFreq) {
|
|
||||||
trampFreqRetries = TRAMP_MAX_RETRIES;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void trampSetFreq(uint16_t freq)
|
|
||||||
{
|
|
||||||
trampDevSetFreq(freq);
|
|
||||||
}
|
|
||||||
|
|
||||||
void trampSendFreq(uint16_t freq)
|
|
||||||
{
|
|
||||||
if (!trampVtxRaceLockEnabled()) {
|
|
||||||
trampSendCommand('F', freq);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void trampSetRFPower(uint16_t level)
|
|
||||||
{
|
|
||||||
trampConfPower = level;
|
|
||||||
if (trampConfPower != trampConfiguredPower) {
|
|
||||||
trampPowerRetries = TRAMP_MAX_RETRIES;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void trampSendRFPower(uint16_t level)
|
|
||||||
{
|
|
||||||
if (!trampVtxRaceLockEnabled()) {
|
|
||||||
trampSendCommand('P', level);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// return false if error
|
|
||||||
bool trampCommitChanges(void)
|
|
||||||
{
|
|
||||||
if (trampStatus != TRAMP_STATUS_ONLINE) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void trampSetPitMode(uint8_t onoff)
|
|
||||||
{
|
|
||||||
trampSendCommand('I', onoff ? 0 : 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
// returns completed response code
|
|
||||||
static char trampHandleResponse(void)
|
static char trampHandleResponse(void)
|
||||||
{
|
{
|
||||||
const uint8_t respCode = trampRespBuffer[1];
|
const uint8_t respCode = trampRespBuffer[1];
|
||||||
|
@ -219,95 +191,66 @@ static char trampHandleResponse(void)
|
||||||
case 'r':
|
case 'r':
|
||||||
{
|
{
|
||||||
const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
||||||
|
// Check we're not reading the request (indicated by freq zero)
|
||||||
if (min_freq != 0) {
|
if (min_freq != 0) {
|
||||||
|
// Got response, update device limits
|
||||||
trampRFFreqMin = min_freq;
|
trampRFFreqMin = min_freq;
|
||||||
trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
||||||
trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
|
trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
|
||||||
return 'r';
|
return 'r';
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
// throw bytes echoed from tx to rx in bidirectional mode away
|
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
|
|
||||||
case 'v':
|
case 'v':
|
||||||
{
|
{
|
||||||
const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
||||||
|
// Check we're not reading the request (indicated by freq zero)
|
||||||
if (freq != 0) {
|
if (freq != 0) {
|
||||||
|
// Got response, update device status
|
||||||
trampCurFreq = freq;
|
trampCurFreq = freq;
|
||||||
if (trampLastFreq != trampCurFreq) {
|
trampCurConfPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
||||||
trampFreqRetries = TRAMP_MAX_RETRIES;
|
trampCurControlMode = trampRespBuffer[6]; // Currently only used for race lock
|
||||||
}
|
trampCurPitMode = trampRespBuffer[7];
|
||||||
trampLastFreq = trampCurFreq;
|
trampCurActPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
|
||||||
|
|
||||||
trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
|
||||||
if (trampConfiguredPower != trampLastPower) {
|
|
||||||
trampPowerRetries = TRAMP_MAX_RETRIES;
|
|
||||||
}
|
|
||||||
trampLastPower = trampConfiguredPower;
|
|
||||||
|
|
||||||
trampControlMode = trampRespBuffer[6]; // Currently only used for race lock
|
|
||||||
|
|
||||||
trampPitMode = trampRespBuffer[7];
|
|
||||||
trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
|
|
||||||
|
|
||||||
|
// Init config with current status if not set
|
||||||
if (trampConfFreq == 0) {
|
if (trampConfFreq == 0) {
|
||||||
trampConfFreq = trampCurFreq;
|
trampConfFreq = trampCurFreq;
|
||||||
}
|
}
|
||||||
if (trampConfPower == 0) {
|
if (trampConfPower == 0) {
|
||||||
trampConfPower = trampConfiguredPower;
|
trampConfPower = trampCurConfPower;
|
||||||
}
|
}
|
||||||
|
|
||||||
return 'v';
|
return 'v';
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
// throw bytes echoed from tx to rx in bidirectional mode away
|
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
|
|
||||||
case 's':
|
case 's':
|
||||||
{
|
{
|
||||||
const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
|
const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
|
||||||
|
// Check we're not reading the request (indicated by temp zero)
|
||||||
if (temp != 0) {
|
if (temp != 0) {
|
||||||
trampTemperature = temp;
|
// Got response, update device status
|
||||||
|
trampCurTemp = temp;
|
||||||
return 's';
|
return 's';
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Likely reading a request, return zero to indicate not accepted
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
typedef enum {
|
// Reset receiver state machine
|
||||||
S_WAIT_LEN = 0, // Waiting for a packet len
|
|
||||||
S_WAIT_CODE, // Waiting for a response code
|
|
||||||
S_DATA, // Waiting for rest of the packet.
|
|
||||||
} trampReceiveState_e;
|
|
||||||
|
|
||||||
static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
|
|
||||||
static int trampReceivePos = 0;
|
|
||||||
|
|
||||||
static void trampResetReceiver(void)
|
static void trampResetReceiver(void)
|
||||||
{
|
{
|
||||||
trampReceiveState = S_WAIT_LEN;
|
trampReceiveState = S_WAIT_LEN;
|
||||||
trampReceivePos = 0;
|
trampReceivePos = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool trampIsValidResponseCode(uint8_t code)
|
|
||||||
{
|
|
||||||
if (code == 'r' || code == 'v' || code == 's') {
|
|
||||||
return true;
|
|
||||||
} else {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// returns completed response code or 0
|
// returns completed response code or 0
|
||||||
static char trampReceive(uint32_t currentTimeUs)
|
static char trampReceive()
|
||||||
{
|
{
|
||||||
UNUSED(currentTimeUs);
|
|
||||||
|
|
||||||
if (!trampSerialPort) {
|
if (!trampSerialPort) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -318,183 +261,214 @@ static char trampReceive(uint32_t currentTimeUs)
|
||||||
|
|
||||||
switch (trampReceiveState) {
|
switch (trampReceiveState) {
|
||||||
case S_WAIT_LEN:
|
case S_WAIT_LEN:
|
||||||
if (c == 0x0F) {
|
{
|
||||||
trampReceiveState = S_WAIT_CODE;
|
if (c == 0x0F) {
|
||||||
} else {
|
// Found header byte, advance to wait for code
|
||||||
trampReceivePos = 0;
|
trampReceiveState = S_WAIT_CODE;
|
||||||
}
|
} else {
|
||||||
break;
|
// Unexpected header, reset state machine
|
||||||
|
trampResetReceiver();
|
||||||
case S_WAIT_CODE:
|
|
||||||
if (trampIsValidResponseCode(c)) {
|
|
||||||
trampReceiveState = S_DATA;
|
|
||||||
} else {
|
|
||||||
trampResetReceiver();
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case S_DATA:
|
|
||||||
if (trampReceivePos == 16) {
|
|
||||||
uint8_t cksum = trampChecksum(trampRespBuffer);
|
|
||||||
|
|
||||||
trampResetReceiver();
|
|
||||||
|
|
||||||
if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
|
|
||||||
return trampHandleResponse();
|
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
break;
|
case S_WAIT_CODE:
|
||||||
|
{
|
||||||
|
if (c == 'r' || c == 'v' || c == 's') {
|
||||||
|
// Code is for response is one we're interested in, advance to data
|
||||||
|
trampReceiveState = S_DATA;
|
||||||
|
} else {
|
||||||
|
// Unexpected code, reset state machine
|
||||||
|
trampResetReceiver();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case S_DATA:
|
||||||
|
{
|
||||||
|
if (trampReceivePos == 16) {
|
||||||
|
// Buffer is full, calculate checksum
|
||||||
|
uint8_t cksum = trampChecksum(trampRespBuffer);
|
||||||
|
|
||||||
|
// Reset state machine ready for next response
|
||||||
|
trampResetReceiver();
|
||||||
|
|
||||||
|
if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
|
||||||
|
// Checksum is correct, process response
|
||||||
|
char r = trampHandleResponse();
|
||||||
|
|
||||||
|
// Check response valid else keep on reading
|
||||||
|
if (r != 0) {
|
||||||
|
return r;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
default:
|
default:
|
||||||
trampResetReceiver();
|
{
|
||||||
break;
|
// Invalid state, reset state machine
|
||||||
|
trampResetReceiver();
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void trampQuery(uint8_t cmd)
|
static void trampQuery(uint8_t cmd)
|
||||||
{
|
{
|
||||||
|
// Reset receive buffer and issue command
|
||||||
trampResetReceiver();
|
trampResetReceiver();
|
||||||
trampSendU16(cmd, 0);
|
trampSendU16(cmd, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
void trampQueryR(void)
|
|
||||||
{
|
|
||||||
trampQuery('r');
|
|
||||||
}
|
|
||||||
|
|
||||||
void trampQueryV(void)
|
|
||||||
{
|
|
||||||
trampQuery('v');
|
|
||||||
}
|
|
||||||
|
|
||||||
void trampQueryS(void)
|
|
||||||
{
|
|
||||||
trampQuery('s');
|
|
||||||
}
|
|
||||||
|
|
||||||
static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
|
static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
|
||||||
{
|
{
|
||||||
UNUSED(vtxDevice);
|
UNUSED(vtxDevice);
|
||||||
|
|
||||||
static timeUs_t lastQueryTimeUs = 0;
|
// Read response from device
|
||||||
static bool initSettingsDoneFlag = false;
|
const char replyCode = trampReceive();
|
||||||
|
|
||||||
#ifdef TRAMP_DEBUG
|
|
||||||
static uint16_t debugFreqReqCounter = 0;
|
|
||||||
static uint16_t debugPowReqCounter = 0;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
const char replyCode = trampReceive(currentTimeUs);
|
|
||||||
|
|
||||||
#ifdef TRAMP_DEBUG
|
|
||||||
debug[0] = trampStatus;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
switch (replyCode) {
|
|
||||||
case 'r':
|
|
||||||
if (trampStatus <= TRAMP_STATUS_OFFLINE) {
|
|
||||||
trampStatus = TRAMP_STATUS_ONLINE;
|
|
||||||
|
|
||||||
// once device is ready enter vtx settings
|
|
||||||
if (!initSettingsDoneFlag) {
|
|
||||||
initSettingsDoneFlag = true;
|
|
||||||
// if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'v':
|
|
||||||
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (trampStatus) {
|
|
||||||
|
|
||||||
|
// Act on state
|
||||||
|
switch(trampStatus) {
|
||||||
case TRAMP_STATUS_OFFLINE:
|
case TRAMP_STATUS_OFFLINE:
|
||||||
case TRAMP_STATUS_ONLINE:
|
{
|
||||||
if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
|
// Offline, check for response
|
||||||
|
if (replyCode == 'r') {
|
||||||
|
// Device replied to reset? request, enter init
|
||||||
|
trampStatus = TRAMP_STATUS_INIT;
|
||||||
|
} else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
|
||||||
|
// Min request period exceeded, issue another reset?
|
||||||
|
trampQuery('r');
|
||||||
|
|
||||||
if (trampStatus == TRAMP_STATUS_OFFLINE) {
|
// Update last time
|
||||||
trampQueryR();
|
trampLastTimeUs = currentTimeUs;
|
||||||
} else {
|
}
|
||||||
static unsigned int cnt = 0;
|
break;
|
||||||
if (((cnt++) & 1) == 0) {
|
}
|
||||||
trampQueryV();
|
case TRAMP_STATUS_INIT:
|
||||||
|
{
|
||||||
|
// Initializing, check for response
|
||||||
|
if (replyCode == 'v') {
|
||||||
|
// Device replied to freq / power / pit query, enter online
|
||||||
|
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
|
||||||
|
} else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
|
||||||
|
// Min request period exceeded, issue another query
|
||||||
|
trampQuery('v');
|
||||||
|
|
||||||
|
// Update last time
|
||||||
|
trampLastTimeUs = currentTimeUs;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT:
|
||||||
|
{
|
||||||
|
uint8_t configUpdateRequired = 0;
|
||||||
|
|
||||||
|
// Note after config a status update request is made, a new status
|
||||||
|
// request is made, this request is handled above and should prevent
|
||||||
|
// subsiquent config updates if the config is now correct
|
||||||
|
if (trampRetryCount > 0 && (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US)) {
|
||||||
|
// Config retries remain and min request period exceeded, check freq
|
||||||
|
if (!trampVtxRaceLockEnabled() && (trampConfFreq != trampCurFreq)) {
|
||||||
|
// Freq can be and needs to be updated, issue request
|
||||||
|
trampSendCommand('F', trampConfFreq);
|
||||||
|
|
||||||
|
// Set flag
|
||||||
|
configUpdateRequired = 1;
|
||||||
|
} else if (!trampVtxRaceLockEnabled() && (trampConfPower != trampCurConfPower)) {
|
||||||
|
// Power can be and needs to be updated, issue request
|
||||||
|
trampSendCommand('P', trampConfPower);
|
||||||
|
|
||||||
|
// Set flag
|
||||||
|
configUpdateRequired = 1;
|
||||||
|
} else if (trampConfPitMode != trampCurPitMode) {
|
||||||
|
// Pit mode needs to be updated, issue request
|
||||||
|
trampSendCommand('I', trampConfPitMode);
|
||||||
|
|
||||||
|
// Set flag
|
||||||
|
configUpdateRequired = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (configUpdateRequired) {
|
||||||
|
// Update required, decrement retry count
|
||||||
|
trampRetryCount--;
|
||||||
|
|
||||||
|
// Update last time
|
||||||
|
trampLastTimeUs = currentTimeUs;
|
||||||
|
|
||||||
|
// Advance state
|
||||||
|
trampStatus = TRAMP_STATUS_ONLINE_CONFIG;
|
||||||
} else {
|
} else {
|
||||||
trampQueryS();
|
// No update required, reset retry count
|
||||||
|
trampRetryCount = TRAMP_MAX_RETRIES;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
lastQueryTimeUs = currentTimeUs;
|
/* Was a config update made? */
|
||||||
}
|
if (!configUpdateRequired) {
|
||||||
break;
|
/* No, look to continue monitoring */
|
||||||
|
if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_STATUS_REQUEST_PERIOD_US) {
|
||||||
|
// Request period exceeded, issue freq/power/pit query
|
||||||
|
trampQuery('v');
|
||||||
|
|
||||||
case TRAMP_STATUS_SET_FREQ_PW:
|
// Update last time
|
||||||
|
trampLastTimeUs = currentTimeUs;
|
||||||
|
} else if (replyCode == 'v') {
|
||||||
|
// Got reply, issue temp query
|
||||||
|
trampQuery('s');
|
||||||
|
|
||||||
|
// Wait for reply
|
||||||
|
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_TEMP;
|
||||||
|
|
||||||
|
// Update last time
|
||||||
|
trampLastTimeUs = currentTimeUs;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case TRAMP_STATUS_ONLINE_MONITOR_TEMP:
|
||||||
{
|
{
|
||||||
bool done = true;
|
// Check request time
|
||||||
if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampCurFreq)) {
|
if (replyCode == 's') {
|
||||||
trampSendFreq(trampConfFreq);
|
// Got reply, return to request freq/power/pit
|
||||||
trampFreqRetries--;
|
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_TEMP;
|
||||||
#ifdef TRAMP_DEBUG
|
} else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
|
||||||
debugFreqReqCounter++;
|
// Timed out after min request period, return to request freq/power/pit query
|
||||||
#endif
|
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
|
||||||
done = false;
|
|
||||||
} else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
|
|
||||||
trampSendRFPower(trampConfPower);
|
|
||||||
trampPowerRetries--;
|
|
||||||
#ifdef TRAMP_DEBUG
|
|
||||||
debugPowReqCounter++;
|
|
||||||
#endif
|
|
||||||
done = false;
|
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case TRAMP_STATUS_ONLINE_CONFIG:
|
||||||
|
{
|
||||||
|
// Param should now be set, check time
|
||||||
|
if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
|
||||||
|
// Min request period exceeded, re-query
|
||||||
|
trampQuery('v');
|
||||||
|
|
||||||
if (!done) {
|
// Advance state
|
||||||
trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
|
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
|
||||||
|
|
||||||
// delay next status query by 300ms
|
// Update last time
|
||||||
lastQueryTimeUs = currentTimeUs + 300 * 1000;
|
trampLastTimeUs = currentTimeUs;
|
||||||
} else {
|
|
||||||
// everything has been done, let's return to original state
|
|
||||||
trampStatus = TRAMP_STATUS_ONLINE;
|
|
||||||
// reset configuration value in case it failed (no more retries)
|
|
||||||
trampConfFreq = trampCurFreq;
|
|
||||||
trampConfPower = trampConfiguredPower;
|
|
||||||
trampFreqRetries = trampPowerRetries = 0;
|
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
|
|
||||||
case TRAMP_STATUS_CHECK_FREQ_PW:
|
|
||||||
if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
|
|
||||||
trampQueryV();
|
|
||||||
lastQueryTimeUs = currentTimeUs;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
{
|
||||||
|
// Invalid state, reset
|
||||||
|
trampStatus = TRAMP_STATUS_OFFLINE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef TRAMP_DEBUG
|
DEBUG_SET(DEBUG_VTX_TRAMP, 0, trampStatus);
|
||||||
debug[1] = debugFreqReqCounter;
|
|
||||||
debug[2] = debugPowReqCounter;
|
|
||||||
debug[3] = 0;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_CMS
|
#ifdef USE_CMS
|
||||||
trampCmsUpdateStatusString();
|
trampCmsUpdateStatusString();
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#ifdef USE_VTX_COMMON
|
#ifdef USE_VTX_COMMON
|
||||||
|
|
||||||
// Interface to common VTX API
|
// Interface to common VTX API
|
||||||
|
@ -507,7 +481,7 @@ static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
|
||||||
|
|
||||||
static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
|
static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
|
||||||
{
|
{
|
||||||
return vtxDevice != NULL && trampStatus > TRAMP_STATUS_OFFLINE;
|
return vtxDevice != NULL && trampStatus >= TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
|
static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
|
||||||
|
@ -520,25 +494,54 @@ static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint
|
||||||
|
|
||||||
static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
|
static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
|
||||||
{
|
{
|
||||||
uint16_t powerValue = 0;
|
uint16_t powerValue;
|
||||||
|
|
||||||
|
// Lookup power level value
|
||||||
if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) {
|
if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) {
|
||||||
trampSetRFPower(powerValue);
|
// Value found, apply
|
||||||
trampCommitChanges();
|
trampConfPower = powerValue;
|
||||||
|
if (trampConfPower != trampLastConfPower) {
|
||||||
|
// Requested power changed, reset retry count
|
||||||
|
trampRetryCount = TRAMP_MAX_RETRIES;
|
||||||
|
trampLastConfPower = trampConfPower;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
|
static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
|
||||||
{
|
{
|
||||||
UNUSED(vtxDevice);
|
UNUSED(vtxDevice);
|
||||||
trampSetPitMode(onoff);
|
|
||||||
|
trampConfPitMode = onoff ? 0 : 1; // note inverted values
|
||||||
|
if (trampConfPitMode != trampLastConfPitMode) {
|
||||||
|
// Requested pitmode changed, reset retry count
|
||||||
|
trampRetryCount = TRAMP_MAX_RETRIES;
|
||||||
|
trampLastConfPitMode = trampConfPitMode;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
|
static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
|
||||||
{
|
{
|
||||||
UNUSED(vtxDevice);
|
UNUSED(vtxDevice);
|
||||||
if (trampValidateFreq(freq)) {
|
|
||||||
trampSetFreq(freq);
|
uint8_t freqValid;
|
||||||
trampCommitChanges();
|
|
||||||
|
// Check frequency valid
|
||||||
|
if (trampRFFreqMin != 0 && trampRFFreqMax != 0) {
|
||||||
|
freqValid = (freq >= trampRFFreqMin && freq <= trampRFFreqMax);
|
||||||
|
} else {
|
||||||
|
freqValid = (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Is frequency valid?
|
||||||
|
if (freqValid) {
|
||||||
|
// Yes, set freq
|
||||||
|
trampConfFreq = freq;
|
||||||
|
if (trampConfFreq != trampLastConfFreq) {
|
||||||
|
// Requested freq changed, reset retry count
|
||||||
|
trampRetryCount = TRAMP_MAX_RETRIES;
|
||||||
|
trampLastConfFreq = trampConfFreq;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -560,16 +563,24 @@ static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (trampConfiguredPower > 0) {
|
// Special case, power not set
|
||||||
for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
|
if (trampConfPower == 0) {
|
||||||
if (trampConfiguredPower <= vtxTablePowerValues[i]) {
|
*pIndex = 0;
|
||||||
*pIndex = i + 1;
|
return true;
|
||||||
break;
|
}
|
||||||
}
|
|
||||||
|
// Lookup value in table
|
||||||
|
for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
|
||||||
|
// Find value that matches current configured power level
|
||||||
|
if (trampConfPower == vtxTablePowerValues[i]) {
|
||||||
|
// Value found, return index
|
||||||
|
*pIndex = i + 1;
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
// Value not found in table
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
|
static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
|
||||||
|
@ -588,8 +599,14 @@ static bool vtxTrampGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
*status = (trampPitMode ? VTX_STATUS_PIT_MODE : 0)
|
// Mirror configued pit mode state rather than use current pitmode as we
|
||||||
| ((trampControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0);
|
// should, otherwise the logic in vtxProcessPitMode may not get us to the
|
||||||
|
// correct state if pitmode is toggled quickly
|
||||||
|
*status = (trampConfPitMode ? 0 : VTX_STATUS_PIT_MODE);
|
||||||
|
|
||||||
|
// Check VTX is not locked
|
||||||
|
*status |= ((trampCurControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -650,7 +667,6 @@ bool vtxTrampInit(void)
|
||||||
for (int i = 0; i < VTX_TRAMP_POWER_COUNT; i++) {
|
for (int i = 0; i < VTX_TRAMP_POWER_COUNT; i++) {
|
||||||
vtxTablePowerValues[i] = trampPowerTable[i];
|
vtxTablePowerValues[i] = trampPowerTable[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@ -660,4 +676,14 @@ bool vtxTrampInit(void)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint16_t vtxTrampGetCurrentActualPower()
|
||||||
|
{
|
||||||
|
return trampCurActPower;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t vtxTrampGetCurrentTemp()
|
||||||
|
{
|
||||||
|
return trampCurTemp;
|
||||||
|
}
|
||||||
|
|
||||||
#endif // VTX_TRAMP
|
#endif // VTX_TRAMP
|
||||||
|
|
|
@ -27,16 +27,7 @@
|
||||||
#define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000 //min freq in MHz
|
#define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000 //min freq in MHz
|
||||||
#define VTX_TRAMP_MAX_FREQUENCY_MHZ 5999 //max freq in MHz
|
#define VTX_TRAMP_MAX_FREQUENCY_MHZ 5999 //max freq in MHz
|
||||||
|
|
||||||
extern uint8_t trampBand;
|
|
||||||
extern uint8_t trampChannel;
|
|
||||||
extern uint16_t trampPower; // Actual transmitting power
|
|
||||||
extern uint8_t trampPitMode;
|
|
||||||
extern uint32_t trampCurFreq;
|
|
||||||
extern uint16_t trampConfiguredPower; // Configured transmitting power
|
|
||||||
extern int16_t trampTemperature;
|
|
||||||
|
|
||||||
bool vtxTrampInit(void);
|
bool vtxTrampInit(void);
|
||||||
bool trampCommitChanges(void);
|
|
||||||
void trampSetPitMode(uint8_t onoff);
|
uint16_t vtxTrampGetCurrentActualPower();
|
||||||
void trampSetBandAndChannel(uint8_t band, uint8_t channel);
|
uint16_t vtxTrampGetCurrentTemp();
|
||||||
void trampSetRFPower(uint16_t level);
|
|
||||||
|
|
Loading…
Reference in New Issue