Merge pull request #4147 from iNavFlight/de_ffpv_24g_vtx

Implement support for FFPV 2.4GHz VTX protocol

make/source.mk

@@ -137,6 +137,7 @@ COMMON_SRC = \
             cms/cms_menu_osd.c \
             cms/cms_menu_vtx_smartaudio.c \
             cms/cms_menu_vtx_tramp.c \
+            cms/cms_menu_vtx_ffpv.c \
             common/colorconversion.c \
             common/gps_conversion.c \
             drivers/display_ug2864hsweg01.c \
@@ -189,6 +190,7 @@ COMMON_SRC = \
             io/vtx_string.c \
             io/vtx_smartaudio.c \
             io/vtx_tramp.c \
+            io/vtx_ffpv24g.c \
             io/vtx_control.c
 
 COMMON_DEVICE_SRC = \

src/main/cms/cms_menu_builtin.c

@@ -53,6 +53,7 @@
 
 #include "cms/cms_menu_vtx_smartaudio.h"
 #include "cms/cms_menu_vtx_tramp.h"
+#include "cms/cms_menu_vtx_ffpv.h"
 
 
 // Info
@@ -119,6 +120,9 @@ static const OSD_Entry menuFeaturesEntries[] =
 #if defined(USE_VTX_TRAMP)
     OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp),
 #endif
+#if defined(USE_VTX_FFPV)
+    OSD_SUBMENU_ENTRY("VTX FFPV", &cmsx_menuVtxFFPV),
+#endif
 #endif // VTX_CONTROL
 #ifdef USE_LED_STRIP
     OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip),

src/main/cms/cms_menu_osd.c

@@ -166,9 +166,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS),
     OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR),
     OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES),
-#ifdef USE_VTX_COMMON
     OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL),
-#endif // VTX
     OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW),
     OSD_ELEMENT_ENTRY("POWER", OSD_POWER),
     OSD_ELEMENT_ENTRY("USED MAH", OSD_MAH_DRAWN),
@@ -252,7 +250,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_END_ENTRY,
 };
 
-#if defined(USE_VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT)
+#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT)
 // All CMS OSD elements should be enabled in this case. The menu has 3 extra
 // elements (label, back and end), but there's an OSD element that we intentionally
 // don't show here (OSD_DEBUG).

src/main/cms/cms_menu_vtx_ffpv.c

