Rebase + touch-up

Includes fix for SmartAudio CMS behavior

Makefile

@@ -614,7 +614,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  := ""
@@ -721,7 +723,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)))

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

src/main/cms/cms_menu_osd.c

@@ -81,6 +81,9 @@ OSD_Entry menuOsdActiveElemsEntries[] =
 #endif // GPS
     {"ALTITUDE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ALTITUDE], 0},
     {"POWER", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_POWER], 0},
+    {"ROLL PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ROLL_PIDS], 0},
+    {"PITCH PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_PITCH_PIDS], 0},
+    {"YAW PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_YAW_PIDS], 0},
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
 };

src/main/drivers/inverter.c

@@ -20,31 +20,98 @@
 
 #include "platform.h"
 
-#ifdef INVERTER
-
 #include "io.h"
 #include "io_impl.h"
 
 #include "inverter.h"
 
-/*
+#ifdef USE_INVERTER
-    TODO: move this to support multiple inverters on different UARTs etc
+static void inverterSet(IO_t pin, bool on)
-    possibly move to put it in the UART driver itself.
-*/
-static IO_t pin = IO_NONE;
-
-void initInverter(void)
-{
-    pin = IOGetByTag(IO_TAG(INVERTER));
-    IOInit(pin, OWNER_INVERTER, 1);
-    IOConfigGPIO(pin, IOCFG_OUT_PP);
-
-    inverterSet(false);
-}
-
-void inverterSet(bool on)
 {
     IOWrite(pin, on);
 }
 
+static void initInverter(ioTag_t ioTag)
+{
+    IO_t pin = IOGetByTag(ioTag);
+    IOInit(pin, OWNER_INVERTER, 1);
+    IOConfigGPIO(pin, IOCFG_OUT_PP);
+
+    inverterSet(pin, false);
+}
 #endif
+
+void initInverters(void)
+{
+#ifdef INVERTER_PIN_USART1
+    initInverter(IO_TAG(INVERTER_PIN_USART1));
+#endif
+
+#ifdef INVERTER_PIN_USART2
+    initInverter(IO_TAG(INVERTER_PIN_USART2));
+#endif
+
+#ifdef INVERTER_PIN_USART3
+    initInverter(IO_TAG(INVERTER_PIN_USART3));
+#endif
+
+#ifdef INVERTER_PIN_USART4
+    initInverter(IO_TAG(INVERTER_PIN_USART4));
+#endif
+
+#ifdef INVERTER_PIN_USART5
+    initInverter(IO_TAG(INVERTER_PIN_USART5));
+#endif
+
+#ifdef INVERTER_PIN_USART6
+    initInverter(IO_TAG(INVERTER_PIN_USART6));
+#endif
+}
+
+void enableInverter(USART_TypeDef *USARTx, bool on)
+{
+#ifdef USE_INVERTER
+    IO_t pin = IO_NONE;
+
+#ifdef INVERTER_PIN_USART1
+    if (USARTx == USART1) {
+        pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART1));
+    }
+#endif
+
+#ifdef INVERTER_PIN_USART2
+    if (USARTx == USART2) {
+        pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART2));
+    }
+#endif
+
+#ifdef INVERTER_PIN_USART3
+    if (USARTx == USART3) {
+        pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART3));
+    }
+#endif
+
+#ifdef INVERTER_PIN_USART4
+    if (USARTx == USART4) {
+        pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART4));
+    }
+#endif
+
+#ifdef INVERTER_PIN_USART5
+    if (USARTx == USART5) {
+        pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART5));
+    }
+#endif
+
+#ifdef INVERTER_PIN_USART6
+    if (USARTx == USART6) {
+        pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART6));
+    }
+#endif
+
+    inverterSet(pin, on);
+#else
+    UNUSED(USARTx);
+    UNUSED(on);
+#endif
+}

src/main/drivers/inverter.h

@@ -17,14 +17,10 @@
 
 #pragma once
 
-#ifdef INVERTER
+#if defined(INVERTER_PIN_USART1) || defined(INVERTER_PIN_USART2) || defined(INVERTER_PIN_USART3) || defined(INVERTER_PIN_USART4) || defined(INVERTER_PIN_USART5) || defined(INVERTER_PIN_USART6)
-void inverterSet(bool on);
+#define USE_INVERTER
-#define INVERTER_OFF inverterSet(false)
-#define INVERTER_ON inverterSet(true)
-#else
-#define INVERTER_OFF do {} while(0)
-#define INVERTER_ON do {} while(0)
 #endif
 
