mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 01:35:41 +03:00
732 lines
18 KiB
C
732 lines
18 KiB
C
/*
|
|
* This file is part of Cleanflight.
|
|
*
|
|
* Cleanflight is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Cleanflight is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
/* Created by jflyper */
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <ctype.h>
|
|
#include <string.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#if defined(VTX_TRAMP) && defined(VTX_CONTROL)
|
|
|
|
#include "build/debug.h"
|
|
|
|
#include "common/utils.h"
|
|
#include "common/printf.h"
|
|
|
|
#include "io/serial.h"
|
|
#include "drivers/serial.h"
|
|
#include "drivers/vtx_common.h"
|
|
#include "io/vtx_tramp.h"
|
|
#include "io/vtx_string.h"
|
|
|
|
#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR)
|
|
|
|
#if defined(CMS) || defined(VTX_COMMON)
|
|
static const uint16_t trampPowerTable[] = {
|
|
25, 100, 200, 400, 600
|
|
};
|
|
|
|
static const char * const trampPowerNames[] = {
|
|
"---", "25 ", "100", "200", "400", "600"
|
|
};
|
|
#endif
|
|
|
|
#if defined(VTX_COMMON)
|
|
static const vtxVTable_t trampVTable; // forward
|
|
static vtxDevice_t vtxTramp = {
|
|
.vTable = &trampVTable,
|
|
.capability.bandCount = 5,
|
|
.capability.channelCount = 8,
|
|
.capability.powerCount = sizeof(trampPowerTable),
|
|
.bandNames = (char **)vtx58BandNames,
|
|
.channelNames = (char **)vtx58ChannelNames,
|
|
.powerNames = (char **)trampPowerNames,
|
|
};
|
|
#endif
|
|
|
|
static serialPort_t *trampSerialPort = NULL;
|
|
|
|
static uint8_t trampReqBuffer[16];
|
|
static uint8_t trampRespBuffer[16];
|
|
|
|
typedef enum {
|
|
TRAMP_STATUS_BAD_DEVICE = -1,
|
|
TRAMP_STATUS_OFFLINE = 0,
|
|
TRAMP_STATUS_ONLINE,
|
|
TRAMP_STATUS_SET_FREQ_PW,
|
|
TRAMP_STATUS_CHECK_FREQ_PW
|
|
} trampStatus_e;
|
|
|
|
trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
|
|
|
|
uint32_t trampRFFreqMin;
|
|
uint32_t trampRFFreqMax;
|
|
uint32_t trampRFPowerMax;
|
|
|
|
uint32_t trampCurFreq = 0;
|
|
uint8_t trampBand = 0;
|
|
uint8_t trampChannel = 0;
|
|
uint16_t trampPower = 0; // Actual transmitting power
|
|
uint16_t trampConfiguredPower = 0; // Configured transmitting power
|
|
int16_t trampTemperature = 0;
|
|
uint8_t trampPitMode = 0;
|
|
|
|
// Maximum number of requests sent to try a config change
|
|
#define TRAMP_MAX_RETRIES 2
|
|
|
|
uint32_t trampConfFreq = 0;
|
|
uint8_t trampFreqRetries = 0;
|
|
|
|
uint16_t trampConfPower = 0;
|
|
uint8_t trampPowerRetries = 0;
|
|
|
|
#ifdef CMS
|
|
static void trampCmsUpdateStatusString(void); // Forward
|
|
#endif
|
|
|
|
static void trampWriteBuf(uint8_t *buf)
|
|
{
|
|
serialWriteBuf(trampSerialPort, buf, 16);
|
|
}
|
|
|
|
static uint8_t trampChecksum(uint8_t *trampBuf)
|
|
{
|
|
uint8_t cksum = 0;
|
|
|
|
for (int i = 1 ; i < 14 ; i++)
|
|
cksum += trampBuf[i];
|
|
|
|
return cksum;
|
|
}
|
|
|
|
void trampCmdU16(uint8_t cmd, uint16_t param)
|
|
{
|
|
if (!trampSerialPort)
|
|
return;
|
|
|
|
memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
|
|
trampReqBuffer[0] = 15;
|
|
trampReqBuffer[1] = cmd;
|
|
trampReqBuffer[2] = param & 0xff;
|
|
trampReqBuffer[3] = (param >> 8) & 0xff;
|
|
trampReqBuffer[14] = trampChecksum(trampReqBuffer);
|
|
trampWriteBuf(trampReqBuffer);
|
|
}
|
|
|
|
void trampSetFreq(uint16_t freq)
|
|
{
|
|
trampConfFreq = freq;
|
|
if(trampConfFreq != trampCurFreq)
|
|
trampFreqRetries = TRAMP_MAX_RETRIES;
|
|
}
|
|
|
|
void trampSendFreq(uint16_t freq)
|
|
{
|
|
trampCmdU16('F', freq);
|
|
}
|
|
|
|
void trampSetBandAndChannel(uint8_t band, uint8_t channel)
|
|
{
|
|
trampSetFreq(vtx58frequencyTable[band - 1][channel - 1]);
|
|
}
|
|
|
|
void trampSetRFPower(uint16_t level)
|
|
{
|
|
trampConfPower = level;
|
|
if(trampConfPower != trampPower)
|
|
trampPowerRetries = TRAMP_MAX_RETRIES;
|
|
}
|
|
|
|
void trampSendRFPower(uint16_t level)
|
|
{
|
|
trampCmdU16('P', level);
|
|
}
|
|
|
|
// return false if error
|
|
bool trampCommitChanges()
|
|
{
|
|
if(trampStatus != TRAMP_STATUS_ONLINE)
|
|
return false;
|
|
|
|
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
|
return true;
|
|
}
|
|
|
|
void trampSetPitMode(uint8_t onoff)
|
|
{
|
|
trampCmdU16('I', onoff ? 0 : 1);
|
|
}
|
|
|
|
// returns completed response code
|
|
char trampHandleResponse(void)
|
|
{
|
|
uint8_t respCode = trampRespBuffer[1];
|
|
|
|
switch (respCode) {
|
|
case 'r':
|
|
{
|
|
uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
|
if(min_freq != 0) {
|
|
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;
|
|
|
|
case 'v':
|
|
{
|
|
uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
|
if(freq != 0) {
|
|
trampCurFreq = freq;
|
|
trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
|
trampPitMode = trampRespBuffer[7];
|
|
trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
|
|
vtx58_Freq2Bandchan(trampCurFreq, &trampBand, &trampChannel);
|
|
|
|
if(trampConfFreq == 0) trampConfFreq = trampCurFreq;
|
|
if(trampConfPower == 0) trampConfPower = trampPower;
|
|
return 'v';
|
|
}
|
|
|
|
// throw bytes echoed from tx to rx in bidirectional mode away
|
|
}
|
|
break;
|
|
|
|
case 's':
|
|
{
|
|
uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
|
|
if(temp != 0) {
|
|
trampTemperature = temp;
|
|
return 's';
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
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;
|
|
|
|
static void trampResetReceiver()
|
|
{
|
|
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)
|
|
{
|
|
UNUSED(currentTimeUs);
|
|
|
|
if (!trampSerialPort)
|
|
return 0;
|
|
|
|
while (serialRxBytesWaiting(trampSerialPort)) {
|
|
uint8_t c = serialRead(trampSerialPort);
|
|
trampRespBuffer[trampReceivePos++] = c;
|
|
|
|
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();
|
|
}
|
|
}
|
|
break;
|
|
|
|
default:
|
|
trampResetReceiver();
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void trampQuery(uint8_t cmd)
|
|
{
|
|
trampResetReceiver();
|
|
trampCmdU16(cmd, 0);
|
|
}
|
|
|
|
void trampQueryR(void)
|
|
{
|
|
trampQuery('r');
|
|
}
|
|
|
|
void trampQueryV(void)
|
|
{
|
|
trampQuery('v');
|
|
}
|
|
|
|
void trampQueryS(void)
|
|
{
|
|
trampQuery('s');
|
|
}
|
|
|
|
void vtxTrampProcess(uint32_t currentTimeUs)
|
|
{
|
|
static uint32_t lastQueryTimeUs = 0;
|
|
|
|
#ifdef TRAMP_DEBUG
|
|
static uint16_t debugFreqReqCounter = 0;
|
|
static uint16_t debugPowReqCounter = 0;
|
|
#endif
|
|
|
|
if (trampStatus == TRAMP_STATUS_BAD_DEVICE)
|
|
return;
|
|
|
|
char replyCode = trampReceive(currentTimeUs);
|
|
|
|
#ifdef TRAMP_DEBUG
|
|
debug[0] = trampStatus;
|
|
#endif
|
|
|
|
switch(replyCode) {
|
|
case 'r':
|
|
if (trampStatus <= TRAMP_STATUS_OFFLINE)
|
|
trampStatus = TRAMP_STATUS_ONLINE;
|
|
break;
|
|
|
|
case 'v':
|
|
if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW)
|
|
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
|
break;
|
|
}
|
|
|
|
switch(trampStatus) {
|
|
|
|
case TRAMP_STATUS_OFFLINE:
|
|
case TRAMP_STATUS_ONLINE:
|
|
if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
|
|
|
|
if (trampStatus == TRAMP_STATUS_OFFLINE)
|
|
trampQueryR();
|
|
else {
|
|
static unsigned int cnt = 0;
|
|
if(((cnt++) & 1) == 0)
|
|
trampQueryV();
|
|
else
|
|
trampQueryS();
|
|
}
|
|
|
|
lastQueryTimeUs = currentTimeUs;
|
|
}
|
|
break;
|
|
|
|
case TRAMP_STATUS_SET_FREQ_PW:
|
|
{
|
|
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;
|
|
}
|
|
|
|
if(!done) {
|
|
trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
|
|
|
|
// 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 = trampPower;
|
|
trampFreqRetries = trampPowerRetries = 0;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case TRAMP_STATUS_CHECK_FREQ_PW:
|
|
if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
|
|
trampQueryV();
|
|
lastQueryTimeUs = currentTimeUs;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
#ifdef TRAMP_DEBUG
|
|
debug[1] = debugFreqReqCounter;
|
|
debug[2] = debugPowReqCounter;
|
|
debug[3] = 0;
|
|
#endif
|
|
|
|
#ifdef CMS
|
|
trampCmsUpdateStatusString();
|
|
#endif
|
|
}
|
|
|
|
#ifdef CMS
|
|
#include "cms/cms.h"
|
|
#include "cms/cms_types.h"
|
|
|
|
|
|
char trampCmsStatusString[31] = "- -- ---- ----";
|
|
// m bc ffff tppp
|
|
// 01234567890123
|
|
|
|
static void trampCmsUpdateStatusString(void)
|
|
{
|
|
trampCmsStatusString[0] = '*';
|
|
trampCmsStatusString[1] = ' ';
|
|
trampCmsStatusString[2] = vtx58BandLetter[trampBand];
|
|
trampCmsStatusString[3] = vtx58ChannelNames[trampChannel][0];
|
|
trampCmsStatusString[4] = ' ';
|
|
|
|
if (trampCurFreq)
|
|
tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq);
|
|
else
|
|
tfp_sprintf(&trampCmsStatusString[5], "----");
|
|
|
|
if (trampPower) {
|
|
tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampPower == trampConfiguredPower) ? ' ' : '*', trampPower);
|
|
}
|
|
else
|
|
tfp_sprintf(&trampCmsStatusString[9], " ----");
|
|
}
|
|
|
|
uint8_t trampCmsPitMode = 0;
|
|
uint8_t trampCmsBand = 1;
|
|
uint8_t trampCmsChan = 1;
|
|
uint16_t trampCmsFreqRef;
|
|
|
|
static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames };
|
|
|
|
static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames };
|
|
|
|
static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
|
|
|
|
static uint8_t trampCmsPower = 1;
|
|
|
|
static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames };
|
|
|
|
static void trampCmsUpdateFreqRef(void)
|
|
{
|
|
if (trampCmsBand > 0 && trampCmsChan > 0)
|
|
trampCmsFreqRef = vtx58frequencyTable[trampCmsBand - 1][trampCmsChan - 1];
|
|
}
|
|
|
|
static long trampCmsConfigBand(displayPort_t *pDisp, const void *self)
|
|
{
|
|
UNUSED(pDisp);
|
|
UNUSED(self);
|
|
|
|
if (trampCmsBand == 0)
|
|
// Bounce back
|
|
trampCmsBand = 1;
|
|
else
|
|
trampCmsUpdateFreqRef();
|
|
|
|
return 0;
|
|
}
|
|
|
|
static long trampCmsConfigChan(displayPort_t *pDisp, const void *self)
|
|
{
|
|
UNUSED(pDisp);
|
|
UNUSED(self);
|
|
|
|
if (trampCmsChan == 0)
|
|
// Bounce back
|
|
trampCmsChan = 1;
|
|
else
|
|
trampCmsUpdateFreqRef();
|
|
|
|
return 0;
|
|
}
|
|
|
|
static long trampCmsConfigPower(displayPort_t *pDisp, const void *self)
|
|
{
|
|
UNUSED(pDisp);
|
|
UNUSED(self);
|
|
|
|
if (trampCmsPower == 0)
|
|
// Bounce back
|
|
trampCmsPower = 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static OSD_INT16_t trampCmsEntTemp = { &trampTemperature, -100, 300, 0 };
|
|
|
|
static const char * const trampCmsPitModeNames[] = {
|
|
"---", "OFF", "ON "
|
|
};
|
|
|
|
static OSD_TAB_t trampCmsEntPitMode = { &trampCmsPitMode, 2, trampCmsPitModeNames };
|
|
|
|
static long trampCmsSetPitMode(displayPort_t *pDisp, const void *self)
|
|
{
|
|
UNUSED(pDisp);
|
|
UNUSED(self);
|
|
|
|
if (trampCmsPitMode == 0) {
|
|
// Bouce back
|
|
trampCmsPitMode = 1;
|
|
} else {
|
|
trampSetPitMode(trampCmsPitMode - 1);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static long trampCmsCommence(displayPort_t *pDisp, const void *self)
|
|
{
|
|
UNUSED(pDisp);
|
|
UNUSED(self);
|
|
|
|
trampSetBandAndChannel(trampCmsBand, trampCmsChan);
|
|
trampSetRFPower(trampPowerTable[trampCmsPower-1]);
|
|
|
|
// If it fails, the user should retry later
|
|
trampCommitChanges();
|
|
|
|
|
|
return MENU_CHAIN_BACK;
|
|
}
|
|
|
|
static void trampCmsInitSettings()
|
|
{
|
|
if(trampBand > 0) trampCmsBand = trampBand;
|
|
if(trampChannel > 0) trampCmsChan = trampChannel;
|
|
|
|
trampCmsUpdateFreqRef();
|
|
trampCmsPitMode = trampPitMode + 1;
|
|
|
|
if (trampConfiguredPower > 0) {
|
|
for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
|
|
if (trampConfiguredPower <= trampPowerTable[i]) {
|
|
trampCmsPower = i + 1;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static long trampCmsOnEnter()
|
|
{
|
|
trampCmsInitSettings();
|
|
return 0;
|
|
}
|
|
|
|
static OSD_Entry trampCmsMenuCommenceEntries[] = {
|
|
{ "CONFIRM", OME_Label, NULL, NULL, 0 },
|
|
{ "YES", OME_Funcall, trampCmsCommence, NULL, 0 },
|
|
{ "BACK", OME_Back, NULL, NULL, 0 },
|
|
{ NULL, OME_END, NULL, NULL, 0 }
|
|
};
|
|
|
|
static CMS_Menu trampCmsMenuCommence = {
|
|
.GUARD_text = "XVTXTRC",
|
|
.GUARD_type = OME_MENU,
|
|
.onEnter = NULL,
|
|
.onExit = NULL,
|
|
.onGlobalExit = NULL,
|
|
.entries = trampCmsMenuCommenceEntries,
|
|
};
|
|
|
|
static OSD_Entry trampMenuEntries[] =
|
|
{
|
|
{ "- TRAMP -", OME_Label, NULL, NULL, 0 },
|
|
|
|
{ "", OME_Label, NULL, trampCmsStatusString, DYNAMIC },
|
|
{ "PIT", OME_TAB, trampCmsSetPitMode, &trampCmsEntPitMode, 0 },
|
|
{ "BAND", OME_TAB, trampCmsConfigBand, &trampCmsEntBand, 0 },
|
|
{ "CHAN", OME_TAB, trampCmsConfigChan, &trampCmsEntChan, 0 },
|
|
{ "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC },
|
|
{ "POWER", OME_TAB, trampCmsConfigPower, &trampCmsEntPower, 0 },
|
|
{ "T(C)", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC },
|
|
{ "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 },
|
|
|
|
{ "BACK", OME_Back, NULL, NULL, 0 },
|
|
{ NULL, OME_END, NULL, NULL, 0 }
|
|
};
|
|
|
|
CMS_Menu cmsx_menuVtxTramp = {
|
|
.GUARD_text = "XVTXTR",
|
|
.GUARD_type = OME_MENU,
|
|
.onEnter = trampCmsOnEnter,
|
|
.onExit = NULL,
|
|
.onGlobalExit = NULL,
|
|
.entries = trampMenuEntries,
|
|
};
|
|
#endif
|
|
|
|
#ifdef VTX_COMMON
|
|
|
|
// Interface to common VTX API
|
|
|
|
vtxDevType_e vtxTrampGetDeviceType(void)
|
|
{
|
|
return VTXDEV_TRAMP;
|
|
}
|
|
|
|
bool vtxTrampIsReady(void)
|
|
{
|
|
return trampStatus > TRAMP_STATUS_OFFLINE;
|
|
}
|
|
|
|
void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel)
|
|
{
|
|
if (band && channel) {
|
|
trampSetBandAndChannel(band, channel);
|
|
trampCommitChanges();
|
|
}
|
|
}
|
|
|
|
void vtxTrampSetPowerByIndex(uint8_t index)
|
|
{
|
|
if (index) {
|
|
trampSetRFPower(trampPowerTable[index - 1]);
|
|
trampCommitChanges();
|
|
}
|
|
}
|
|
|
|
void vtxTrampSetPitMode(uint8_t onoff)
|
|
{
|
|
trampSetPitMode(onoff);
|
|
}
|
|
|
|
bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
|
|
{
|
|
if (!vtxTrampIsReady())
|
|
return false;
|
|
|
|
*pBand = trampBand;
|
|
*pChannel = trampChannel;
|
|
return true;
|
|
}
|
|
|
|
bool vtxTrampGetPowerIndex(uint8_t *pIndex)
|
|
{
|
|
if (!vtxTrampIsReady())
|
|
return false;
|
|
|
|
if (trampConfiguredPower > 0) {
|
|
for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
|
|
if (trampConfiguredPower <= trampPowerTable[i]) {
|
|
*pIndex = i + 1;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool vtxTrampGetPitMode(uint8_t *pOnOff)
|
|
{
|
|
if (!vtxTrampIsReady())
|
|
return false;
|
|
|
|
*pOnOff = trampPitMode;
|
|
return true;
|
|
}
|
|
|
|
static const vtxVTable_t trampVTable = {
|
|
.process = vtxTrampProcess,
|
|
.getDeviceType = vtxTrampGetDeviceType,
|
|
.isReady = vtxTrampIsReady,
|
|
.setBandAndChannel = vtxTrampSetBandAndChannel,
|
|
.setPowerByIndex = vtxTrampSetPowerByIndex,
|
|
.setPitMode = vtxTrampSetPitMode,
|
|
.getBandAndChannel = vtxTrampGetBandAndChannel,
|
|
.getPowerIndex = vtxTrampGetPowerIndex,
|
|
.getPitMode = vtxTrampGetPitMode,
|
|
};
|
|
|
|
#endif
|
|
|
|
bool vtxTrampInit(void)
|
|
{
|
|
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
|
|
|
|
if (portConfig) {
|
|
trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
|
|
}
|
|
|
|
if (!trampSerialPort) {
|
|
return false;
|
|
}
|
|
|
|
#if defined(VTX_COMMON)
|
|
vtxCommonRegisterDevice(&vtxTramp);
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
#endif // VTX_TRAMP
|