@@ -0,0 +1,216 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <ctype.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#if defined(USE_CMS) && defined(USE_VTX_FFPV)
+
+#include "common/printf.h"
+#include "common/utils.h"
+
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+
+#include "drivers/vtx_common.h"
+
+#include "fc/config.h"
+
+#include "io/vtx_string.h"
+#include "io/vtx_ffpv24g.h"
+#include "io/vtx.h"
+
+static bool ffpvCmsDrawStatusString(char *buf, unsigned bufsize)
+{
+    const char *defaultString = "- -- ---- ---";
+//                               m bc ffff ppp
+//                               01234567890123
+
+    if (bufsize < strlen(defaultString) + 1) {
+        return false;
+    }
+
+    strcpy(buf, defaultString);
+
+    vtxDevice_t *vtxDevice = vtxCommonDevice();
+    if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_FFPV || !vtxCommonDeviceIsReady(vtxDevice)) {
+        return true;
+    }
+
+    buf[0] = '*';
+    buf[1] = ' ';
+    buf[2] = ffpvBandLetters[ffpvGetRuntimeState()->band];
+    buf[3] = ffpvChannelNames[ffpvGetRuntimeState()->channel][0];
+    buf[4] = ' ';
+
+    tfp_sprintf(&buf[5], "%4d", ffpvGetRuntimeState()->frequency);
+    tfp_sprintf(&buf[9], " %3d", ffpvGetRuntimeState()->powerMilliwatt);
+
+    return true;
+}
+
+uint8_t ffpvCmsBand = 1;
+uint8_t ffpvCmsChan = 1;
+uint16_t ffpvCmsFreqRef;
+static uint8_t ffpvCmsPower = 1;
+
+static const OSD_TAB_t ffpvCmsEntBand = { &ffpvCmsBand, VTX_FFPV_BAND_COUNT, ffpvBandNames };
+static const OSD_TAB_t ffpvCmsEntChan = { &ffpvCmsChan, VTX_FFPV_CHANNEL_COUNT, ffpvChannelNames };
+static const OSD_TAB_t ffpvCmsEntPower = { &ffpvCmsPower, VTX_FFPV_POWER_COUNT, ffpvPowerNames };
+
+static void ffpvCmsUpdateFreqRef(void)
+{
+    if (ffpvCmsBand > 0 && ffpvCmsChan > 0) {
+        ffpvCmsFreqRef = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1];
+    }
+}
+
+static long ffpvCmsConfigBand(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (ffpvCmsBand == 0) {
+        // Bounce back
+        ffpvCmsBand = 1;
+    }
+    else {
+        ffpvCmsUpdateFreqRef();
+    }
+
+    return 0;
+}
+
+static long ffpvCmsConfigChan(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (ffpvCmsChan == 0) {
+        // Bounce back
+        ffpvCmsChan = 1;
+    }
+    else {
+        ffpvCmsUpdateFreqRef();
+    }
+
+    return 0;
+}
+
+static long ffpvCmsConfigPower(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (ffpvCmsPower == 0) {
+        // Bounce back
+        ffpvCmsPower = 1;
+    }
+
+    return 0;
+}
+
+static long ffpvCmsCommence(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    // call driver directly
+    ffpvSetBandAndChannel(ffpvCmsBand, ffpvCmsChan);
+    ffpvSetRFPowerByIndex(ffpvCmsPower);
+
+    // update'vtx_' settings
+    vtxSettingsConfigMutable()->band = ffpvCmsBand;
+    vtxSettingsConfigMutable()->channel = ffpvCmsChan;
+    vtxSettingsConfigMutable()->power = ffpvCmsPower;
+    vtxSettingsConfigMutable()->freq = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1];
+
+    saveConfigAndNotify();
+
+    return MENU_CHAIN_BACK;
+}
+
+static void ffpvCmsInitSettings(void)
+{
+    ffpvCmsBand = ffpvGetRuntimeState()->band;
+    ffpvCmsChan = ffpvGetRuntimeState()->channel;
+    ffpvCmsPower = ffpvGetRuntimeState()->powerIndex;
+
+    ffpvCmsUpdateFreqRef();
+}
+
+static long ffpvCmsOnEnter(const OSD_Entry *from)
+{
+    UNUSED(from);
+
+    ffpvCmsInitSettings();
+    return 0;
+}
+
+static const OSD_Entry ffpvCmsMenuCommenceEntries[] =
+{
+    OSD_LABEL_ENTRY("CONFIRM"),
+    OSD_FUNC_CALL_ENTRY("YES", ffpvCmsCommence),
+
+    OSD_BACK_ENTRY,
+    OSD_END_ENTRY,
+};
+
+static const CMS_Menu ffpvCmsMenuCommence = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "XVTXTRC",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = NULL,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = ffpvCmsMenuCommenceEntries,
+};
+
+static const OSD_Entry ffpvMenuEntries[] =
+{
+    OSD_LABEL_ENTRY("- TRAMP -"),
+
+    OSD_LABEL_FUNC_DYN_ENTRY("", ffpvCmsDrawStatusString),
+    OSD_TAB_CALLBACK_ENTRY("BAND", ffpvCmsConfigBand, &ffpvCmsEntBand),
+    OSD_TAB_CALLBACK_ENTRY("CHAN", ffpvCmsConfigChan, &ffpvCmsEntChan),
+    OSD_UINT16_RO_ENTRY("(FREQ)", &ffpvCmsFreqRef),
+    OSD_TAB_CALLBACK_ENTRY("POWER", ffpvCmsConfigPower, &ffpvCmsEntPower),
+    OSD_SUBMENU_ENTRY("SET", &ffpvCmsMenuCommence),
+
+    OSD_BACK_ENTRY,
+    OSD_END_ENTRY,
+};
+
+const CMS_Menu cmsx_menuVtxFFPV = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "XVTXTR",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = ffpvCmsOnEnter,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = ffpvMenuEntries,
+};
+#endif

src/main/cms/cms_menu_vtx_ffpv.h

@@ -0,0 +1,23 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+
+extern const CMS_Menu cmsx_menuVtxFFPV;

src/main/drivers/vtx_common.c

@@ -25,8 +25,6 @@
 #include "platform.h"
 #include "build/debug.h"
 
-#if defined(USE_VTX_COMMON)
-
 #include "vtx_common.h"
 
 static vtxDevice_t *commonVtxDevice = NULL;
@@ -152,4 +150,3 @@ bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t
     }
     return false;
 }
-#endif

src/main/drivers/vtx_common.h

@@ -72,6 +72,7 @@ typedef enum {
     // 2 reserved
     VTXDEV_SMARTAUDIO = 3,
     VTXDEV_TRAMP      = 4,
+    VTXDEV_FFPV       = 5,
     VTXDEV_UNKNOWN    = 0xFF,
 } vtxDevType_e;
 