-void initInverter(void);
+void initInverters(void);
 
+void enableInverter(USART_TypeDef *USARTx, bool on);

src/main/drivers/serial_uart.c

@@ -36,15 +36,15 @@
 #include "serial_uart_impl.h"
 
 static void usartConfigurePinInversion(uartPort_t *uartPort) {
-#if !defined(INVERTER) && !defined(STM32F303xC)
+#if !defined(USE_INVERTER) && !defined(STM32F303xC)
     UNUSED(uartPort);
 #else
     bool inverted = uartPort->port.options & SERIAL_INVERTED;
 
-#ifdef INVERTER
+#ifdef USE_INVERTER
-    if (inverted && uartPort->USARTx == INVERTER_USART) {
+    if (inverted) {
         // Enable hardware inverter if available.
-        INVERTER_ON;
+        enableInverter(uartPort->USARTx, true);
     }
 #endif
 

src/main/drivers/serial_usb_vcp.c

@@ -204,8 +204,8 @@ serialPort_t *usbVcpOpen(void)
 #else
     Set_System();
     Set_USBClock();
-    USB_Interrupts_Config();
     USB_Init();
+    USB_Interrupts_Config();
 #endif
 
     s = &vcpPort;

src/main/fc/cli.c

@@ -2765,9 +2765,8 @@ static void cliReboot(void)
 static void cliDfu(char *cmdLine)
 {
     UNUSED(cmdLine);
-#ifndef CLI_MINIMAL_VERBOSITY
+
-    cliPrint("\r\nRestarting in DFU mode");
+    cliPrintHashLine("restarting in DFU mode");
-#endif
     cliRebootEx(true);
 }
 
@@ -2775,9 +2774,7 @@ static void cliExit(char *cmdline)
 {
     UNUSED(cmdline);
 
-#ifndef CLI_MINIMAL_VERBOSITY
+    cliPrintHashLine("leaving CLI mode, unsaved changes lost");
-    cliPrint("\r\nLeaving CLI mode, unsaved changes lost.\r\n");
-#endif
     bufWriterFlush(cliWriter);
 
     *cliBuffer = '\0';
@@ -3043,7 +3040,7 @@ static void cliSave(char *cmdline)
 {
     UNUSED(cmdline);
 
-    cliPrint("Saving");
+    cliPrintHashLine("saving");
     writeEEPROM();
     cliReboot();
 }
@@ -3052,7 +3049,7 @@ static void cliDefaults(char *cmdline)
 {
     UNUSED(cmdline);
 
-    cliPrint("Resetting to defaults");
+    cliPrintHashLine("resetting to defaults");
     resetEEPROM();
     cliReboot();
 }
@@ -3561,13 +3558,12 @@ static void printConfig(char *cmdline, bool doDiff)
         cliPrintHashLine("version");
         cliVersion(NULL);
 
-#ifndef CLI_MINIMAL_VERBOSITY
         if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) {
-            cliPrintHashLine("reset configuration to default settings\r\ndefaults");
+            cliPrintHashLine("reset configuration to default settings");
+            cliPrint("defaults\r\n");
         }
 
         cliPrintHashLine("name");
-#endif
         printName(dumpMask);
 
 #ifdef USE_RESOURCE_MGMT
@@ -3653,19 +3649,16 @@ static void printConfig(char *cmdline, bool doDiff)
                 }
 
                 changeControlRateProfile(currentRateIndex);
-#ifndef CLI_MINIMAL_VERBOSITY
                 cliPrintHashLine("restore original rateprofile selection");
                 cliRateProfile("");
-#endif
             }
 
             changeProfile(activeProfile);
-#ifndef CLI_MINIMAL_VERBOSITY
             cliPrintHashLine("restore original profile selection");
             cliProfile("");
 
-            cliPrintHashLine("save configuration\r\nsave");
+            cliPrintHashLine("save configuration");
-#endif
+            cliPrint("save");
         } else {
             cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig);
             cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig);

src/main/fc/fc_core.c

@@ -209,9 +209,10 @@ void processRcCommand(void)
     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
     static int16_t factor, rcInterpolationFactor;
     static uint16_t currentRxRefreshRate;
-    const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 1;
+    const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
     uint16_t rxRefreshRate;
     bool readyToCalculateRate = false;
