Some enhancements

Makefile

@@ -611,6 +611,7 @@ HIGHEND_SRC = \
             telemetry/ibus.c \
             sensors/esc_sensor.c \
             drivers/vtx_var.c \
+            io/vtx_common.c \
             io/vtx_smartaudio.c \
             io/vtx_tramp.c
 

src/main/drivers/vtx_var.c

@@ -19,6 +19,8 @@ const char * const vtx58BandNames[] = {
     "RACEBAND",
 };
 
+const char vtx58BandLetter[] = "-ABEFR";
+
 const char * const vtx58ChanNames[] = {
     "-", "1", "2", "3", "4", "5", "6", "7", "8",
 };

src/main/drivers/vtx_var.h

@@ -1,5 +1,8 @@
+#pragma once
+
 #include <stdint.h>
 
 extern const uint16_t vtx58FreqTable[5][8];
 extern const char * const vtx58BandNames[];
 extern const char * const vtx58ChanNames[];
+extern const char vtx58BandLetter[];

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

src/main/io/vtx_common.c

@@ -0,0 +1,52 @@
+/*
+ * 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"
+#include "build/debug.h"
+
+#if defined(VTX_TRAMP)
+#include "drivers/vtx_var.h"
+
+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

src/main/io/vtx_common.h

@@ -33,3 +33,5 @@ typedef struct vtxVTable_s {
 // PIT mode is defined as LOWEST POSSIBLE RF POWER.
 // - It can be a dedicated mode, or lowest RF power possible.
 // - It is *NOT* RF on/off control.
+
+bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan);

src/main/io/vtx_tramp.c

@@ -29,15 +29,39 @@
 #include "build/debug.h"
 
 #include "common/utils.h"
+#include "common/printf.h"
 
 #include "io/serial.h"
 #include "drivers/serial.h"
 #include "drivers/vtx_var.h"
 #include "io/vtx_tramp.h"
+#include "io/vtx_common.h"
 
 static serialPort_t *trampSerialPort = NULL;
 
-static uint8_t trampCmdBuffer[16];
+static uint8_t trampReqBuffer[16];
+static uint8_t trampRespBuffer[16];
+
+typedef enum {
+    TRAMP_STATUS_BAD_DEVICE = -1,
+    TRAMP_STATUS_OFFLINE = 0,
+    TRAMP_STATUS_ONLINE
+} 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;
+
+#ifdef CMS
+void trampCmsUpdateStatusString(void); // Forward
+#endif
 
 static void trampWriteBuf(uint8_t *buf)
 {
@@ -59,13 +83,13 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
     if (!trampSerialPort)
         return;
 
-    memset(trampCmdBuffer, 0, ARRAYLEN(trampCmdBuffer));
+    memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
-    trampCmdBuffer[0] = 15;
+    trampReqBuffer[0] = 15;
-    trampCmdBuffer[1] = cmd;
+    trampReqBuffer[1] = cmd;
-    trampCmdBuffer[2] = param & 0xff;
+    trampReqBuffer[2] = param & 0xff;
-    trampCmdBuffer[3] = (param >> 8) & 0xff;
+    trampReqBuffer[3] = (param >> 8) & 0xff;
-    trampCmdBuffer[14] = trampChecksum(trampCmdBuffer);
+    trampReqBuffer[14] = trampChecksum(trampReqBuffer);
-    trampWriteBuf(trampCmdBuffer);
+    trampWriteBuf(trampReqBuffer);
 }
 
 void trampSetFreq(uint16_t freq)
@@ -88,12 +112,38 @@ void trampSetPitmode(uint8_t onoff)
     trampCmdU16('I', (uint16_t)onoff);
 }
 
+static uint8_t trampPendingQuery = 0; // XXX Assume no code/resp == 0
+
+void trampQuery(uint8_t cmd)
+{
+    trampPendingQuery = cmd;
+    trampCmdU16(cmd, 0);
+}
+
+void trampQueryR(void)
+{
+    trampQuery('r');
+}
+
+void trampQueryV(void)
+{
+    trampQuery('v');
+}
+
+void trampQueryS(void)
+{
+    trampQuery('s');
+}
+
+//#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR)
+#define TRAMP_SERIAL_OPTIONS (0)
+
 bool trampInit()
 {
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL);
 
     if (portConfig) {
-        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, 0);
+        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
     }
 
     if (!trampSerialPort) {
@@ -103,10 +153,151 @@ bool trampInit()
     return true;
 }
 
+void trampHandleResponse(void)
+{
+    uint8_t respCode = trampRespBuffer[1];
+
+    switch (respCode) {
+    case 'r':
+        trampRFFreqMin = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+        trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
+        trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
+        trampStatus = TRAMP_STATUS_ONLINE;
+        break;
+
+    case 'v':
+        trampCurFreq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+        vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan);
+        break;
+
+    case 's':
+        break;
+    }
+
+    if (trampPendingQuery == respCode)
+        trampPendingQuery = 0;
+}
+
+static bool trampIsValidResponseCode(uint8_t code)
+{
+    if (code == 'r' || code == 'v' || code == 's')
+        return true;
+    else
+        return false;
+}
+
+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 uint32_t trampFrameStartUs = 0;
+
+// Frame timeout. An actual frame (16B) takes only 16.6 msec,
+// but a frame arrival may span two scheduling intervals (200msec * 2).
+// Effectively same as waiting for a next trampProcess() to run.
+
+#define TRAMP_FRAME_TIMO_US (200 * 1000)
+
+void trampReceive(uint32_t currentTimeUs)
+{
+    if (!trampSerialPort)
+        return;
+
+    if ((trampReceiveState != S_WAIT_LEN) && (currentTimeUs - trampFrameStartUs > TRAMP_FRAME_TIMO_US)) {
+        trampReceiveState = S_WAIT_LEN;
+        trampReceivePos = 0;
+    }
+
+    while (serialRxBytesWaiting(trampSerialPort)) {
+        uint8_t c = serialRead(trampSerialPort);
+        trampRespBuffer[trampReceivePos++] = c;
+
+        switch(c) {
+        case S_WAIT_LEN:
+            if (c == 0x0F) {
+                trampReceiveState = S_WAIT_CODE;
+                trampFrameStartUs = currentTimeUs;
+            }
+            break;
+
+        case S_WAIT_CODE:
+            if (trampIsValidResponseCode(c)) {
+                trampReceiveState = S_DATA;
+            } else {
+                trampReceiveState = S_WAIT_LEN;
+                trampReceivePos = 0;
+            }
+            break;
+
+        case S_DATA:
+            if (trampReceivePos == 16) {
+                uint8_t cksum = trampChecksum(trampRespBuffer);
+                if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0))
+                    trampHandleResponse();
+
+                trampReceiveState = S_WAIT_LEN;
+                trampReceivePos = 0;
+            }
+            break;
+        }
+    }
+}
+
+void trampProcess(uint32_t currentTimeUs)
+{
+    static uint32_t lastQueryRTimeUs = 0;
+
+    if (trampStatus == TRAMP_STATUS_BAD_DEVICE)
+        return;
+
+debug[0]++;
+    trampReceive(currentTimeUs);
+
+    if (trampStatus == TRAMP_STATUS_OFFLINE) {
+        if (currentTimeUs - lastQueryRTimeUs > 1000 * 1000) {
+            trampQueryR();
+            lastQueryRTimeUs = currentTimeUs;
+        }
+    } else if (trampPendingQuery) {
+        trampQuery(trampPendingQuery);
+    } else {
+        trampQueryV();
+    }
+
+#ifdef CMS
+    trampCmsUpdateStatusString();
+#endif
+}
+
 #ifdef CMS
 #include "cms/cms.h"
 #include "cms/cms_types.h"
 
+char trampCmsStatusString[16];
+
+void trampCmsUpdateStatusString(void)
+{
+    trampCmsStatusString[0] = '*';
+    trampCmsStatusString[1] = ' ';
+    trampCmsStatusString[3] = vtx58BandLetter[trampCurBand];
+    trampCmsStatusString[4] = vtx58ChanNames[trampCurChan][0];
+    trampCmsStatusString[5] = ' ';
+
+    if (trampCurFreq)
+        tfp_sprintf(&trampCmsStatusString[6], "%4d", trampCurFreq);
+    else
+        tfp_sprintf(&trampCmsStatusString[6], "----");
+
+    if (trampCurPower)
+        tfp_sprintf(&trampCmsStatusString[10], " %3d", trampCurPower);
+    else
+        tfp_sprintf(&trampCmsStatusString[10], " ---");
+}
+
 uint8_t trampCmsPitmode = 0;
 uint8_t trampCmsBand = 1;
 uint8_t trampCmsChan = 1;
@@ -182,8 +373,8 @@ static OSD_Entry trampMenuEntries[] =
 {
     { "- TRAMP -", OME_Label, NULL, NULL, 0 },
 
-    //{ "",       OME_Label,   NULL,                   saCmsStatusString,  DYNAMIC },
+    { "",       OME_Label,   NULL,                   trampCmsStatusString,  DYNAMIC },
-    { "PIT",    OME_TAB,     trampCmsSetPitmode,    &trampCmsEntPitmode,   0 },
+    { "PIT",    OME_TAB,     trampCmsSetPitmode,     &trampCmsEntPitmode,   0 },
     { "BAND",   OME_TAB,     NULL,                   &trampCmsEntBand,      0 },
     { "CHAN",   OME_TAB,     NULL,                   &trampCmsEntChan,      0 },
     { "(FREQ)", OME_UINT16,  NULL,                   &trampCmsEntFreqRef,   DYNAMIC },

src/main/io/vtx_tramp.h

@@ -1,6 +1,7 @@
 #pragma once
 
 bool trampInit();
+void trampProcess(uint32_t currentTimeUs);
 
 #ifdef CMS
 #include "cms/cms.h"

src/main/target/common.h

@@ -103,7 +103,7 @@
 #define TELEMETRY_IBUS
 #define USE_RX_MSP
 #define USE_SERIALRX_JETIEXBUS
-//#define VTX_CONTROL
+#define VTX_CONTROL
 //#define VTX_SMARTAUDIO
 #define VTX_TRAMP
 #define USE_SENSOR_NAMES