src/main/fc/fc_init.c

@@ -107,6 +107,7 @@
 #include "io/vtx_control.h"
 #include "io/vtx_smartaudio.h"
 #include "io/vtx_tramp.h"
+#include "io/vtx_ffpv24g.h"
 #include "io/piniobox.h"
 
 #include "msp/msp_serial.h"
@@ -637,13 +638,10 @@ void init(void)
     pitotStartCalibration();
 #endif
 
-#if defined(USE_VTX_COMMON) && defined(USE_VTX_CONTROL)
+#if defined(USE_VTX_CONTROL)
     vtxControlInit();
-
-#if defined(USE_VTX_COMMON)
     vtxCommonInit();
     vtxInit();
-#endif
 
 #ifdef USE_VTX_SMARTAUDIO
     vtxSmartAudioInit();
@@ -653,7 +651,11 @@ void init(void)
     vtxTrampInit();
 #endif
 
-#endif // USE_VTX_COMMON && USE_VTX_CONTROL
+#ifdef USE_VTX_FFPV
+    vtxFuriousFPVInit();
+#endif
+
+#endif // USE_VTX_CONTROL
 
     // Now that everything has powered up the voltage and cell count be determined.
     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))

src/main/fc/fc_msp.c

@@ -1280,7 +1280,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         }
         break;
 
-#if defined(USE_VTX_COMMON)
     case MSP_VTX_CONFIG:
         {
             vtxDevice_t *vtxDevice = vtxCommonDevice();
@@ -1309,7 +1308,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
         }
         break;
-#endif
 
     case MSP_NAME:
         {
@@ -2158,7 +2156,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;
 #endif // USE_OSD
 
-#if defined(USE_VTX_COMMON)
     case MSP_SET_VTX_CONFIG:
         if (dataSize >= 2) {
             vtxDevice_t *vtxDevice = vtxCommonDevice();
@@ -2197,7 +2194,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             return MSP_RESULT_ERROR;
         }
         break;
-#endif
 
 #ifdef USE_FLASHFS
     case MSP_DATAFLASH_ERASE:

src/main/fc/fc_tasks.c

@@ -531,11 +531,11 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#if defined(USE_VTX_COMMON) && defined(USE_VTX_CONTROL)
+#if defined(USE_VTX_CONTROL)
     [TASK_VTXCTRL] = {
         .taskName = "VTXCTRL",
         .taskFunc = vtxUpdate,
-        .desiredPeriod = TASK_PERIOD_HZ(5),          // 5Hz @200msec
+        .desiredPeriod = TASK_PERIOD_HZ(50),          // 50Hz @20msec
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif

src/main/fc/settings.yaml

@@ -1697,7 +1697,7 @@ groups:
   - name: PG_VTX_CONFIG
     type: vtxConfig_t
     headers: ["io/vtx_control.h"]
-    condition: USE_VTX_CONTROL && USE_VTX_COMMON
+    condition: USE_VTX_CONTROL
     members:
       - name: vtx_halfduplex
         field: halfDuplex
@@ -1706,7 +1706,6 @@ groups:
   - name: PG_VTX_SETTINGS_CONFIG
     type: vtxSettingsConfig_t
     headers: ["drivers/vtx_common.h", "io/vtx.h"]
-    condition: USE_VTX_COMMON
     members:
       - name: vtx_band
         field: band

src/main/io/osd.c

@@ -1588,7 +1588,6 @@ static bool osdDrawSingleElement(uint8_t item)
         break;
     }
 
-#if defined(VTX) || defined(USE_VTX_COMMON)
     case OSD_VTX_CHANNEL:
 #if defined(VTX)
         // FIXME: This doesn't actually work. It's for boards with
@@ -1616,7 +1615,6 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 #endif
         break;
-#endif // VTX || VTX_COMMON
 
     case OSD_CROSSHAIRS:
         osdCrosshairsBounds(&elemPosX, &elemPosY, NULL);

src/main/io/serial.h

@@ -48,6 +48,7 @@ typedef enum {
     FUNCTION_OPTICAL_FLOW        = (1 << 14), // 16384
     FUNCTION_DEBUG_TRACE         = (1 << 15), // 32768
     FUNCTION_RANGEFINDER         = (1 << 16), // 65536
+    FUNCTION_VTX_FFPV            = (1 << 17), // 131072
 } serialPortFunction_e;
 
 typedef enum {

src/main/io/vtx.c

@@ -23,8 +23,6 @@
 
 #include "platform.h"
 
-#if defined(USE_VTX_COMMON)
-
 #include "common/maths.h"
 #include "common/time.h"
 
@@ -275,5 +273,3 @@ void vtxUpdate(timeUs_t currentTimeUs)
         }
     }
 }
