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",
|
||||
"RX_TIMING",
|
||||
"D_LPF",
|
||||
"VTX_TRAMP",
|
||||
};
|
||||
|
|
|
@ -113,6 +113,7 @@ typedef enum {
|
|||
DEBUG_GYRO_SAMPLE,
|
||||
DEBUG_RX_TIMING,
|
||||
DEBUG_D_LPF,
|
||||
DEBUG_VTX_TRAMP,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
@ -45,6 +45,9 @@ char trampCmsStatusString[31] = "- -- ---- ----";
|
|||
// m bc ffff tppp
|
||||
// 01234567890123
|
||||
|
||||
static int16_t trampCmsTemp;
|
||||
static OSD_INT16_t trampCmsEntTemp = { &trampCmsTemp, -100, 300, 0 };
|
||||
|
||||
void trampCmsUpdateStatusString(void)
|
||||
{
|
||||
vtxDevice_t *vtxDevice = vtxCommonDevice();
|
||||
|
@ -67,16 +70,23 @@ void trampCmsUpdateStatusString(void)
|
|||
}
|
||||
trampCmsStatusString[4] = ' ';
|
||||
|
||||
if (trampCurFreq)
|
||||
tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq);
|
||||
else
|
||||
uint16_t freq;
|
||||
if (!vtxCommonGetFrequency(vtxDevice, &freq) || (freq == 0)) {
|
||||
tfp_sprintf(&trampCmsStatusString[5], "----");
|
||||
} else {
|
||||
tfp_sprintf(&trampCmsStatusString[5], "%4d", freq);
|
||||
}
|
||||
|
||||
if (trampPower) {
|
||||
tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampPower == trampConfiguredPower) ? ' ' : '*', trampPower);
|
||||
uint16_t actualPower = vtxTrampGetCurrentActualPower();
|
||||
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 {
|
||||
tfp_sprintf(&trampCmsStatusString[9], " ----");
|
||||
}
|
||||
|
||||
trampCmsTemp = vtxTrampGetCurrentTemp();
|
||||
}
|
||||
|
||||
uint8_t trampCmsPitMode = 0;
|
||||
|
@ -105,11 +115,12 @@ static const void *trampCmsConfigBand(displayPort_t *pDisp, const void *self)
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
if (trampCmsBand == 0)
|
||||
if (trampCmsBand == 0) {
|
||||
// Bounce back
|
||||
trampCmsBand = 1;
|
||||
else
|
||||
} else {
|
||||
trampCmsUpdateFreqRef();
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
@ -119,11 +130,12 @@ static const void *trampCmsConfigChan(displayPort_t *pDisp, const void *self)
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
if (trampCmsChan == 0)
|
||||
if (trampCmsChan == 0) {
|
||||
// Bounce back
|
||||
trampCmsChan = 1;
|
||||
else
|
||||
} else {
|
||||
trampCmsUpdateFreqRef();
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
@ -133,14 +145,17 @@ static const void *trampCmsConfigPower(displayPort_t *pDisp, const void *self)
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
if (trampCmsPower == 0)
|
||||
if (trampCmsPower == 0) {
|
||||
// Bounce back
|
||||
trampCmsPower = 1;
|
||||
}
|
||||
|
||||
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[] = {
|
||||
"---", "OFF", "ON "
|
||||
|
@ -153,11 +168,12 @@ static const void *trampCmsSetPitMode(displayPort_t *pDisp, const void *self)
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
if (trampCmsPitMode == 0) {
|
||||
if (trampCmsPitMode == TRAMP_PIT_STATUS_NA) {
|
||||
// Bouce back
|
||||
trampCmsPitMode = 1;
|
||||
trampCmsPitMode = TRAMP_PIT_STATUS_OFF;
|
||||
} else {
|
||||
trampSetPitMode(trampCmsPitMode - 1);
|
||||
vtxCommonSetPitMode(vtxCommonDevice(),
|
||||
(trampCmsPitMode == TRAMP_PIT_STATUS_OFF) ? 0 : 1);
|
||||
}
|
||||
|
||||
return NULL;
|
||||
|
@ -172,9 +188,6 @@ static const void *trampCmsCommence(displayPort_t *pDisp, const void *self)
|
|||
vtxCommonSetBandAndChannel(device, trampCmsBand, trampCmsChan);
|
||||
vtxCommonSetPowerByIndex(device, trampCmsPower);
|
||||
|
||||
// If it fails, the user should retry later
|
||||
trampCommitChanges();
|
||||
|
||||
// update'vtx_' settings
|
||||
vtxSettingsConfigMutable()->band = trampCmsBand;
|
||||
vtxSettingsConfigMutable()->channel = trampCmsChan;
|
||||
|
@ -189,6 +202,7 @@ static const void *trampCmsCommence(displayPort_t *pDisp, const void *self)
|
|||
static bool trampCmsInitSettings(void)
|
||||
{
|
||||
vtxDevice_t *device = vtxCommonDevice();
|
||||
unsigned vtxStatus;
|
||||
|
||||
if (!device) {
|
||||
return false;
|
||||
|
@ -197,12 +211,14 @@ static bool trampCmsInitSettings(void)
|
|||
vtxCommonGetBandAndChannel(device, &trampCmsBand, &trampCmsChan);
|
||||
|
||||
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)) {
|
||||
trampCmsPower = 1;
|
||||
}
|
||||
if (!vtxCommonGetPowerIndex(vtxCommonDevice(), &trampCmsPower)) {
|
||||
trampCmsPower = 1;
|
||||
}
|
||||
|
||||
trampCmsEntBand.val = &trampCmsBand;
|
||||
|
|
|
@ -44,6 +44,18 @@
|
|||
#include "io/vtx_control.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)
|
||||
const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
|
||||
25, 100, 200, 400, 600
|
||||
|
@ -61,52 +73,72 @@ static vtxDevice_t vtxTramp = {
|
|||
};
|
||||
#endif
|
||||
|
||||
// Device serial port instance
|
||||
static serialPort_t *trampSerialPort = NULL;
|
||||
|
||||
// Serial transmit and receive buffers
|
||||
static uint8_t trampReqBuffer[16];
|
||||
static uint8_t trampRespBuffer[16];
|
||||
|
||||
// Module state machine
|
||||
typedef enum {
|
||||
TRAMP_STATUS_BAD_DEVICE = -1,
|
||||
// Offline - device hasn't responded yet
|
||||
TRAMP_STATUS_OFFLINE = 0,
|
||||
TRAMP_STATUS_ONLINE,
|
||||
TRAMP_STATUS_SET_FREQ_PW,
|
||||
TRAMP_STATUS_CHECK_FREQ_PW
|
||||
// Init - fetching current settings from device
|
||||
TRAMP_STATUS_INIT,
|
||||
// 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 = 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 trampRFFreqMax = 0;
|
||||
static uint32_t trampRFPowerMax;
|
||||
|
||||
uint32_t trampCurFreq = 0;
|
||||
static uint32_t trampLastFreq = 0;
|
||||
uint16_t trampPower = 0; // Actual transmitting power
|
||||
uint16_t trampConfiguredPower = 0; // Configured transmitting power
|
||||
static uint16_t trampLastPower = 0;
|
||||
int16_t trampTemperature = 0;
|
||||
uint8_t trampPitMode = 0;
|
||||
static uint8_t trampControlMode = 0;
|
||||
// Device status, read from device periodically
|
||||
static uint32_t trampCurFreq = 0;
|
||||
static uint16_t trampCurConfPower = 0; // Configured power
|
||||
static uint16_t trampCurActPower = 0; // Actual power
|
||||
static uint8_t trampCurPitMode = 0;
|
||||
static int16_t trampCurTemp = 0;
|
||||
static uint8_t trampCurControlMode = 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
|
||||
// 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
|
||||
// Last device configuration, last desired state of device - used to reset
|
||||
// retry count
|
||||
static uint32_t trampLastConfFreq = 0;
|
||||
static uint16_t trampLastConfPower = 0;
|
||||
static uint8_t trampLastConfPitMode = 0;
|
||||
|
||||
uint32_t trampConfFreq = 0;
|
||||
uint8_t trampFreqRetries = 0;
|
||||
// Retry count
|
||||
static uint8_t trampRetryCount = TRAMP_MAX_RETRIES;
|
||||
|
||||
uint16_t trampConfPower = 0;
|
||||
uint8_t trampPowerRetries = 0;
|
||||
// Receive state machine
|
||||
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)
|
||||
{
|
||||
serialWriteBuf(trampSerialPort, buf, 16);
|
||||
}
|
||||
static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
|
||||
|
||||
// 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)
|
||||
{
|
||||
uint8_t cksum = 0;
|
||||
|
@ -118,16 +150,13 @@ static uint8_t trampChecksum(uint8_t *trampBuf)
|
|||
return cksum;
|
||||
}
|
||||
|
||||
static bool trampVtxControlEnabled(void)
|
||||
{
|
||||
return !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE);
|
||||
}
|
||||
|
||||
// Check if race lock is enabled
|
||||
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)
|
||||
{
|
||||
if (!trampSerialPort) {
|
||||
|
@ -135,82 +164,25 @@ static void trampSendU16(uint8_t cmd, uint16_t param)
|
|||
}
|
||||
|
||||
memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
|
||||
trampReqBuffer[0] = 15;
|
||||
trampReqBuffer[0] = 0x0F;
|
||||
trampReqBuffer[1] = cmd;
|
||||
trampReqBuffer[2] = param & 0xff;
|
||||
trampReqBuffer[3] = (param >> 8) & 0xff;
|
||||
trampReqBuffer[14] = trampChecksum(trampReqBuffer);
|
||||
trampWriteBuf(trampReqBuffer);
|
||||
serialWriteBuf(trampSerialPort, trampReqBuffer, 16);
|
||||
}
|
||||
|
||||
// Send command to device
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
static bool trampValidateFreq(uint16_t freq)
|
||||
{
|
||||
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
|
||||
// Process response and return code if valid else 0
|
||||
static char trampHandleResponse(void)
|
||||
{
|
||||
const uint8_t respCode = trampRespBuffer[1];
|
||||
|
@ -219,95 +191,66 @@ static char trampHandleResponse(void)
|
|||
case 'r':
|
||||
{
|
||||
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) {
|
||||
// Got response, update device limits
|
||||
trampRFFreqMin = min_freq;
|
||||
trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
||||
trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
|
||||
return 'r';
|
||||
}
|
||||
|
||||
// throw bytes echoed from tx to rx in bidirectional mode away
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'v':
|
||||
{
|
||||
const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
||||
// Check we're not reading the request (indicated by freq zero)
|
||||
if (freq != 0) {
|
||||
// Got response, update device status
|
||||
trampCurFreq = freq;
|
||||
if (trampLastFreq != trampCurFreq) {
|
||||
trampFreqRetries = TRAMP_MAX_RETRIES;
|
||||
}
|
||||
trampLastFreq = trampCurFreq;
|
||||
|
||||
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);
|
||||
trampCurConfPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
||||
trampCurControlMode = trampRespBuffer[6]; // Currently only used for race lock
|
||||
trampCurPitMode = trampRespBuffer[7];
|
||||
trampCurActPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
|
||||
|
||||
// Init config with current status if not set
|
||||
if (trampConfFreq == 0) {
|
||||
trampConfFreq = trampCurFreq;
|
||||
}
|
||||
if (trampConfPower == 0) {
|
||||
trampConfPower = trampConfiguredPower;
|
||||
trampConfPower = trampCurConfPower;
|
||||
}
|
||||
|
||||
return 'v';
|
||||
}
|
||||
|
||||
// throw bytes echoed from tx to rx in bidirectional mode away
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 's':
|
||||
{
|
||||
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) {
|
||||
trampTemperature = temp;
|
||||
// Got response, update device status
|
||||
trampCurTemp = temp;
|
||||
return 's';
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// Likely reading a request, return zero to indicate not accepted
|
||||
return 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 trampReceiveState_e trampReceiveState = S_WAIT_LEN;
|
||||
static int trampReceivePos = 0;
|
||||
|
||||
// Reset receiver state machine
|
||||
static void trampResetReceiver(void)
|
||||
{
|
||||
trampReceiveState = S_WAIT_LEN;
|
||||
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
|
||||
static char trampReceive(uint32_t currentTimeUs)
|
||||
static char trampReceive()
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
if (!trampSerialPort) {
|
||||
return 0;
|
||||
}
|
||||
|
@ -318,183 +261,214 @@ static char trampReceive(uint32_t currentTimeUs)
|
|||
|
||||
switch (trampReceiveState) {
|
||||
case S_WAIT_LEN:
|
||||
if (c == 0x0F) {
|
||||
trampReceiveState = S_WAIT_CODE;
|
||||
} else {
|
||||
trampReceivePos = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
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();
|
||||
{
|
||||
if (c == 0x0F) {
|
||||
// Found header byte, advance to wait for code
|
||||
trampReceiveState = S_WAIT_CODE;
|
||||
} else {
|
||||
// Unexpected header, reset state machine
|
||||
trampResetReceiver();
|
||||
}
|
||||
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:
|
||||
trampResetReceiver();
|
||||
break;
|
||||
{
|
||||
// Invalid state, reset state machine
|
||||
trampResetReceiver();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void trampQuery(uint8_t cmd)
|
||||
static void trampQuery(uint8_t cmd)
|
||||
{
|
||||
// Reset receive buffer and issue command
|
||||
trampResetReceiver();
|
||||
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)
|
||||
{
|
||||
UNUSED(vtxDevice);
|
||||
|
||||
static timeUs_t lastQueryTimeUs = 0;
|
||||
static bool initSettingsDoneFlag = false;
|
||||
|
||||
#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) {
|
||||
// Read response from device
|
||||
const char replyCode = trampReceive();
|
||||
|
||||
// Act on state
|
||||
switch(trampStatus) {
|
||||
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) {
|
||||
trampQueryR();
|
||||
} else {
|
||||
static unsigned int cnt = 0;
|
||||
if (((cnt++) & 1) == 0) {
|
||||
trampQueryV();
|
||||
// Update last time
|
||||
trampLastTimeUs = currentTimeUs;
|
||||
}
|
||||
break;
|
||||
}
|
||||
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 {
|
||||
trampQueryS();
|
||||
// No update required, reset retry count
|
||||
trampRetryCount = TRAMP_MAX_RETRIES;
|
||||
}
|
||||
}
|
||||
|
||||
lastQueryTimeUs = currentTimeUs;
|
||||
}
|
||||
break;
|
||||
/* Was a config update made? */
|
||||
if (!configUpdateRequired) {
|
||||
/* 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;
|
||||
if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampCurFreq)) {
|
||||
trampSendFreq(trampConfFreq);
|
||||
trampFreqRetries--;
|
||||
#ifdef TRAMP_DEBUG
|
||||
debugFreqReqCounter++;
|
||||
#endif
|
||||
done = false;
|
||||
} else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
|
||||
trampSendRFPower(trampConfPower);
|
||||
trampPowerRetries--;
|
||||
#ifdef TRAMP_DEBUG
|
||||
debugPowReqCounter++;
|
||||
#endif
|
||||
done = false;
|
||||
// Check request time
|
||||
if (replyCode == 's') {
|
||||
// Got reply, return to request freq/power/pit
|
||||
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_TEMP;
|
||||
} else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
|
||||
// Timed out after min request period, return to request freq/power/pit query
|
||||
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
|
||||
}
|
||||
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) {
|
||||
trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
|
||||
// Advance state
|
||||
trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
|
||||
|
||||
// delay next status query by 300ms
|
||||
lastQueryTimeUs = currentTimeUs + 300 * 1000;
|
||||
} 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;
|
||||
// Update last time
|
||||
trampLastTimeUs = currentTimeUs;
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case TRAMP_STATUS_CHECK_FREQ_PW:
|
||||
if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
|
||||
trampQueryV();
|
||||
lastQueryTimeUs = currentTimeUs;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
{
|
||||
// Invalid state, reset
|
||||
trampStatus = TRAMP_STATUS_OFFLINE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef TRAMP_DEBUG
|
||||
debug[1] = debugFreqReqCounter;
|
||||
debug[2] = debugPowReqCounter;
|
||||
debug[3] = 0;
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_VTX_TRAMP, 0, trampStatus);
|
||||
|
||||
#ifdef USE_CMS
|
||||
trampCmsUpdateStatusString();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#ifdef USE_VTX_COMMON
|
||||
|
||||
// Interface to common VTX API
|
||||
|
@ -507,7 +481,7 @@ static vtxDevType_e vtxTrampGetDeviceType(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)
|
||||
|
@ -520,25 +494,54 @@ static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint
|
|||
|
||||
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)) {
|
||||
trampSetRFPower(powerValue);
|
||||
trampCommitChanges();
|
||||
// Value found, apply
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
UNUSED(vtxDevice);
|
||||
if (trampValidateFreq(freq)) {
|
||||
trampSetFreq(freq);
|
||||
trampCommitChanges();
|
||||
|
||||
uint8_t freqValid;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
if (trampConfiguredPower > 0) {
|
||||
for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
|
||||
if (trampConfiguredPower <= vtxTablePowerValues[i]) {
|
||||
*pIndex = i + 1;
|
||||
break;
|
||||
}
|
||||
// Special case, power not set
|
||||
if (trampConfPower == 0) {
|
||||
*pIndex = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
// 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)
|
||||
|
@ -588,8 +599,14 @@ static bool vtxTrampGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
|
|||
return false;
|
||||
}
|
||||
|
||||
*status = (trampPitMode ? VTX_STATUS_PIT_MODE : 0)
|
||||
| ((trampControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0);
|
||||
// Mirror configued pit mode state rather than use current pitmode as we
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
@ -650,7 +667,6 @@ bool vtxTrampInit(void)
|
|||
for (int i = 0; i < VTX_TRAMP_POWER_COUNT; i++) {
|
||||
vtxTablePowerValues[i] = trampPowerTable[i];
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
@ -660,4 +676,14 @@ bool vtxTrampInit(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
uint16_t vtxTrampGetCurrentActualPower()
|
||||
{
|
||||
return trampCurActPower;
|
||||
}
|
||||
|
||||
uint16_t vtxTrampGetCurrentTemp()
|
||||
{
|
||||
return trampCurTemp;
|
||||
}
|
||||
|
||||
#endif // VTX_TRAMP
|
||||
|
|
|
@ -27,16 +27,7 @@
|
|||
#define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000 //min 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 trampCommitChanges(void);
|
||||
void trampSetPitMode(uint8_t onoff);
|
||||
void trampSetBandAndChannel(uint8_t band, uint8_t channel);
|
||||
void trampSetRFPower(uint16_t level);
|
||||
|
||||
uint16_t vtxTrampGetCurrentActualPower();
|
||||
uint16_t vtxTrampGetCurrentTemp();
|
||||
|
|
Loading…
Reference in New Issue