1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 08:45:36 +03:00
betaflight/src/main/io/vtx_tramp.c
2017-05-26 14:03:28 +01:00

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