-
-#endif

src/main/io/vtx_control.c

@@ -37,7 +37,7 @@
 #include "io/vtx_control.h"
 
 
-#if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
+#if defined(USE_VTX_CONTROL)
 
 PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2);
 

src/main/io/vtx_control.h

@@ -33,6 +33,15 @@ typedef struct vtxConfig_s {
     uint8_t halfDuplex;
 } vtxConfig_t;
 
+typedef struct vtxRunState_s {
+    int pitMode;
+    int band;
+    int channel;
+    int frequency;
+    int powerIndex;
+    int powerMilliwatt;
+} vtxRunState_t;
+
 PG_DECLARE(vtxConfig_t, vtxConfig);
 
 void vtxControlInit(void);

src/main/io/vtx_ffpv24g.c

@@ -0,0 +1,531 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <ctype.h>
+#include <string.h>
+
+#if defined(USE_VTX_FFPV) && defined(USE_VTX_CONTROL)
+
+#include "build/debug.h"
+
+#include "drivers/time.h"
+#include "drivers/vtx_common.h"
+
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "scheduler/protothreads.h"
+
+//#include "cms/cms_menu_vtx_ffpv24g.h"
+
+#include "io/vtx.h"
+#include "io/vtx_ffpv24g.h"
+#include "io/vtx_control.h"
+#include "io/vtx_string.h"
+#include "io/serial.h"
+
+
+#define VTX_FFPV_CMD_TIMEOUT_MS     250
+#define VTX_FFPV_HEARTBEAT_MS       1000
+
+#define VTX_FFPV_MIN_BAND           (1)
+#define VTX_FFPV_MAX_BAND           (VTX_FFPV_MIN_BAND + VTX_FFPV_BAND_COUNT - 1)
+#define VTX_FFPV_MIN_CHANNEL        (1)
+#define VTX_FFPV_MAX_CHANNEL        (VTX_FFPV_MIN_CHANNEL + VTX_FFPV_CHANNEL_COUNT -1)
+
+#define VTX_UPDATE_REQ_NONE         0x00
+#define VTX_UPDATE_REQ_FREQUENCY    0x01
+#define VTX_UPDATE_REQ_POWER        0x02
+
+typedef struct __attribute__((__packed__)) ffpvPacket_s {
+    uint8_t header;
+    uint8_t cmd;
+    uint8_t data[12];
+    uint8_t checksum;
+    uint8_t footer;
+} ffpvPacket_t;
+
+typedef struct {
+    bool ready;
+    int protoTimeouts;
+    unsigned updateReqMask;
+
+    // VTX capabilities
+    struct {
+        unsigned freqMin;
+        unsigned freqMax;
+        unsigned powerMin;
+        unsigned powerMax;
+    } capabilities;
+
+    // Requested VTX state
+    struct {
+        bool     setByFrequency;
+        int      band;
+        int      channel;
+        unsigned freq;
+        unsigned power;
+        unsigned powerIndex;
+    } request;
+
+    // Actual VTX state
+    struct {
+        unsigned freq;
+        unsigned power;
+    } state;
+
+    // Comms flags and state
+    ffpvPacket_t sendPkt;
+    ffpvPacket_t recvPkt;
+    unsigned     recvPtr;
+    bool         pktReceived;
+} vtxProtoState_t;
+
+/*****************************************************************************/
+const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = {
+    "--------",
+    "FFPV 2.4 A",
+    "FFPV 2.4 B",
+};
+
+const char * ffpvBandLetters = "-AB";
+
+const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT] =
+{
+    { 2410, 2430, 2450, 2470, 2370, 2390, 2490, 2510 }, // FFPV 2.4 A
+    { 2414, 2432, 2450, 2468, 2411, 2433, 2453, 2473 }, // FFPV 2.4 A
+};
+
+const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1] = {
+    "-", "1", "2", "3", "4", "5", "6", "7", "8",
+};
+
+const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1] = {
+    "---", "25 ", "200", "500", "800"
+};
+
+const unsigned ffpvPowerTable[VTX_FFPV_POWER_COUNT] = {
+    25, 200, 500, 800
+};
+
+
+/*******************************************************************************/
+static serialPort_t * vtxSerialPort = NULL;
+static vtxProtoState_t vtxState;
+
+static uint8_t vtxCalcChecksum(ffpvPacket_t * pkt)
+{
+    uint8_t sum = pkt->cmd;
+    for (int i = 0; i < 12; i++) {
+        sum += pkt->data[i];
+    }
+    return sum;
+}
+
+static bool vtxProtoRecv(void)
+{
+    // Return success instantly if packet is already awaiting processing
+    if (vtxState.pktReceived) {
+        return true;
+    }
+
+    uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt;
+    while (serialRxBytesWaiting(vtxSerialPort) && !vtxState.pktReceived) {
+        const uint8_t c = serialRead(vtxSerialPort);
+
+        if (vtxState.recvPtr == 0) {
+            // Wait for sync byte
+            if (c == 0x0F) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+        }
+        else {
+            // Sync byte ok - wait for full packet
+            if (vtxState.recvPtr < sizeof(vtxState.recvPkt)) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+
+            // Received full packet - do some processing
+            if (vtxState.recvPtr == sizeof(vtxState.recvPkt)) {
+                // Full packet received - validate packet, make sure it's the one we expect
+                const bool pktValid = (vtxState.recvPkt.header == 0x0F && vtxState.recvPkt.cmd == vtxState.sendPkt.cmd && vtxState.recvPkt.footer == 0x00 && vtxState.recvPkt.checksum == vtxCalcChecksum(&vtxState.recvPkt));
+                if (!pktValid) {
+                    // Reset the receiver state - keep waiting
+                    vtxState.pktReceived = false;
+                    vtxState.recvPtr = 0;
+                }
+                // Make sure it's not the echo one (half-duplex serial might receive it's own data)
+                else if (memcmp(&vtxState.recvPkt.data, &vtxState.sendPkt.data, sizeof(vtxState.recvPkt.data)) == 0) {
+                    vtxState.pktReceived = false;
+                    vtxState.recvPtr = 0;
+                }
+                // Valid receive packet
+                else {
+                    vtxState.pktReceived = true;
+                    return true;
+                }
+            }
+        }
+    }
+
+    return false;
+}
+
+static void vtxProtoSend(uint8_t cmd, const uint8_t * data)
+{
+    // Craft and send FPV packet
+    vtxState.sendPkt.header = 0x0F;
+    vtxState.sendPkt.cmd = cmd;
+
+    if (data) {
+        memcpy(vtxState.sendPkt.data, data, sizeof(vtxState.sendPkt.data));
+    }
+    else {
+        memset(vtxState.sendPkt.data, 0, sizeof(vtxState.sendPkt.data));
+    }
+
+    vtxState.sendPkt.checksum = vtxCalcChecksum(&vtxState.sendPkt);
+    vtxState.sendPkt.footer = 0x00;
+
+    // Send data 
+    serialWriteBuf(vtxSerialPort, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt));
+
+    // Reset cmd response state
+    vtxState.pktReceived = false;
+    vtxState.recvPtr = 0;
+}
+
+static void vtxProtoSend_SetFreqency(unsigned freq)
+{
+    uint8_t data[12] = {0};
+    data[0] = freq & 0xFF;
+    data[1] = (freq >> 8) & 0xFF;
+    vtxProtoSend(0x46, data);
+}
+
+static void vtxProtoSend_SetPower(unsigned power)
+{
+    uint8_t data[12] = {0};
+    data[0] = power & 0xFF;
+    data[1] = (power >> 8) & 0xFF;
+    vtxProtoSend(0x50, data);
+}
+
+STATIC_PROTOTHREAD(impl_VtxProtocolThread)
+{
+    ptBegin(impl_VtxProtocolThread);
+
+    // 0: Detect VTX. Dwell here infinitely until we get a valid response from VTX
+    vtxState.ready = false;
+    while(!vtxState.ready) {
+        // Send capabilities request and wait
+        vtxProtoSend(0x72, NULL);
+        ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS);
+
+        // Check if we got a valid response
+        if (vtxState.pktReceived) {
+            vtxState.capabilities.freqMin = vtxState.recvPkt.data[0] | (vtxState.recvPkt.data[1] << 8);
+            vtxState.capabilities.freqMax = vtxState.recvPkt.data[2] | (vtxState.recvPkt.data[3] << 8);
+            vtxState.capabilities.powerMin = 0;
+            vtxState.capabilities.powerMax = vtxState.recvPkt.data[4] | (vtxState.recvPkt.data[5] << 8);
+            vtxState.ready = true;
+        }
+    }
+
+    // 1 : Periodically poll VTX for current channel and power, send updates
+    vtxState.protoTimeouts = 0;
+    vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+
+    while(vtxState.ready) {
+        // Wait for request for update or time to check liveness
+        ptWaitTimeout(vtxState.updateReqMask != VTX_UPDATE_REQ_NONE, VTX_FFPV_HEARTBEAT_MS);
+
+        if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
+            if (vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) {
+                vtxProtoSend_SetFreqency(vtxState.request.freq);
+                vtxState.updateReqMask &= ~VTX_UPDATE_REQ_FREQUENCY;
+                ptDelayMs(VTX_FFPV_CMD_TIMEOUT_MS);
+            }
+            else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) {
+                vtxProtoSend_SetPower(vtxState.request.power);
+                vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER;
+            }
+            else {
+                // Unsupported request - reset
+                vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+            }
+        }
+        else {
+            // Periodic check for VTX liveness
+            vtxProtoSend(0x76, NULL);
+            ptWaitTimeout(vtxProtoRecv(), VTX_FFPV_CMD_TIMEOUT_MS);
+
+            if (vtxState.pktReceived) {
+                // Got a valid state from VTX
+                vtxState.state.freq = (uint16_t)vtxState.recvPkt.data[0] | ((uint16_t)vtxState.recvPkt.data[1] << 8);
+                vtxState.state.power = (uint16_t)vtxState.recvPkt.data[2] | ((uint16_t)vtxState.recvPkt.data[3] << 8);
+                vtxState.protoTimeouts = 0;
+
+                // Check if VTX state matches VTX request
+                if (vtxState.state.freq != vtxState.request.freq) {
+                    vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
+                }
+
+                if (vtxState.state.power != vtxState.request.power) {
+                    vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
+                }
+            }
+            else {
+                vtxState.protoTimeouts++;
+            }
+        }
+
+        // Sanity check. If we got more than 3 protocol erros
+        if (vtxState.protoTimeouts >= 3) {
+            // Reset ready flag - thread will terminate and restart
+            vtxState.ready = false;
+        }
+    }
+
+    ptEnd(0);
+}
+
+
+static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
+{
+    // Glue function betwen VTX VTable and actual driver protothread
+    UNUSED(vtxDevice);
+    UNUSED(currentTimeUs);
+
+    impl_VtxProtocolThread();
+
+    // If thread stopped - vtx comms failed - restart thread and re-init VTX comms
+    if (ptIsStopped(ptGetHandle(impl_VtxProtocolThread))) {
+        ptRestart(ptGetHandle(impl_VtxProtocolThread));
+    }
+}
+
+static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice)
+{
+    UNUSED(vtxDevice);
+    return VTXDEV_FFPV;
+}
+
+static bool impl_IsReady(const vtxDevice_t *vtxDevice)
+{
+    return vtxDevice != NULL && vtxSerialPort != NULL && vtxState.ready;
+}
+
+static bool impl_DevSetFreq(uint16_t freq)
+{
+    if (!vtxState.ready || freq < vtxState.capabilities.freqMin || freq > vtxState.capabilities.freqMax) {
+        return false;
+    }
+
+    vtxState.request.freq = freq;
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
+
+    return true;
+}
+
+static void impl_SetFreq(vtxDevice_t * vtxDevice, uint16_t freq)
+{
+    UNUSED(vtxDevice);
+
+    if (impl_DevSetFreq(freq)) {
+        // Keep track that we set frequency directly
+        vtxState.request.setByFrequency = true;
+    }
+}
+
+void ffpvSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    // Validate band and channel
+    if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) {
+        return;
+    }
+
+    if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) {
+        // Keep track of band/channel data
+        vtxState.request.setByFrequency = false;
+        vtxState.request.band = band;
+        vtxState.request.channel = channel;
+    }
+}
+
+
+static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
+{
+    UNUSED(vtxDevice);
+    ffpvSetBandAndChannel(band, channel);
+}
+
+void ffpvSetRFPowerByIndex(uint16_t index)
+{
+    // Validate index
+    if (index < 1 || index > VTX_FFPV_POWER_COUNT) {
+        return;
+    }
+
+    const unsigned power = ffpvPowerTable[index - 1];
+    if (!vtxState.ready || power < vtxState.capabilities.powerMin || power > vtxState.capabilities.powerMax) {
+        return;
+    }
+
+    vtxState.request.power = power;
+    vtxState.request.powerIndex = index;
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
+}
+
+static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
+{
+    UNUSED(vtxDevice);
+    ffpvSetRFPowerByIndex(index);
+}
+
+static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
+{
+    // TODO: Not implemented
+    UNUSED(vtxDevice);
+    UNUSED(onoff);
+}
+
+static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
+{
+    if (!impl_IsReady(vtxDevice)) {
+        return false;
+    }
+
+    // if in user-freq mode then report band as zero
+    *pBand = vtxState.request.setByFrequency ? 0 : vtxState.request.band;
+    *pChannel = vtxState.request.channel;
+    return true;
+}
+
+static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
+{
+    if (!impl_IsReady(vtxDevice)) {
+        return false;
+    }
+
+    *pIndex = vtxState.request.powerIndex;
+
+    return true;
+}
+
+static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
+{
+    if (!impl_IsReady(vtxDevice)) {
+        return false;
+    }
+
+    // TODO: Not inplemented
+    *pOnOff = 0;
+    return true;
+}
+
+static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
+{
+    if (!impl_IsReady(vtxDevice)) {
+        return false;
+    }
+
+    *pFreq = vtxState.request.freq;
+    return true;
+}
+
+vtxRunState_t * ffpvGetRuntimeState(void)
+{
+    static vtxRunState_t state;
+
+    if (vtxState.ready) {
+        state.pitMode = 0;
+        state.band = vtxState.request.band;
+        state.channel = vtxState.request.channel;
+        state.frequency = vtxState.request.freq;
+        state.powerIndex = vtxState.request.powerIndex;
+        state.powerMilliwatt = vtxState.request.power;
+    }
+    else {
+        state.pitMode = 0;
+        state.band = 1;
+        state.channel = 1;
+        state.frequency = ffpvFrequencyTable[0][0];
+        state.powerIndex = 1;
+        state.powerMilliwatt = 25;
+    }
+    return &state;
+}
+
+/*****************************************************************************/
+static const vtxVTable_t impl_vtxVTable = {
+    .process = impl_Process,
+    .getDeviceType = impl_GetDeviceType,
+    .isReady = impl_IsReady,
+    .setBandAndChannel = impl_SetBandAndChannel,
+    .setPowerByIndex = impl_SetPowerByIndex,
+    .setPitMode = impl_SetPitMode,
+    .setFrequency = impl_SetFreq,
+    .getBandAndChannel = impl_GetBandAndChannel,
+    .getPowerIndex = impl_GetPowerIndex,
+    .getPitMode = impl_GetPitMode,
+    .getFrequency = impl_GetFreq,
+};
+
+static vtxDevice_t impl_vtxDevice = {
+    .vTable = &impl_vtxVTable,
+    .capability.bandCount = VTX_FFPV_BAND_COUNT,
+    .capability.channelCount = VTX_FFPV_CHANNEL_COUNT,
+    .capability.powerCount = VTX_FFPV_POWER_COUNT,
+    .bandNames = (char **)ffpvBandNames,
+    .channelNames = (char **)ffpvChannelNames,
+    .powerNames = (char **)ffpvPowerNames,
+};
+
+bool vtxFuriousFPVInit(void)
+{
+    serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_VTX_FFPV);
+
+    if (portConfig) {
+        portOptions_t portOptions = 0;
+        portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
+        vtxSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_FFPV, NULL, NULL, 9600, MODE_RXTX, portOptions);
+    }
+
+    if (!vtxSerialPort) {
+        return false;
+    }
+
+    vtxCommonSetDevice(&impl_vtxDevice);
+
+    ptRestart(ptGetHandle(impl_VtxProtocolThread));
+
+    return true;
+}
+
+#endif

