rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #9638 from pgreenland/vtx_irc_tramp_improvements

This commit is contained in:
Michael Keller 2020-07-26 12:57:32 +12:00 committed by GitHub
parent 899cae197c 367cc3a513
commit 0e4a54fc81
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 366 additions and 331 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/build/debug.c
View File

@ -97,4 +97,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"GYRO_SAMPLE",
"RX_TIMING",
"D_LPF",
"VTX_TRAMP",
};

1
src/main/build/debug.h
View File

@ -113,6 +113,7 @@ typedef enum {
DEBUG_GYRO_SAMPLE,
DEBUG_RX_TIMING,
DEBUG_D_LPF,
DEBUG_VTX_TRAMP,
DEBUG_COUNT
} debugType_e;

60
src/main/cms/cms_menu_vtx_tramp.c
View File

@ -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;

620
src/main/io/vtx_tramp.c
View File

@ -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

15
src/main/io/vtx_tramp.h
View File

@ -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();