+    uint8_t readyToCalculateRateAxisCnt = 0;
 
     if (isRXDataNew) {
         currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
@@ -241,7 +242,7 @@ void processRcCommand(void)
                 debug[3] = rxRefreshRate;
             }
 
-            for (int channel=ROLL; channel <= interpolationChannels; channel++) {
+            for (int channel=ROLL; channel < interpolationChannels; channel++) {
                 deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
                 lastCommand[channel] = rcCommand[channel];
             }
@@ -252,24 +253,29 @@ void processRcCommand(void)
         }
 
         // Interpolate steps of rcCommand
+        int channel;
         if (factor > 0) {
-            for (int channel=ROLL; channel <= interpolationChannels; channel++)
+            for (channel=ROLL; channel < interpolationChannels; channel++)
                 rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
         } else {
             factor = 0;
         }
-
+        readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation
         readyToCalculateRate = true;
     } else {
         factor = 0; // reset factor in case of level modes flip flopping
     }
 
     if (readyToCalculateRate || isRXDataNew) {
+        if (isRXDataNew)
+            readyToCalculateRateAxisCnt = FD_YAW;
+
         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
             scaleRcCommandToFpvCamAngle();
 
-        for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]);
+        for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
+            calculateSetpointRate(axis, rcCommand[axis]);
 
         isRXDataNew = false;
     }

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"
 
@@ -308,8 +309,8 @@ void init(void)
     beeperInit(beeperConfig());
 #endif
 /* temp until PGs are implemented. */
-#ifdef INVERTER
+#ifdef USE_INVERTER
-    initInverter();
+    initInverters();
 #endif
 
 #ifdef USE_BST
@@ -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();

src/main/fc/fc_msp.c