src/main/io/vtx_ffpv24g.h

@@ -0,0 +1,51 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "platform.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <ctype.h>
+#include <string.h>
+
+#include "io/vtx.h"
+#include "io/vtx_control.h"
+
+#define VTX_FFPV_BAND_COUNT         2
+#define VTX_FFPV_CHANNEL_COUNT      8
+#define VTX_FFPV_POWER_COUNT        4
+
+extern const char * ffpvBandLetters;
+extern const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1];
+extern const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1];
+extern const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1];
+extern const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT];
+
+bool vtxFuriousFPVInit(void);
+void ffpvSetBandAndChannel(uint8_t band, uint8_t channel);
+void ffpvSetRFPowerByIndex(uint16_t index);
+
+vtxRunState_t * ffpvGetRuntimeState(void);

src/main/io/vtx_smartaudio.c

@@ -63,13 +63,10 @@ serialPort_t *debugSerialPort = NULL;
 
 static serialPort_t *smartAudioSerialPort = NULL;
 
-#if defined(USE_CMS) || defined(USE_VTX_COMMON)
 const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = {
     "---", "25 ", "200", "500", "800",
 };
-#endif
 
-#ifdef USE_VTX_COMMON
 static const vtxVTable_t saVTable;    // Forward
 static vtxDevice_t vtxSmartAudio = {
     .vTable = &saVTable,
@@ -80,7 +77,6 @@ static vtxDevice_t vtxSmartAudio = {
     .channelNames = (char **)vtx58ChannelNames,
     .powerNames = (char **)saPowerNames,
 };
-#endif
 
 // SmartAudio command and response codes
 enum {
@@ -340,10 +336,6 @@ static void saProcessResponse(uint8_t *buf, int len)
     }
     saDevicePrev = saDevice;
 
-#ifdef USE_VTX_COMMON
-    // Todo: Update states in saVtxDevice?
-#endif
-
 #ifdef USE_CMS
     // Export current device status for CMS
     saCmsUpdate();
@@ -671,12 +663,7 @@ bool vtxSmartAudioInit(void)
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO);
     if (portConfig) {
         portOptions_t portOptions = SERIAL_BIDIR_NOPULL;
-#if defined(USE_VTX_COMMON)
         portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR);
