diff --git a/Makefile b/Makefile
index 1c8fa50377..c7f86dfdae 100644
--- a/Makefile
+++ b/Makefile
@@ -612,7 +612,9 @@ HIGHEND_SRC = \
telemetry/mavlink.c \
telemetry/ibus.c \
sensors/esc_sensor.c \
- io/vtx_smartaudio.c
+ io/vtx_common.c \
+ io/vtx_smartaudio.c \
+ io/vtx_tramp.c
SPEED_OPTIMISED_SRC := ""
SIZE_OPTIMISED_SRC := ""
@@ -717,7 +719,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
cms/cms_menu_misc.c \
cms/cms_menu_osd.c \
cms/cms_menu_vtx.c \
- io/vtx_smartaudio.c
+ io/vtx_smartaudio.c \
+ io/vtx_tramp.c
endif #F3
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c
index a0dc6b19ea..1827655268 100644
--- a/src/main/cms/cms_menu_builtin.c
+++ b/src/main/cms/cms_menu_builtin.c
@@ -48,6 +48,7 @@
// User supplied menus
#include "io/vtx_smartaudio_cms.h"
+#include "io/vtx_tramp.h"
// Info
@@ -96,9 +97,14 @@ static OSD_Entry menuFeaturesEntries[] =
#if defined(VTX) || defined(USE_RTC6705)
{"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0},
#endif // VTX || USE_RTC6705
+#if defined(VTX_CONTROL)
#if defined(VTX_SMARTAUDIO)
- {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0},
+ {"VTX SA", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0},
#endif
+#if defined(VTX_TRAMP)
+ {"VTX TR", OME_Submenu, cmsMenuChange, &cmsx_menuVtxTramp, 0},
+#endif
+#endif // VTX_CONTROL
#ifdef LED_STRIP
{"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0},
#endif // LED_STRIP
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 51d1421558..00c9475814 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -92,6 +92,7 @@
#include "io/displayport_msp.h"
#include "io/vtx.h"
#include "io/vtx_smartaudio.h"
+#include "io/vtx_tramp.h"
#include "scheduler/scheduler.h"
@@ -523,10 +524,18 @@ void init(void)
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
+#ifdef VTX_CONTROL
+
#ifdef VTX_SMARTAUDIO
smartAudioInit();
#endif
+#ifdef VTX_TRAMP
+ trampInit();
+#endif
+
+#endif // VTX_CONTROL
+
// start all timers
// TODO - not implemented yet
timerStart();
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 6442870268..b746f0252e 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -53,6 +53,7 @@
#include "io/serial.h"
#include "io/transponder_ir.h"
#include "io/vtx_smartaudio.h"
+#include "io/vtx_tramp.h"
#include "msp/msp_serial.h"
@@ -215,6 +216,9 @@ void taskVtxControl(uint32_t currentTime)
#ifdef VTX_SMARTAUDIO
smartAudioProcess(currentTime);
#endif
+#ifdef VTX_TRAMP
+ trampProcess(currentTime);
+#endif
}
#endif
@@ -300,7 +304,7 @@ void fcTasksInit(void)
setTaskEnabled(TASK_STACK_CHECK, true);
#endif
#ifdef VTX_CONTROL
-#ifdef VTX_SMARTAUDIO
+#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
setTaskEnabled(TASK_VTXCTRL, true);
#endif
#endif
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 6bbfb0420f..5a68a18041 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -37,8 +37,9 @@ typedef enum {
FUNCTION_BLACKBOX = (1 << 7), // 128
FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512
FUNCTION_ESC_SENSOR = (1 << 10), // 1024
- FUNCTION_VTX_CONTROL = (1 << 11), // 2048
- FUNCTION_TELEMETRY_IBUS = (1 << 12) // 4096
+ FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048
+ FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096
+ FUNCTION_VTX_TRAMP = (1 << 13), // 8192
} serialPortFunction_e;
typedef enum {
diff --git a/src/main/io/vtx_common.c b/src/main/io/vtx_common.c
new file mode 100644
index 0000000000..5f89be8dd3
--- /dev/null
+++ b/src/main/io/vtx_common.c
@@ -0,0 +1,75 @@
+/*
+ * 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 .
+ */
+
+/* Created by jflyper */
+
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+#include "build/debug.h"
+
+#if defined(VTX_CONTROL)
+
+const uint16_t vtx58FreqTable[5][8] =
+{
+ { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A
+ { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B
+ { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E
+ { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark
+ { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand
+};
+
+const char * const vtx58BandNames[] = {
+ "--------",
+ "BOSCAM A",
+ "BOSCAM B",
+ "BOSCAM E",
+ "FATSHARK",
+ "RACEBAND",
+};
+
+const char vtx58BandLetter[] = "-ABEFR";
+
+const char * const vtx58ChannelNames[] = {
+ "-", "1", "2", "3", "4", "5", "6", "7", "8",
+};
+
+bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan)
+{
+ uint8_t band;
+ uint8_t chan;
+
+ for (band = 0 ; band < 5 ; band++) {
+ for (chan = 0 ; chan < 8 ; chan++) {
+ if (vtx58FreqTable[band][chan] == freq) {
+ *pBand = band + 1;
+ *pChan = chan + 1;
+ return true;
+ }
+ }
+ }
+
+ *pBand = 0;
+ *pChan = 0;
+
+ return false;
+}
+
+#endif
diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_common.h
new file mode 100644
index 0000000000..cd593048b6
--- /dev/null
+++ b/src/main/io/vtx_common.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include
+
+#if defined(VTX_CONTROL)
+
+extern const uint16_t vtx58FreqTable[5][8];
+extern const char * const vtx58BandNames[];
+extern const char * const vtx58ChannelNames[];
+extern const char vtx58BandLetter[];
+
+bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan);
+
+#endif
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index 2fbe947435..19b9cad91c 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -23,7 +23,7 @@
#include "platform.h"
-#ifdef VTX_SMARTAUDIO
+#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
#include "cms/cms.h"
#include "cms/cms_types.h"
@@ -35,6 +35,7 @@
#include "drivers/serial.h"
#include "io/serial.h"
#include "io/vtx_smartaudio.h"
+#include "io/vtx_common.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
@@ -130,18 +131,6 @@ static smartAudioStat_t saStat = {
.badcode = 0,
};
-// The band/chan to frequency table
-// XXX Should really be consolidated among different vtx drivers
-static const uint16_t saFreqTable[5][8] =
-{
- { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boacam A
- { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B
- { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E
- { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark
- { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand
-};
-// XXX Should frequencies not usable in locked state be detected?
-
typedef struct saPowerTable_s {
int rfpower;
int16_t valueV1;
@@ -663,9 +652,9 @@ bool smartAudioInit()
}
#endif
- serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL);
+ serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO);
if (portConfig) {
- smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP);
+ smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP);
}
if (!smartAudioSerialPort) {
@@ -767,9 +756,9 @@ void saCmsUpdate(void)
saCmsBand = (saDevice.chan / 8) + 1;
saCmsChan = (saDevice.chan % 8) + 1;
- saCmsFreqRef = saFreqTable[saDevice.chan / 8][saDevice.chan % 8];
+ saCmsFreqRef = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8];
- saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8];
+ saCmsDeviceFreq = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8];
if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) {
saCmsRFState = SACMS_TXMODE_ACTIVE;
@@ -828,7 +817,7 @@ else
tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.freq);
else
tfp_sprintf(&saCmsStatusString[5], "%4d",
- saFreqTable[saDevice.chan / 8][saDevice.chan % 8]);
+ vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]);
saCmsStatusString[9] = ' ';
@@ -868,7 +857,7 @@ dprintf(("saCmsConfigBand: band req %d ", saCmsBand));
if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred))
saSetBandChan(saCmsBand - 1, saCmsChan - 1);
- saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1];
+ saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1];
return 0;
}
@@ -893,7 +882,7 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self)
if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred))
saSetBandChan(saCmsBand - 1, saCmsChan - 1);
- saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1];
+ saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1];
return 0;
}
@@ -994,22 +983,9 @@ static CMS_Menu saCmsMenuStats = {
.entries = saCmsMenuStatsEntries
};
-static const char * const saCmsBandNames[] = {
- "--------",
- "BOSCAM A",
- "BOSCAM B",
- "BOSCAM E",
- "FATSHARK",
- "RACEBAND",
-};
+static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL };
-static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, &saCmsBandNames[0], NULL };
-
-static const char * const saCmsChanNames[] = {
- "-", "1", "2", "3", "4", "5", "6", "7", "8",
-};
-
-static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &saCmsChanNames[0], NULL };
+static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &vtx58ChannelNames[0], NULL };
static const char * const saCmsPowerNames[] = {
"---",
diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c
new file mode 100644
index 0000000000..4e4716836a
--- /dev/null
+++ b/src/main/io/vtx_tramp.c
@@ -0,0 +1,597 @@
+/*
+ * 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 .
+ */
+
+/* Created by jflyper */
+
+#include
+#include
+#include
+#include
+
+#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/system.h"
+#include "io/vtx_tramp.h"
+#include "io/vtx_common.h"
+
+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 trampCurBand = 0;
+uint8_t trampCurChan = 0;
+uint16_t trampCurPower = 0; // Actual transmitting power
+uint16_t trampCurConfigPower = 0; // Configured transmitting power
+int16_t trampCurTemp = 0;
+uint8_t trampCurPitmode = 0;
+
+uint32_t trampConfFreq = 0;
+uint16_t trampConfPower = 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;
+}
+
+void trampSendFreq(uint16_t freq)
+{
+ trampCmdU16('F', freq);
+}
+
+void trampSetBandChan(uint8_t band, uint8_t chan)
+{
+ trampSetFreq(vtx58FreqTable[band - 1][chan - 1]);
+}
+
+void trampSetRFPower(uint16_t level)
+{
+ trampConfPower = level;
+}
+
+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;
+ trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
+ trampCurPitmode = trampRespBuffer[7];
+ trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
+ vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan);
+ 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) {
+ trampCurTemp = 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');
+}
+
+#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR)
+
+bool trampInit()
+{
+ 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;
+ }
+
+ return true;
+}
+
+void trampProcess(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 != trampCurFreq) {
+ trampSendFreq(trampConfFreq);
+#ifdef TRAMP_DEBUG
+ debugFreqReqCounter++;
+#endif
+ done = false;
+ }
+ else if (trampConfPower != trampCurConfigPower) {
+ trampSendRFPower(trampConfPower);
+#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;
+ }
+ }
+ 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[trampCurBand];
+ trampCmsStatusString[3] = vtx58ChannelNames[trampCurChan][0];
+ trampCmsStatusString[4] = ' ';
+
+ if (trampCurFreq)
+ tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq);
+ else
+ tfp_sprintf(&trampCmsStatusString[5], "----");
+
+ if (trampCurPower) {
+ tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampCurPower == trampCurConfigPower) ? ' ' : '*', trampCurPower);
+ }
+ 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, NULL };
+
+static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL };
+
+static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
+
+static const char * const trampCmsPowerNames[] = {
+ "25 ", "100", "200", "400", "600"
+};
+
+static const uint16_t trampCmsPowerTable[] = {
+ 25, 100, 200, 400, 600
+};
+
+static uint8_t trampCmsPower = 0;
+
+static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampCmsPowerNames, NULL };
+
+static void trampCmsUpdateFreqRef(void)
+{
+ if (trampCmsBand > 0 && trampCmsChan > 0)
+ trampCmsFreqRef = vtx58FreqTable[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 OSD_INT16_t trampCmsEntTemp = { &trampCurTemp, -100, 300, 0 };
+
+static const char * const trampCmsPitmodeNames[] = {
+ "---", "OFF", "ON "
+};
+
+static OSD_TAB_t trampCmsEntPitmode = { &trampCmsPitmode, 2, trampCmsPitmodeNames, NULL };
+
+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);
+
+ trampSetBandChan(trampCmsBand, trampCmsChan);
+ trampSetRFPower(trampCmsPowerTable[trampCmsPower]);
+
+ // If it fails, the user should retry later
+ trampCommitChanges();
+
+
+ return MENU_CHAIN_BACK;
+}
+
+static void trampCmsInitSettings()
+{
+ if(trampCurBand > 0) trampCmsBand = trampCurBand;
+ if(trampCurChan > 0) trampCmsChan = trampCurChan;
+
+ trampCmsUpdateFreqRef();
+ trampCmsPitmode = trampCurPitmode + 1;
+
+ if (trampCurConfigPower > 0) {
+ for (uint8_t i = 0; i < sizeof(trampCmsPowerTable); i++) {
+ if (trampCurConfigPower <= trampCmsPowerTable[i]) {
+ trampCmsPower = i;
+ 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, NULL, &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
+
+#endif // VTX_TRAMP
diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h
new file mode 100644
index 0000000000..1864cf0a97
--- /dev/null
+++ b/src/main/io/vtx_tramp.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#if defined(VTX_TRAMP) && defined(VTX_CONTROL)
+
+bool trampInit();
+void trampProcess(uint32_t currentTimeUs);
+
+#ifdef CMS
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+extern CMS_Menu cmsx_menuVtxTramp;
+#endif
+
+#endif
diff --git a/src/main/target/common.h b/src/main/target/common.h
index f71963ee9b..b818ddf574 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -105,6 +105,7 @@
#define USE_SERIALRX_JETIEXBUS
#define VTX_CONTROL
#define VTX_SMARTAUDIO
+#define VTX_TRAMP
#define USE_SENSOR_NAMES
#else
#define SKIP_CLI_COMMAND_HELP
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 193124c071..d881516c14 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -22,5 +22,4 @@
// Targets with built-in vtx do not need external vtx
#if defined(VTX) || defined(USE_RTC6705)
# undef VTX_CONTROL
-# undef VTX_SMARTAUDIO
#endif