@@ -1237,7 +1237,6 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
 static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 {
     uint32_t i;
-    uint16_t tmp;
     uint8_t value;
     const unsigned int dataSize = sbufBytesRemaining(src);
 #ifdef GPS
@@ -1375,10 +1374,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_MISC:
-        tmp = sbufReadU16(src);
+        rxConfig()->midrc = sbufReadU16(src);
-        if (tmp < 1600 && tmp > 1400)
-            rxConfig()->midrc = tmp;
-
         motorConfig()->minthrottle = sbufReadU16(src);
         motorConfig()->maxthrottle = sbufReadU16(src);
         motorConfig()->mincommand = sbufReadU16(src);
@@ -1639,7 +1635,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 
 #ifdef USE_RTC6705
     case MSP_SET_VTX_CONFIG:
-        tmp = sbufReadU16(src);
+        ;
+        uint16_t tmp = sbufReadU16(src);
         if  (tmp < 40)
             masterConfig.vtx_channel = tmp;
         if (current_vtx_channel != masterConfig.vtx_channel) {

src/main/fc/fc_tasks.c

@@ -215,6 +215,9 @@ void taskVtxControl(uint32_t currentTime)
 #ifdef VTX_COMMON
     vtxCommonProcess(currentTime);
 #endif
+#ifdef VTX_TRAMP
+    trampProcess(currentTime);
+#endif
 }
 #endif
 
@@ -300,7 +303,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/fc/fc_tasks.c.orig

@@ -0,0 +1,517 @@
+/*
+ * 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/>.
+ */
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "cms/cms.h"
+
+#include "common/axis.h"
+#include "common/color.h"
+#include "common/utils.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+#include "drivers/compass.h"
+#include "drivers/serial.h"
+#include "drivers/stack_check.h"
+#include "drivers/vtx_common.h"
+
+#include "fc/config.h"
+#include "fc/fc_msp.h"
+#include "fc/fc_tasks.h"
+#include "fc/fc_core.h"
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+#include "fc/cli.h"
+#include "fc/fc_dispatch.h"
+
+#include "flight/pid.h"
+#include "flight/altitudehold.h"
+
+#include "io/beeper.h"
+#include "io/dashboard.h"
+#include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/osd.h"
+#include "io/serial.h"
+#include "io/transponder_ir.h"
+<<<<<<< HEAD
+=======
+#include "io/vtx_smartaudio.h"
+#include "io/vtx_tramp.h"
+>>>>>>> betaflight/master
+
+#include "msp/msp_serial.h"
+
+#include "rx/rx.h"
+
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/battery.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
+#include "sensors/sonar.h"
+#include "sensors/esc_sensor.h"
+
+#include "scheduler/scheduler.h"
+
+#include "telemetry/telemetry.h"
+
+#include "config/feature.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+
+#ifdef USE_BST
+void taskBstMasterProcess(timeUs_t currentTimeUs);
+#endif
+
+#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
+#define TASK_PERIOD_MS(ms) ((ms) * 1000)
+#define TASK_PERIOD_US(us) (us)
+
+/* VBAT monitoring interval (in microseconds) - 1s*/
+#define VBATINTERVAL (6 * 3500)
+/* IBat monitoring interval (in microseconds) - 6 default looptimes */
+#define IBATINTERVAL (6 * 3500)
+
+
+static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    accUpdate(&accelerometerConfig()->accelerometerTrims);
+}
+
+static void taskHandleSerial(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+#ifdef USE_CLI
+    // in cli mode, all serial stuff goes to here. enter cli mode by sending #
+    if (cliMode) {
+        cliProcess();
+        return;
+    }
+#endif
+    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
+}
+
+static void taskUpdateBattery(timeUs_t currentTimeUs)
+{
+#if defined(USE_ADC) || defined(USE_ESC_SENSOR)
+    if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) {
+        static uint32_t vbatLastServiced = 0;
+        if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) {
+            vbatLastServiced = currentTimeUs;
+            updateBattery();
+        }
+    }
+#endif
+
+    if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
+        static uint32_t ibatLastServiced = 0;
+        const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced);
+
+        if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
+            ibatLastServiced = currentTimeUs;
+            updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
+        }
+    }
+}
+
+static void taskUpdateRxMain(timeUs_t currentTimeUs)
+{
+    processRx(currentTimeUs);
+    isRXDataNew = true;
+
+#if !defined(BARO) && !defined(SONAR)
+    // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
+    updateRcCommands();
+#endif
+    updateLEDs();
+
+#ifdef BARO
+    if (sensors(SENSOR_BARO)) {
+        updateAltHoldState();
+    }
+#endif
+
+#ifdef SONAR
+    if (sensors(SENSOR_SONAR)) {
+        updateSonarAltHoldState();
+    }
+#endif
+}
+
+#ifdef MAG
+static void taskUpdateCompass(timeUs_t currentTimeUs)
+{
+    if (sensors(SENSOR_MAG)) {
+        compassUpdate(currentTimeUs, &compassConfig()->magZero);
+    }
+}
+#endif
+
+#ifdef BARO
+static void taskUpdateBaro(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (sensors(SENSOR_BARO)) {
+        const uint32_t newDeadline = baroUpdate();
+        if (newDeadline != 0) {
+            rescheduleTask(TASK_SELF, newDeadline);
+        }
+    }
+}
+#endif
+
+#if defined(BARO) || defined(SONAR)
+static void taskCalculateAltitude(timeUs_t currentTimeUs)
+{
+    if (false
+#if defined(BARO)
+        || (sensors(SENSOR_BARO) && isBaroReady())
+#endif
+#if defined(SONAR)
+        || sensors(SENSOR_SONAR)
+#endif
+        ) {
+        calculateEstimatedAltitude(currentTimeUs);
+    }}
+#endif
+
+#ifdef TELEMETRY
+static void taskTelemetry(timeUs_t currentTimeUs)
+{
+    telemetryCheckState();
+
+    if (!cliMode && feature(FEATURE_TELEMETRY)) {
+        telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
+    }
+}
+#endif
+
+#ifdef VTX_CONTROL
+// Everything that listens to VTX devices
+void taskVtxControl(uint32_t currentTime)
+{
+    if (ARMING_FLAG(ARMED))
+        return;
+
+#ifdef VTX_COMMON
+    vtxCommonProcess(currentTime);
+#endif
+#ifdef VTX_TRAMP
+    trampProcess(currentTime);
+#endif
+}
+#endif
+
+void fcTasksInit(void)
+{
+    schedulerInit();
+    rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
+    setTaskEnabled(TASK_GYROPID, true);
+
+    if (sensors(SENSOR_ACC)) {
+        setTaskEnabled(TASK_ACCEL, true);
+        rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
+    }
+
+    setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
+    setTaskEnabled(TASK_SERIAL, true);
+    rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));
+    setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
+    setTaskEnabled(TASK_RX, true);
+
+    setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled());
+
+#ifdef BEEPER
+    setTaskEnabled(TASK_BEEPER, true);
+#endif
+#ifdef GPS
+    setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
+#endif
+#ifdef MAG
+    setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
+#if defined(USE_SPI) && defined(USE_MAG_AK8963)
+    // fixme temporary solution for AK6983 via slave I2C on MPU9250
+    rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
+#endif
+#endif
+#ifdef BARO
+    setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
+#endif
+#ifdef SONAR
+    setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
+#endif
+#if defined(BARO) || defined(SONAR)
+    setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
+#endif
+#ifdef USE_DASHBOARD
+    setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
+#endif
+#ifdef TELEMETRY
+    setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
+    if (feature(FEATURE_TELEMETRY)) {
+        if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) {
+            // Reschedule telemetry to 500hz for Jeti Exbus
+            rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
+        } else if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
+            // Reschedule telemetry to 500hz, 2ms for CRSF
+            rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
+        }
+    }
+#endif
+#ifdef LED_STRIP
+    setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
+#endif
+#ifdef TRANSPONDER
+    setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
+#endif
+#ifdef OSD
+    setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
+#endif
+#ifdef USE_BST
+    setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
+#endif
+#ifdef USE_ESC_SENSOR
+    setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR));
+#endif
+#ifdef CMS
+#ifdef USE_MSP_DISPLAYPORT
+    setTaskEnabled(TASK_CMS, true);
+#else
+    setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
+#endif
+#endif
+#ifdef STACK_CHECK
+    setTaskEnabled(TASK_STACK_CHECK, true);
+#endif
+#ifdef VTX_CONTROL
+#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
+    setTaskEnabled(TASK_VTXCTRL, true);
+#endif
+#endif
+}
+
+cfTask_t cfTasks[TASK_COUNT] = {
+    [TASK_SYSTEM] = {
+        .taskName = "SYSTEM",
+        .taskFunc = taskSystem,
+        .desiredPeriod = TASK_PERIOD_HZ(10),        // 10Hz, every 100 ms
+        .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
+    },
+
+    [TASK_GYROPID] = {
+        .taskName = "PID",
+        .subTaskName = "GYRO",
+        .taskFunc = taskMainPidLoop,
+        .desiredPeriod = TASK_GYROPID_DESIRED_PERIOD,
+        .staticPriority = TASK_PRIORITY_REALTIME,
+    },
+
+    [TASK_ACCEL] = {
+        .taskName = "ACCEL",
+        .taskFunc = taskUpdateAccelerometer,
+        .desiredPeriod = TASK_PERIOD_HZ(1000),      // 1000Hz, every 1ms
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+
+    [TASK_ATTITUDE] = {
+        .taskName = "ATTITUDE",
+        .taskFunc = imuUpdateAttitude,
+        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+
+    [TASK_RX] = {
+        .taskName = "RX",
+        .checkFunc = rxUpdateCheck,
+        .taskFunc = taskUpdateRxMain,
+        .desiredPeriod = TASK_PERIOD_HZ(50),        // If event-based scheduling doesn't work, fallback to periodic scheduling
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+    [TASK_SERIAL] = {
+        .taskName = "SERIAL",
+        .taskFunc = taskHandleSerial,
+        .desiredPeriod = TASK_PERIOD_HZ(100),       // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+
+    [TASK_DISPATCH] = {
+        .taskName = "DISPATCH",
+        .taskFunc = dispatchProcess,
+        .desiredPeriod = TASK_PERIOD_HZ(1000),
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+    [TASK_BATTERY] = {
+        .taskName = "BATTERY",
+        .taskFunc = taskUpdateBattery,
+        .desiredPeriod = TASK_PERIOD_HZ(50),        // 50 Hz
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+
+#ifdef BEEPER
+    [TASK_BEEPER] = {
+        .taskName = "BEEPER",
+        .taskFunc = beeperUpdate,
+        .desiredPeriod = TASK_PERIOD_HZ(100),       // 100 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef GPS
+    [TASK_GPS] = {
+        .taskName = "GPS",
+        .taskFunc = gpsUpdate,
+        .desiredPeriod = TASK_PERIOD_HZ(10),        // GPS usually don't go raster than 10Hz
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
+#ifdef MAG
+    [TASK_COMPASS] = {
+        .taskName = "COMPASS",
+        .taskFunc = taskUpdateCompass,
+        .desiredPeriod = TASK_PERIOD_HZ(10),        // Compass is updated at 10 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef BARO
+    [TASK_BARO] = {
+        .taskName = "BARO",
+        .taskFunc = taskUpdateBaro,
+        .desiredPeriod = TASK_PERIOD_HZ(20),
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef SONAR
+    [TASK_SONAR] = {
+        .taskName = "SONAR",
+        .taskFunc = sonarUpdate,
+        .desiredPeriod = TASK_PERIOD_MS(70),        // 70ms required so that SONAR pulses do not interfer with each other
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#if defined(BARO) || defined(SONAR)
+    [TASK_ALTITUDE] = {
+        .taskName = "ALTITUDE",
+        .taskFunc = taskCalculateAltitude,
+        .desiredPeriod = TASK_PERIOD_HZ(40),
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef TRANSPONDER
+    [TASK_TRANSPONDER] = {
+        .taskName = "TRANSPONDER",
+        .taskFunc = transponderUpdate,
+        .desiredPeriod = TASK_PERIOD_HZ(250),       // 250 Hz, 4ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef USE_DASHBOARD
+    [TASK_DASHBOARD] = {
+        .taskName = "DASHBOARD",
+        .taskFunc = dashboardUpdate,
+        .desiredPeriod = TASK_PERIOD_HZ(10),
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+#ifdef OSD
+    [TASK_OSD] = {
+        .taskName = "OSD",
+        .taskFunc = osdUpdate,
+        .desiredPeriod = TASK_PERIOD_HZ(60),        // 60 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+#ifdef TELEMETRY
+    [TASK_TELEMETRY] = {
+        .taskName = "TELEMETRY",
+        .taskFunc = taskTelemetry,
+        .desiredPeriod = TASK_PERIOD_HZ(250),       // 250 Hz, 4ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef LED_STRIP
+    [TASK_LEDSTRIP] = {
+        .taskName = "LEDSTRIP",
+        .taskFunc = ledStripUpdate,
+        .desiredPeriod = TASK_PERIOD_HZ(100),       // 100 Hz, 10ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef USE_BST
+    [TASK_BST_MASTER_PROCESS] = {
+        .taskName = "BST_MASTER_PROCESS",
+        .taskFunc = taskBstMasterProcess,
+        .desiredPeriod = TASK_PERIOD_HZ(50),        // 50 Hz, 20ms
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
+#ifdef USE_ESC_SENSOR
+    [TASK_ESC_SENSOR] = {
+        .taskName = "ESC_SENSOR",
+        .taskFunc = escSensorProcess,
+        .desiredPeriod = TASK_PERIOD_HZ(100),       // 100 Hz every 10ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef CMS
+    [TASK_CMS] = {
+        .taskName = "CMS",
+        .taskFunc = cmsHandler,
+        .desiredPeriod = TASK_PERIOD_HZ(60),        // 60 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
+#ifdef STACK_CHECK
+    [TASK_STACK_CHECK] = {
+        .taskName = "STACKCHECK",
+        .taskFunc = taskStackCheck,
+        .desiredPeriod = TASK_PERIOD_HZ(10),          // 10 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
+#ifdef VTX_CONTROL
+    [TASK_VTXCTRL] = {
+        .taskName = "VTXCTRL",
+        .taskFunc = taskVtxControl,
+        .desiredPeriod = TASK_PERIOD_HZ(5),          // 5Hz @200msec
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+};

src/main/flight/pid.h

@@ -21,6 +21,7 @@
 
 #define PID_CONTROLLER_BETAFLIGHT 1
 #define PID_MIXER_SCALING 100.0f
+#define PID_SERVO_MIXER_SCALING 7.0f
 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
 #define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
 #define PIDSUM_LIMIT 0.5f

src/main/flight/servos.c

@@ -341,9 +341,9 @@ STATIC_UNIT_TESTED void servoMixer(void)
         input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
     } else {
         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
-        input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL];
+        input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL] * PID_SERVO_MIXER_SCALING;
-        input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH];
+        input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH] * PID_SERVO_MIXER_SCALING;
-        input[INPUT_STABILIZED_YAW] = axisPIDf[YAW];
+        input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING;
 
         // Reverse yaw servo when inverted in 3D mode
         if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {

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_VTX_SMARTAUDIO      = (1 << 11), // 2048
-    FUNCTION_TELEMETRY_IBUS      = (1 << 12)  // 4096
+    FUNCTION_TELEMETRY_IBUS      = (1 << 12), // 4096
+    FUNCTION_VTX_TRAMP           = (1 << 13), // 8192
 } serialPortFunction_e;
 
 typedef enum {

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 <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_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

src/main/io/vtx_common.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <stdint.h>
+
+#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

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"
@@ -37,6 +37,7 @@
 #include "drivers/vtx_common.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"
@@ -151,18 +152,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;
@@ -688,9 +677,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) {
@@ -907,9 +896,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;
@@ -968,7 +957,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] = ' ';
 
@@ -1006,7 +995,7 @@ static long saCmsConfigBandByGvar(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;
 }
@@ -1031,7 +1020,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;
 }
@@ -1137,7 +1126,15 @@ static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL };
 
 static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChanNames, NULL };
 
-static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saPowerNames};
+static const char * const saCmsPowerNames[] = {
+    "---",
+    "25 ",
+    "200",
+    "500",
+    "800",
+};
+
+static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames};
 
 static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 };
 

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 <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/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

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

src/main/target/ALIENFLIGHTF4/target.h

@@ -33,8 +33,7 @@
 #define BEEPER                  PC13
 #define BEEPER_INVERTED
 
-#define INVERTER                PC15
+#define INVERTER_PIN_USART2     PC15
-#define INVERTER_USART          USART2
 
 // MPU interrupt
 #define USE_EXTI

src/main/target/BLUEJAYF4/target.h

@@ -38,8 +38,8 @@
 #define BEEPER_OPT              PB7
 #define BEEPER_INVERTED
 
-#define INVERTER                PB15
+#define INVERTER_PIN_USART6     PB15
-#define INVERTER_USART          USART6
+//#define INVERTER_PIN_USART1     PC9
 
 #define UART1_INVERTER          PC9
 

src/main/target/CC3D/target.h

@@ -19,8 +19,7 @@
 
 #define LED0                    PB3
 
-#define INVERTER                PB2 // PB2 (BOOT1) used as inverter select GPIO
+#define INVERTER_PIN_USART1     PB2 // PB2 (BOOT1) used as inverter select GPIO
-#define INVERTER_USART          USART1
 
 #define BEEPER                  PA15
 #define BEEPER_OPT              PA2

src/main/target/COLIBRI/target.h

@@ -33,8 +33,7 @@
 
 #define BEEPER                  PC5
 
-#define INVERTER                PB2 // PB2 used as inverter select GPIO
+#define INVERTER_PIN_USART2     PB2 // PB2 used as inverter select GPIO
-#define INVERTER_USART          USART2
 
 #define MPU6000_CS_PIN          PC4
 #define MPU6000_SPI_INSTANCE    SPI1

src/main/target/F4BY/target.h

@@ -28,8 +28,7 @@
 
 #define BEEPER                  PE5
 
-#define INVERTER                PD3
+#define INVERTER_PIN_USART6     PD3
-#define INVERTER_USART          USART6
 
 
 

src/main/target/FISHDRONEF4/target.h

@@ -28,8 +28,7 @@
 #define BEEPER                  PC15
 #define BEEPER_INVERTED
 
-#define INVERTER PC8
+#define INVERTER_PIN_USART6 PC8
-#define INVERTER_USART USART6
 
 #define USE_EXTI
 #define MPU_INT_EXTI            PC4

src/main/target/FURYF4/target.h

@@ -28,8 +28,7 @@
 #define BEEPER                  PA8
 #define BEEPER_INVERTED
 
-#define INVERTER PC0 // PC0 used as inverter select GPIO
+#define INVERTER_PIN_USART1     PC0 // PC0 used as inverter select GPIO
-#define INVERTER_USART USART1
 
 // MPU6000 interrupts
 #define USE_EXTI

src/main/target/KIWIF4/target.h

@@ -28,8 +28,7 @@
 
 #define BEEPER                  PA8
 
-#define INVERTER PC0 // PC0 used as inverter select GPIO
+#define INVERTER_PIN_USART1 PC0 // PC0 used as inverter select GPIO
-#define INVERTER_USART USART1
 
 // MPU6000 interrupts
 #define USE_EXTI

src/main/target/MICROSCISKY/target.h

@@ -27,8 +27,7 @@
 #define BARO_XCLR_PIN           PC13
 #define BARO_EOC_PIN            PC14
 
-#define INVERTER                PB2 // PB2 (BOOT1) abused as inverter select GPIO
+#define INVERTER_PIN_USART2                PB2 // PB2 (BOOT1) abused as inverter select GPIO
-#define INVERTER_USART          USART2
 
 #define USE_EXTI
 #define MAG_INT_EXTI            PC14

src/main/target/NAZE/target.h

@@ -44,8 +44,7 @@
 //#define BARO_XCLR_PIN           PC13
 //#define BARO_EOC_PIN            PC14
 
-#define INVERTER                PB2 // PB2 (BOOT1) abused as inverter select GPIO
+#define INVERTER_PIN_USART2       PB2 // PB2 (BOOT1) abused as inverter select GPIO
-#define INVERTER_USART          USART2
 
 #define USE_EXTI
 #define MAG_INT_EXTI            PC14
@@ -122,16 +121,16 @@
 #define USE_UART2
 /* only 2 uarts available on the NAZE, add ifdef here if present on other boards */
 //#define USE_UART3
-//#define USE_SOFTSERIAL1
+#define USE_SOFTSERIAL1
-//#define USE_SOFTSERIAL2
+#define USE_SOFTSERIAL2
-#define SERIAL_PORT_COUNT       2
+#define SERIAL_PORT_COUNT       4
 
-//#define SOFTSERIAL_1_TIMER TIM3
+#define SOFTSERIAL_1_TIMER TIM3
-//#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
+#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
-//#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
+#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
-//#define SOFTSERIAL_2_TIMER TIM3
+#define SOFTSERIAL_2_TIMER TIM3
-//#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
+#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
-//#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
+#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
 
 #define UART3_RX_PIN            PB11
 #define UART3_TX_PIN            PB10

src/main/target/OMNIBUSF4/target.h

@@ -35,8 +35,7 @@
 #define BEEPER                  PB4
 #define BEEPER_INVERTED
 
-#define INVERTER                PC0 // PC0 used as inverter select GPIO
+#define INVERTER_PIN_USART1     PC0 // PC0 used as inverter select GPIO
-#define INVERTER_USART          USART1
 
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_INSTANCE    SPI1

src/main/target/REVO/target.h

@@ -69,8 +69,7 @@
 #endif
 
 // PC0 used as inverter select GPIO
-#define INVERTER                PC0
+#define INVERTER_PIN_USART1     PC0
-#define INVERTER_USART          USART1
 
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_INSTANCE    SPI1

src/main/target/REVONANO/target.h

@@ -30,8 +30,7 @@
 
 #define BEEPER                  PC13
 
-#define INVERTER                PC15
+#define INVERTER_PIN_USART2     PC15 //Sbus on USART 2 of nano.
-#define INVERTER_USART          USART2 //Sbus on USART 2 of nano.
 
 #define MPU9250_CS_PIN          PB12
 #define MPU9250_SPI_INSTANCE    SPI2

src/main/target/SPARKY2/target.h

@@ -32,8 +32,7 @@
 #define BEEPER                  PC9
 #define BEEPER_INVERTED
 
-#define INVERTER                PC6
+#define INVERTER_PIN_USART6     PC6
-#define INVERTER_USART          USART6
 
 #define USE_ESC_SENSOR
 

src/main/target/VRRACE/target.h

@@ -27,8 +27,7 @@
 #define BEEPER PA0
 #define BEEPER_INVERTED
 
-#define INVERTER PD7
+#define INVERTER_PIN_USART6 PD7
-#define INVERTER_USART USART6
 
 #define MPU6500_CS_PIN        PE10
 #define MPU6500_SPI_INSTANCE  SPI2

src/main/target/YUPIF4/target.h

@@ -29,8 +29,7 @@
 #define BEEPER                  PC9
 //#define BEEPER_INVERTED
 
-#define INVERTER                PB15
+#define INVERTER_PIN_USART6     PB15
-#define INVERTER_USART          USART6
 
 
 // Gyro interrupt

src/main/target/common.h

@@ -106,6 +106,7 @@
 #define VTX_COMMON
 #define VTX_CONTROL
 #define VTX_SMARTAUDIO
+#define VTX_TRAMP
 #define USE_SENSOR_NAMES
 #else
 #define SKIP_CLI_COMMAND_HELP

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

src/main/telemetry/smartport.c

@@ -322,14 +322,14 @@ void freeSmartPortTelemetryPort(void)
 
 void configureSmartPortTelemetryPort(void)
 {
-    portOptions_t portOptions;
-
     if (!portConfig) {
         return;
     }
 
+    portOptions_t portOptions = 0;
+    
     if (telemetryConfig->sportHalfDuplex) {
-        portOptions = SERIAL_BIDIR;
+        portOptions |= SERIAL_BIDIR;
     }
     
     if (telemetryConfig->telemetry_inversion) {