-#else
-        portOptions = SERIAL_BIDIR;
-#endif
-
         smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions);
     }
 
@@ -766,7 +753,6 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
     }
 }
 
-#ifdef USE_VTX_COMMON
 // Interface to common VTX API
 
 vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice)
@@ -893,7 +879,6 @@ static const vtxVTable_t saVTable = {
     .getPitMode = vtxSAGetPitMode,
     .getFrequency = vtxSAGetFreq,
 };
-#endif // VTX_COMMON
 
 
 #endif // VTX_SMARTAUDIO

src/main/io/vtx_string.c

@@ -25,8 +25,6 @@
 #include "platform.h"
 #include "build/debug.h"
 
-#if defined(USE_VTX_COMMON)
-
 #define VTX_STRING_BAND_COUNT 5
 #define VTX_STRING_CHAN_COUNT 8
 
@@ -89,5 +87,3 @@ uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel)
     }
     return 0;
 }
-
-#endif

src/main/io/vtx_tramp.c

@@ -44,7 +44,7 @@
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
-#if defined(USE_CMS) || defined(USE_VTX_COMMON)
+#if defined(USE_CMS)
 const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
     25, 100, 200, 400, 600
 };
@@ -54,7 +54,6 @@ const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
 };
 #endif
 
-#if defined(USE_VTX_COMMON)
 static const vtxVTable_t trampVTable; // forward
 static vtxDevice_t vtxTramp = {
     .vTable = &trampVTable,
@@ -65,7 +64,6 @@ static vtxDevice_t vtxTramp = {
     .channelNames = (char **)vtx58ChannelNames,
     .powerNames = (char **)trampPowerNames,
 };
-#endif
 
 static serialPort_t *trampSerialPort = NULL;
 
@@ -479,8 +477,6 @@ static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 }
 
 
-#ifdef USE_VTX_COMMON
-
 // Interface to common VTX API
 
 static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
@@ -588,7 +584,6 @@ static const vtxVTable_t trampVTable = {
     .getFrequency = vtxTrampGetFreq,
 };
 
-#endif
 
 bool vtxTrampInit(void)
 {
@@ -596,11 +591,7 @@ bool vtxTrampInit(void)
 
     if (portConfig) {
         portOptions_t portOptions = 0;
-#if defined(USE_VTX_COMMON)
         portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
-#else
-        portOptions = SERIAL_BIDIR;
-#endif
 
         trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
     }
@@ -609,9 +600,7 @@ bool vtxTrampInit(void)
         return false;
     }
 
-#if defined(USE_VTX_COMMON)
     vtxCommonSetDevice(&vtxTramp);
-#endif
 
     return true;
 }

src/main/target/SPRACINGF3NEO/target.h

@@ -111,6 +111,7 @@
 #define USE_VTX_RTC6705
 #define VTX_RTC6705_OPTIONAL    // VTX/OSD board is OPTIONAL
 
+#undef USE_VTX_FFPV
 #undef USE_VTX_SMARTAUDIO           // Disabled due to flash size
 #undef USE_VTX_TRAMP                // Disabled due to flash size
 

src/main/target/common.h

@@ -133,10 +133,10 @@
 #define USE_PITOT_ADC
 
 //Enable VTX control
-#define USE_VTX_COMMON
 #define USE_VTX_CONTROL
 #define USE_VTX_SMARTAUDIO
 #define USE_VTX_TRAMP
+#define USE_VTX_FFPV
 
 //Enable DST calculations
 #define RTC_AUTOMATIC_DST