Merge branch 'development' into de_rx_and_failsafe_cleanup

Makefile

@@ -661,6 +661,7 @@ HIGHEND_SRC = \
             drivers/rangefinder_hcsr04_i2c.c \
             drivers/rangefinder_srf10.c \
             drivers/rangefinder_vl53l0x.c \
+            drivers/vtx_common.c \
             io/dashboard.c \
             io/displayport_max7456.c \
             io/displayport_msp.c \
@@ -689,7 +690,11 @@ HIGHEND_SRC = \
             telemetry/ltm.c \
             telemetry/mavlink.c \
             telemetry/smartport.c \
-            telemetry/telemetry.c
+            telemetry/telemetry.c \
+            io/vtx_string.c \
+            io/vtx_smartaudio.c \
+            io/vtx_tramp.c \
+            io/vtx_control.c
 
 ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
 VCP_SRC = \

src/main/cms/cms.c

@@ -630,11 +630,11 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
 
     cmsInMenu = false;
 
+    displayClearScreen(pDisplay);
     displayRelease(pDisplay);
     currentCtx.menu = NULL;
 
     if (exitType == CMS_EXIT_SAVEREBOOT) {
-        displayClearScreen(pDisplay);
         displayWrite(pDisplay, 5, 3, "REBOOTING...");
 
         displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
@@ -722,10 +722,9 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
             }
             break;
 
-        case OME_Funcall:;
-            long retval;
+        case OME_Funcall:
             if (p->func && key == KEY_RIGHT) {
-                retval = p->func(pDisplay, p->data);
+                long retval = p->func(pDisplay, p->data);
                 if (retval == MENU_CHAIN_BACK)
                     cmsMenuBack(pDisplay);
                 res = BUTTON_PAUSE;

src/main/cms/cms_menu_builtin.c

@@ -45,6 +45,10 @@
 #include "cms/cms_menu_ledstrip.h"
 #include "cms/cms_menu_misc.h"
 
+// User supplied menus
+
+#include "io/vtx_smartaudio_cms.h"
+#include "io/vtx_tramp.h"
 
 // Info
 
@@ -93,6 +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 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/common/bitarray.c

@@ -20,19 +20,55 @@
 
 #include "bitarray.h"
 
-#define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op (1 << ((bit) % (sizeof((array)[0]) * 8))))
+// bit[0] in an element must be the MSB to allow using clz
+// to find set bits faster.
+#define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op ((1u<<31) >> ((bit) % (sizeof((array)[0]) * 8))))
 
-bool bitArrayGet(const void *array, unsigned bit)
+bool bitArrayGet(const bitarrayElement_t *array, unsigned bit)
 {
     return BITARRAY_BIT_OP((uint32_t*)array, bit, &);
 }
 
-void bitArraySet(void *array, unsigned bit)
+void bitArraySet(bitarrayElement_t *array, unsigned bit)
 {
     BITARRAY_BIT_OP((uint32_t*)array, bit, |=);
 }
 
-void bitArrayClr(void *array, unsigned bit)
+void bitArrayClr(bitarrayElement_t *array, unsigned bit)
 {
     BITARRAY_BIT_OP((uint32_t*)array, bit, &=~);
 }
+
+__attribute__((always_inline)) static inline uint8_t __CLZ(uint32_t val)
+{
+#ifdef __arm__
+    uint8_t lz;
+    __asm__ volatile ("clz %0, %1" : "=r" (lz) : "r" (val) );
+    return lz;
+#else
+    // __builtin_clz is not defined for zero, since it's ARCH
+    // dependant. Make it return 32 like ARM's CLZ.
+    return val ? __builtin_clz(val) : 32;
+#endif
+}
+
+int bitArrayFindFirstSet(const bitarrayElement_t *array, unsigned start, size_t size)
+{
+    const uint32_t *ptr = (uint32_t*)array;
+    const uint32_t *end = ptr + (size / 4);
+    const uint32_t *p = ptr + start / (8 * 4);
+    int ret;
+    // First iteration might need to mask some bits
+    uint32_t mask = 0xFFFFFFFF >> (start % (8 * 4));
+    if ((ret = __CLZ(*p & mask)) != 32) {
+        return ret;
+    }
+    p++;
+    while (p < end) {
+        if ((ret = __CLZ(*p)) != 32) {
+            return (((char *)p) - ((char *)ptr)) * 8 + ret;
+        }
+        p++;
+    }
+    return -1;
+}

src/main/common/bitarray.h

@@ -15,6 +15,29 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-bool bitArrayGet(const void *array, unsigned bit);
-void bitArraySet(void *array, unsigned bit);
-void bitArrayClr(void *array, unsigned bit);
+#pragma once
+
+#include <stdbool.h>
+#include <stddef.h>
+
+/*
+ * These functions expect array to be 4-byte aligned since they alias array
+ * to an uint32_t pointer in order to work fast. Use the BITARRAY_DECLARE()
+ * macro to declare a bit array that can be safely used by them.
+ */
+
+typedef uint32_t bitarrayElement_t;
+
+#define BITARRAY_DECLARE(name, bits) bitarrayElement_t name[(bits + 31) / 32]
+
+bool bitArrayGet(const bitarrayElement_t *array, unsigned bit);
+void bitArraySet(bitarrayElement_t *array, unsigned bit);
+void bitArrayClr(bitarrayElement_t *array, unsigned bit);
+
+// Returns the first set bit with pos >= start_bit, or -1 if all bits
+// are zero. Note that size must indicate the size of array in bytes.
+// In most cases, you should use the BITARRAY_FIND_FIRST_SET() macro
+// to call this function.
+int bitArrayFindFirstSet(const bitarrayElement_t *array, unsigned start_bit, size_t size);
+
+#define BITARRAY_FIND_FIRST_SET(array, start_bit) bitArrayFindFirstSet(array, start_bit, sizeof(array))

src/main/common/time.c

@@ -120,11 +120,16 @@ static bool rtcIsDateTimeValid(dateTime_t *dateTime)
            (dateTime->millis <= 999);
 }
 
+static void dateTimeWithOffset(dateTime_t *dateTimeOffset, dateTime_t *dateTimeInitial, uint16_t minutes)
+{
+    rtcTime_t initialTime = dateTimeToRtcTime(dateTimeInitial);
+    rtcTime_t offsetTime = rtcTimeMake(rtcTimeGetSeconds(&initialTime) + minutes * 60, rtcTimeGetMillis(&initialTime));
+    rtcTimeToDateTime(dateTimeOffset, offsetTime);
+}
+
 static bool dateTimeFormat(char *buf, dateTime_t *dateTime, int16_t offset)
 {
     dateTime_t local;
-    rtcTime_t utcTime;
-    rtcTime_t localTime;
 
     int tz_hours = 0;
     int tz_minutes = 0;
@@ -134,9 +139,7 @@ static bool dateTimeFormat(char *buf, dateTime_t *dateTime, int16_t offset)
     if (offset != 0) {
         tz_hours = offset / 60;
         tz_minutes = ABS(offset % 60);
-        utcTime = dateTimeToRtcTime(dateTime);
-        localTime = rtcTimeMake(rtcTimeGetSeconds(&utcTime) + offset * 60, rtcTimeGetMillis(&utcTime));
-        rtcTimeToDateTime(&local, localTime);
+        dateTimeWithOffset(&local, dateTime, offset);
         dateTime = &local;
     }
 
@@ -146,6 +149,8 @@ static bool dateTimeFormat(char *buf, dateTime_t *dateTime, int16_t offset)
         retVal = false;
     }
 
+    // XXX: Changes to this format might require updates in
+    // dateTimeSplitFormatted()
     tfp_sprintf(buf, "%04u-%02u-%02uT%02u:%02u:%02u.%03u%c%02d:%02d",
         dateTime->year, dateTime->month, dateTime->day,
         dateTime->hours, dateTime->minutes, dateTime->seconds, dateTime->millis,
@@ -179,6 +184,26 @@ bool dateTimeFormatLocal(char *buf, dateTime_t *dt)
     return dateTimeFormat(buf, dt, timeConfig()->tz_offset);
 }
 
+void dateTimeUTCToLocal(dateTime_t *utcDateTime, dateTime_t *localDateTime)
+{
+    dateTimeWithOffset(localDateTime, utcDateTime, timeConfig()->tz_offset);
+}
+
+bool dateTimeSplitFormatted(char *formatted, char **date, char **time)
+{
+    // Just look for the T and replace it with a zero
+    // XXX: Keep in sync with dateTimeFormat()
+    for (char *p = formatted; *p; p++) {
+        if (*p == 'T') {
+            *date = formatted;
+            *time = (p+1);
+            *p = '\0';
+            return true;
+        }
+    }
+    return false;
+}
+
 bool rtcHasTime()
 {
     return started != 0;

src/main/common/time.h

@@ -75,6 +75,12 @@ typedef struct _dateTime_s {
 bool dateTimeFormatUTC(char *buf, dateTime_t *dt);
 bool dateTimeFormatLocal(char *buf, dateTime_t *dt);
 
+void dateTimeUTCToLocal(dateTime_t *utcDateTime, dateTime_t *localDateTime);
+// dateTimeSplitFormatted splits a formatted date into its date
+// and time parts. Note that the string pointed by formatted will
+// be modifed and will become invalid after calling this function.
+bool dateTimeSplitFormatted(char *formatted, char **date, char **time);
+
 bool rtcHasTime();
 
 bool rtcGet(rtcTime_t *t);

src/main/common/utils.h

@@ -96,3 +96,5 @@ void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("me
 #else
 #define FALLTHROUGH do {} while(0)
 #endif
+
+#define ALIGNED(x) __attribute__ ((aligned(x)))

src/main/config/parameter_group_ids.h

@@ -72,7 +72,7 @@
 //#define PG_DEBUG_CONFIG 51
 #define PG_SERVO_CONFIG 52
 //#define PG_IBUS_TELEMETRY_CONFIG 53
-//#define PG_VTX_CONFIG 54
+#define PG_VTX_CONFIG 54
 #define PG_ELERES_CONFIG 55
 #define PG_CF_END 56
 

src/main/drivers/accgyro/accgyro_l3gd20.c

@@ -81,7 +81,7 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
 
     DISABLE_L3GD20;
 
-    spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
+    spiSetSpeed(L3GD20_SPI, SPI_CLOCK_STANDARD);
 }
 
 void l3gd20GyroInit(gyroDev_t *gyro)

src/main/drivers/accgyro/accgyro_spi_mpu6000.c

@@ -131,13 +131,13 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
 
     mpu6000AccAndGyroInit(gyro);
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
+    spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
     // Accel and Gyro DLPF Setting
     mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
     delayMicroseconds(1);
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);  // 18 MHz SPI clock
+    spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);  // 18 MHz SPI clock
 
     mpuGyroRead(gyro);
 
@@ -159,7 +159,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
     IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
+    spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
     mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
 
@@ -205,7 +205,7 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
         return;
     }
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
+    spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
     // Device Reset
     mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
@@ -246,7 +246,7 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
     delayMicroseconds(15);
 #endif
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
     delayMicroseconds(1);
 
     mpuSpi6000InitDone = true;

src/main/drivers/accgyro/accgyro_spi_mpu6500.c

@@ -75,7 +75,7 @@ static void mpu6500SpiInit(const busDevice_t *bus)
     IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
 
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetSpeed(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
 
     hardwareInitialised = true;
 }
@@ -133,7 +133,7 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
 
 void mpu6500SpiGyroInit(gyroDev_t *gyro)
 {
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
+    spiSetSpeed(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
     delayMicroseconds(1);
 
     mpu6500GyroInit(gyro);
@@ -142,7 +142,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
     mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
     delay(100);
 
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetSpeed(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
     delayMicroseconds(1);
 }
 

src/main/drivers/accgyro/accgyro_spi_mpu9250.c

@@ -122,7 +122,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
         return;
     }
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
+    spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
 
     mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
     delay(50);
@@ -150,7 +150,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
     verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
 #endif
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
 
     mpuSpi9250InitDone = true; //init done
 }
@@ -161,7 +161,7 @@ bool mpu9250SpiDetect(const busDevice_t *bus)
     IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
+    spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
     mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 
     uint8_t attemptsRemaining = 20;
@@ -177,7 +177,7 @@ bool mpu9250SpiDetect(const busDevice_t *bus)
         }
     } while (attemptsRemaining--);
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
 
     return true;
 }
@@ -207,7 +207,7 @@ static void mpu9250SpiGyroInit(gyroDev_t *gyro)
 
     spiResetErrorCounter(MPU9250_SPI_INSTANCE);
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
+    spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
 
     mpuGyroRead(gyro);
 

src/main/drivers/barometer/barometer_spi_bmp280.c

@@ -69,7 +69,7 @@ void bmp280SpiInit(void)
 
     DISABLE_BMP280;
 
-    spiSetDivisor(BMP280_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    spiSetSpeed(BMP280_SPI_INSTANCE, SPI_CLOCK_STANDARD);
 
     hardwareInitialised = true;
 }

src/main/drivers/barometer/barometer_spi_ms56xx.c

@@ -67,7 +67,7 @@ void ms56xxSpiInit(void)
 
     DISABLE_MS56XX(spiCsnPin);
 
-    spiSetDivisor(MS56XX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    spiSetSpeed(MS56XX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
 
     hardwareInitialised = true;
 }

src/main/drivers/bus_spi.c

@@ -72,16 +72,50 @@
 #define SPI3_NSS_PIN NONE
 #endif
 
+#if defined(STM32F1) || defined(STM32F3)
+static const uint16_t spiDivisorMapFast[] = { 
+    SPI_BaudRatePrescaler_256,    // SPI_CLOCK_INITIALIZATON      281.25 KBits/s
+    SPI_BaudRatePrescaler_128,    // SPI_CLOCK_SLOW               562.5 KBits/s
+    SPI_BaudRatePrescaler_8,      // SPI_CLOCK_STANDARD           9.0 MBits/s
+    SPI_BaudRatePrescaler_4,      // SPI_CLOCK_FAST               18.0 MBits/s
+    SPI_BaudRatePrescaler_4       // SPI_CLOCK_ULTRAFAST          18.0 MBits/s
+};
+static const uint16_t spiDivisorMapSlow[] = { 
+    SPI_BaudRatePrescaler_256,    // SPI_CLOCK_INITIALIZATON      140.625 KBits/s
+    SPI_BaudRatePrescaler_64,     // SPI_CLOCK_SLOW               562.5 KBits/s
+    SPI_BaudRatePrescaler_4,      // SPI_CLOCK_STANDARD           9.0 MBits/s
+    SPI_BaudRatePrescaler_2,      // SPI_CLOCK_FAST               18.0 MBits/s
+    SPI_BaudRatePrescaler_2       // SPI_CLOCK_ULTRAFAST          18.0 MBits/s
+};
+#elif defined(STM32F4)
+static const uint16_t spiDivisorMapFast[] = { 
+    SPI_BaudRatePrescaler_256,    // SPI_CLOCK_INITIALIZATON      328.125 KBits/s
+    SPI_BaudRatePrescaler_128,    // SPI_CLOCK_SLOW               656.25 KBits/s
+    SPI_BaudRatePrescaler_8,      // SPI_CLOCK_STANDARD           10.5 MBits/s
+    SPI_BaudRatePrescaler_4,      // SPI_CLOCK_FAST               21.0 MBits/s
+    SPI_BaudRatePrescaler_2       // SPI_CLOCK_ULTRAFAST          42.0 MBits/s
+};
+static const uint16_t spiDivisorMapSlow[] = { 
+    SPI_BaudRatePrescaler_256,    // SPI_CLOCK_INITIALIZATON      164.062 KBits/s
+    SPI_BaudRatePrescaler_64,     // SPI_CLOCK_SLOW               656.25 KBits/s
+    SPI_BaudRatePrescaler_4,      // SPI_CLOCK_STANDARD           10.5 MBits/s
+    SPI_BaudRatePrescaler_2,      // SPI_CLOCK_FAST               21.0 MBits/s
+    SPI_BaudRatePrescaler_2       // SPI_CLOCK_ULTRAFAST          21.0 MBits/s
+};
+#else
+#error "Invalid CPU"
+#endif
+
 static spiDevice_t spiHardwareMap[] = {
 #if defined(STM32F1)
-    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false },
-    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false },
+    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false, .divisorMap = spiDivisorMapFast },
+    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false, .divisorMap = spiDivisorMapSlow },
 #else
-    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false },
-    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false },
+    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false, .divisorMap = spiDivisorMapFast },
+    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false, .divisorMap = spiDivisorMapSlow },
 #endif
 #if defined(STM32F3) || defined(STM32F4)
-    { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false }
+    { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false, .divisorMap = spiDivisorMapSlow }
 #endif
 };
 
@@ -300,56 +334,19 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
     return true;
 }
 
-void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
+void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed)
 {
 #define BR_CLEAR_MASK 0xFFC7
+    SPIDevice device = spiDeviceByInstance(instance);
+    if (device == SPIINVALID) {
+        return;
+    }
 
     SPI_Cmd(instance, DISABLE);
 
     uint16_t tempRegister = instance->CR1;
-
-    switch (divisor) {
-    case 2:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_2;
-        break;
-
-    case 4:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_4;
-        break;
-
-    case 8:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_8;
-        break;
-
-    case 16:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_16;
-        break;
-
-    case 32:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_32;
-        break;
-
-    case 64:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_64;
-        break;
-
-    case 128:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_128;
-        break;
-
-    case 256:
-        tempRegister &= BR_CLEAR_MASK;
-        tempRegister |= SPI_BaudRatePrescaler_256;
-        break;
-    }
-
+    tempRegister &= BR_CLEAR_MASK;
+    tempRegister |= spiHardwareMap[device].divisorMap[speed];
     instance->CR1 = tempRegister;
 
     SPI_Cmd(instance, ENABLE);

src/main/drivers/bus_spi.h

@@ -41,25 +41,14 @@
 /*
   Flash M25p16 tolerates 20mhz, SPI_CLOCK_FAST should sit around 20 or less.
 */
+
 typedef enum {
-    SPI_CLOCK_INITIALIZATON = 256,
-#if defined(STM32F4)
-    SPI_CLOCK_SLOW          = 128, //00.65625 MHz
-    SPI_CLOCK_STANDARD      = 8,   //10.50000 MHz
-    SPI_CLOCK_FAST          = 4,   //21.00000 MHz
-    SPI_CLOCK_ULTRAFAST     = 2    //42.00000 MHz
-#elif defined(STM32F7)
-    SPI_CLOCK_SLOW          = 256, //00.42188 MHz
-    SPI_CLOCK_STANDARD      = 16,  //06.57500 MHz
-    SPI_CLOCK_FAST          = 4,   //27.00000 MHz
-    SPI_CLOCK_ULTRAFAST     = 2    //54.00000 MHz
-#else
-    SPI_CLOCK_SLOW          = 128, //00.56250 MHz
-    SPI_CLOCK_STANDARD      = 4,   //09.00000 MHz
-    SPI_CLOCK_FAST          = 2,   //18.00000 MHz
-    SPI_CLOCK_ULTRAFAST     = 2    //18.00000 MHz
-#endif
-} SPIClockDivider_e;
+    SPI_CLOCK_INITIALIZATON = 0,    // Lowest possible
+    SPI_CLOCK_SLOW          = 1,    // ~1 MHz    
+    SPI_CLOCK_STANDARD      = 2,    // ~10MHz
+    SPI_CLOCK_FAST          = 3,    // ~20MHz
+    SPI_CLOCK_ULTRAFAST     = 4     // Highest possible
+} SPIClockSpeed_e;
 
 typedef enum SPIDevice {
     SPIINVALID = -1,
@@ -94,10 +83,11 @@ typedef struct SPIDevice_s {
     DMA_HandleTypeDef hdma;
     uint8_t dmaIrqHandler;
 #endif
+    const uint16_t * divisorMap;
 } spiDevice_t;
 
 bool spiInit(SPIDevice device);
-void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor);
+void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed);
 uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in);
 bool spiIsBusBusy(SPI_TypeDef *instance);
 

src/main/drivers/bus_spi_hal.c

@@ -68,12 +68,26 @@
 #define SPI4_NSS_PIN NONE
 #endif
 
+static const uint16_t spiDivisorMapFast[] = { 
+    SPI_BAUDRATEPRESCALER_256,    // SPI_CLOCK_INITIALIZATON      421.875 KBits/s
+    SPI_BAUDRATEPRESCALER_128,    // SPI_CLOCK_SLOW               843.75 KBits/s
+    SPI_BAUDRATEPRESCALER_16,     // SPI_CLOCK_STANDARD           6.75 MBits/s
+    SPI_BAUDRATEPRESCALER_4,      // SPI_CLOCK_FAST               27.0 MBits/s
+    SPI_BAUDRATEPRESCALER_2       // SPI_CLOCK_ULTRAFAST          54.0 MBits/s
+};
+static const uint16_t spiDivisorMapSlow[] = { 
+    SPI_BAUDRATEPRESCALER_256,    // SPI_CLOCK_INITIALIZATON      210.937 KBits/s
+    SPI_BAUDRATEPRESCALER_64,     // SPI_CLOCK_SLOW               843.75 KBits/s
+    SPI_BAUDRATEPRESCALER_8,      // SPI_CLOCK_STANDARD           6.75 MBits/s
+    SPI_BAUDRATEPRESCALER_2,      // SPI_CLOCK_FAST               27.0 MBits/s
+    SPI_BAUDRATEPRESCALER_2       // SPI_CLOCK_ULTRAFAST          27.0 MBits/s
+};
 
 static spiDevice_t spiHardwareMap[] = {
-    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER },
-    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER },
-    { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
-    { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER }
+    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER, .divisorMap = spiDivisorMapFast },
+    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER, .divisorMap = spiDivisorMapSlow },
+    { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER, .divisorMap = spiDivisorMapSlow },
+    { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER, .divisorMap = spiDivisorMapSlow }
 };
 
 SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
@@ -288,50 +302,14 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
 }
 
 
-void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
+void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed)
 {
     SPIDevice device = spiDeviceByInstance(instance);
-    if (HAL_SPI_DeInit(&spiHardwareMap[device].hspi) == HAL_OK)
-    {
-    }
+    HAL_SPI_DeInit(&spiHardwareMap[device].hspi);
 
-    switch (divisor) {
-    case 2:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
-        break;
+    spiHardwareMap[device].hspi.Init.BaudRatePrescaler = spiHardwareMap[device].divisorMap[speed];
 
-    case 4:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
-        break;
-
-    case 8:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
-        break;
-
-    case 16:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
-        break;
-
-    case 32:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
-        break;
-
-    case 64:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64;
-        break;
-
-    case 128:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128;
-        break;
-
-    case 256:
-        spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
-        break;
-    }
-
-    if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK)
-    {
-    }
+    HAL_SPI_Init(&spiHardwareMap[device].hspi);
 }
 
 uint16_t spiGetErrorCounter(SPI_TypeDef *instance)

src/main/drivers/flash_m25p16.c

@@ -234,7 +234,7 @@ bool m25p16_init(ioTag_t csTag)
 
 #ifndef M25P16_SPI_SHARED
     //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
-    spiSetDivisor(M25P16_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetSpeed(M25P16_SPI_INSTANCE, SPI_CLOCK_FAST);
 #endif
 
     return m25p16_readIdentification();

src/main/drivers/max7456.c

@@ -18,14 +18,15 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
-#include <stdlib.h>
 #include <string.h>
 
 #include "platform.h"
 
 #ifdef USE_MAX7456
 
+#include "common/bitarray.h"
 #include "common/printf.h"
+#include "common/utils.h"
 
 #include "drivers/bus_spi.h"
 #include "drivers/light_led.h"
@@ -150,37 +151,50 @@
 #define NVM_RAM_SIZE            54
 #define WRITE_NVR               0xA0
 
-#define CHARS_PER_LINE      30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
+#define CHARS_PER_LINE          30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
+#define CHAR_BLANK              0x20
 
 // On shared SPI buss we want to change clock for OSD chip and restore for other devices.
 
 #ifdef MAX7456_SPI_CLK
-    #define ENABLE_MAX7456        {spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK);IOLo(max7456CsPin);}
+    #define ENABLE_MAX7456()    do { \
+                                    spiSetSpeed(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK); \
+                                    IOLo(max7456CsPin); \
+                                } while(0)
 #else
-    #define ENABLE_MAX7456        IOLo(max7456CsPin)
+    #define ENABLE_MAX7456()    do { \
+                                    IOLo(max7456CsPin); \
+                                } while(0)
 #endif
 
 #ifdef MAX7456_RESTORE_CLK
-    #define DISABLE_MAX7456       {IOHi(max7456CsPin);spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_RESTORE_CLK);}
+    #define DISABLE_MAX7456()   do { \
+                                    IOHi(max7456CsPin); \
+                                    spiSetSpeed(MAX7456_SPI_INSTANCE, MAX7456_RESTORE_CLK); \
+                                } while(0)
 #else
-    #define DISABLE_MAX7456       IOHi(max7456CsPin)
+    #define DISABLE_MAX7456()   do { \
+                                    IOHi(max7456CsPin); \
+                                } while(0)
 #endif
 
 uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
 
-// we write everything in screenBuffer and then comapre
-// screenBuffer with shadowBuffer to upgrade only changed chars
-// this solution is faster then redraw all screen
-static uint8_t screenBuffer[VIDEO_BUFFER_CHARS_PAL+40]; //for faster writes we use memcpy so we need some space to don't overwrite buffer
-static uint8_t shadowBuffer[VIDEO_BUFFER_CHARS_PAL];
+// we write everything in screenBuffer and set a dirty bit
+// in screenIsDirty to upgrade only changed chars this solution
+// is faster than redrawing the whole screen on each frame
+static uint8_t screenBuffer[VIDEO_BUFFER_CHARS_PAL] ALIGNED(4);
+static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL);
 
 //max chars to update in one idle
-#define MAX_CHARS2UPDATE    50
+#define MAX_CHARS2UPDATE        10
+#define BYTES_PER_CHAR2UPDATE   6 // 3 spi regs + 3 values
 #ifdef MAX7456_DMA_CHANNEL_TX
 volatile bool dmaTransactionInProgress = false;
 #endif
 
-static uint8_t spiBuff[MAX_CHARS2UPDATE*6];
+#define SPI_BUFF_SIZE (MAX_CHARS2UPDATE*BYTES_PER_CHAR2UPDATE)
+static uint8_t spiBuff[SPI_BUFF_SIZE];
 
 static uint8_t  videoSignalCfg   = 0;
 static uint8_t  videoSignalReg   = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default
@@ -256,7 +270,7 @@ static void max7456SendDma(void* tx_buffer, void* rx_buffer, uint16_t buffer_siz
 #endif
 
     // Enable SPI TX/RX request
-    ENABLE_MAX7456;
+    ENABLE_MAX7456();
     dmaTransactionInProgress = true;
 
     SPI_I2S_DMACmd(MAX7456_SPI_INSTANCE,
@@ -292,7 +306,7 @@ void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor)
 #endif
                 SPI_I2S_DMAReq_Tx, DISABLE);
 
-        DISABLE_MAX7456;
+        DISABLE_MAX7456();
         dmaTransactionInProgress = false;
     }
 
@@ -315,7 +329,8 @@ uint8_t max7456GetRowsCount(void)
 }
 
 //because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup
-//and in case of restart we need to reinitialize chip
+//and in case of restart we need to reinitialize chip. Note that we can't touch screenBuffer here, since
+//it might already have some data by the first time this function is called.
 void max7456ReInit(void)
 {
     uint8_t maxScreenRows;
@@ -327,7 +342,7 @@ void max7456ReInit(void)
     if (millis() < 1500)
         return;
 
-    ENABLE_MAX7456;
+    ENABLE_MAX7456();
 
     switch (videoSignalCfg) {
         case PAL:
@@ -358,10 +373,10 @@ void max7456ReInit(void)
     // make sure the Max7456 is enabled
     max7456Send(MAX7456ADD_VM0, videoSignalReg);
     max7456Send(MAX7456ADD_DMM, CLEAR_DISPLAY);
-    DISABLE_MAX7456;
+    DISABLE_MAX7456();
 
-    //clear shadow to force redraw all screen in non-dma mode
-    memset(shadowBuffer, 0, maxScreenSize);
+    // force redrawing all screen in non-dma mode
+    memset(screenIsDirty, 0xFF, sizeof(screenIsDirty));
     if (firstInit)
     {
         max7456RefreshAll();
@@ -379,26 +394,30 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
     IOInit(max7456CsPin, OWNER_OSD, RESOURCE_SPI_CS, 0);
     IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);
 
-    spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    spiSetSpeed(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
     // force soft reset on Max7456
-    ENABLE_MAX7456;
+    ENABLE_MAX7456();
     max7456Send(MAX7456ADD_VM0, MAX7456_RESET);
-    DISABLE_MAX7456;
+    DISABLE_MAX7456();
     videoSignalCfg = pVcdProfile->video_system;
 
+    // Set screenbuffer to all blanks
+    memset(screenBuffer, CHAR_BLANK, sizeof(screenBuffer));
+
 #ifdef MAX7456_DMA_CHANNEL_TX
     dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0);
 #endif
     //real init will be made letter when driver idle detect
 }
 
-//just fill with spaces with some tricks
 void max7456ClearScreen(void)
 {
-    uint16_t x;
-    uint32_t *p = (uint32_t*)&screenBuffer[0];
-    for (x = 0; x < VIDEO_BUFFER_CHARS_PAL/4; x++)
-        p[x] = 0x20202020;
+    for (uint_fast16_t ii = 0; ii < sizeof(screenBuffer); ii++) {
+        if (screenBuffer[ii] != CHAR_BLANK) {
+            screenBuffer[ii] = CHAR_BLANK;
+            bitArraySet(screenIsDirty, ii);
+        }
+    }
 }
 
 uint8_t* max7456GetScreenBuffer(void) {
@@ -407,15 +426,29 @@ uint8_t* max7456GetScreenBuffer(void) {
 
 void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c)
 {
-    screenBuffer[y*30+x] = c;
+    unsigned pos = y * CHARS_PER_LINE + x;
+    if (screenBuffer[pos] != c) {
+        screenBuffer[pos] = c;
+        bitArraySet(screenIsDirty, pos);
+    }
 }
 
 void max7456Write(uint8_t x, uint8_t y, const char *buff)
 {
     uint8_t i = 0;
-    for (i = 0; *(buff+i); i++)
-        if (x+i < 30) //do not write over screen
-            screenBuffer[y*30+x+i] = *(buff+i);
+    uint8_t c;
+    unsigned pos = y * CHARS_PER_LINE + x;
+    for (i = 0; *buff; i++, buff++, pos++) {
+        //do not write past screen's end of line
+        if (x + i >= CHARS_PER_LINE) {
+            break;
+        }
+        c = *buff;
+        if (screenBuffer[pos] != c) {
+            screenBuffer[pos] = c;
+            bitArraySet(screenIsDirty, pos);
+        }
+    }
 }
 
 bool max7456DmaInProgress(void)
@@ -429,21 +462,22 @@ bool max7456DmaInProgress(void)
 
 void max7456DrawScreenPartial(void)
 {
+    static uint32_t lastSigCheckMs = 0;
+    static uint32_t videoDetectTimeMs = 0;
+
     uint8_t stallCheck;
     uint8_t videoSense;
-    static uint32_t lastSigCheckMs = 0;
     uint32_t nowMs;
-    static uint32_t videoDetectTimeMs = 0;
-    static uint16_t pos = 0;
-    int k = 0, buff_len=0;
+    int pos;
+    int buff_len = 0;
 
     if (!max7456Lock && !fontIsLoading) {
         // (Re)Initialize MAX7456 at startup or stall is detected.
-        
+
         max7456Lock = true;
-        ENABLE_MAX7456;
+        ENABLE_MAX7456();
         stallCheck = max7456Send(MAX7456ADD_VM0|MAX7456ADD_READ, 0x00);
-        DISABLE_MAX7456;
+        DISABLE_MAX7456();
 
         nowMs = millis();
 
@@ -454,9 +488,9 @@ void max7456DrawScreenPartial(void)
 
             // Adjust output format based on the current input format.
 
-            ENABLE_MAX7456;
+            ENABLE_MAX7456();
             videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
-            DISABLE_MAX7456;
+            DISABLE_MAX7456();
 
             if (videoSense & STAT_LOS) {
                 videoDetectTimeMs = 0;
@@ -479,22 +513,28 @@ void max7456DrawScreenPartial(void)
 
         //------------   end of (re)init-------------------------------------
 
-        for (k=0; k< MAX_CHARS2UPDATE; k++) {
-            if (screenBuffer[pos] != shadowBuffer[pos]) {
-                spiBuff[buff_len++] = MAX7456ADD_DMAH;
-                spiBuff[buff_len++] = pos >> 8;
-                spiBuff[buff_len++] = MAX7456ADD_DMAL;
-                spiBuff[buff_len++] = pos & 0xff;
-                spiBuff[buff_len++] = MAX7456ADD_DMDI;
-                spiBuff[buff_len++] = screenBuffer[pos];
-                shadowBuffer[pos] = screenBuffer[pos];
-                k++;
-            }
-
-            if (++pos >= maxScreenSize) {
-                pos = 0;
+        for (pos = 0;;) {
+            pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos);
+            if (pos < 0 || pos >= maxScreenSize) {
+                // No more dirty chars.
                 break;
             }
+            // Found one dirty character to send
+            spiBuff[buff_len++] = MAX7456ADD_DMAH;
+            spiBuff[buff_len++] = pos >> 8;
+            spiBuff[buff_len++] = MAX7456ADD_DMAL;
+            spiBuff[buff_len++] = pos & 0xff;
+            spiBuff[buff_len++] = MAX7456ADD_DMDI;
+            spiBuff[buff_len++] = screenBuffer[pos];
+            bitArrayClr(screenIsDirty, pos);
+            if (buff_len == SPI_BUFF_SIZE) {
+                // If the buffer is full, don't increment
+                // pos since we won't check the buffer
+                // until the next iteration.
+                break;
+            }
+            // Start next search at next bit
+            pos++;
         }
 
         if (buff_len) {
@@ -502,10 +542,9 @@ void max7456DrawScreenPartial(void)
             if (buff_len > 0)
                 max7456SendDma(spiBuff, NULL, buff_len);
             #else
-            ENABLE_MAX7456;
-            for (k=0; k < buff_len; k++)
-                spiTransferByte(MAX7456_SPI_INSTANCE, spiBuff[k]);
-            DISABLE_MAX7456;
+            ENABLE_MAX7456();
+            spiTransfer(MAX7456_SPI_INSTANCE, NULL, spiBuff, buff_len);
+            DISABLE_MAX7456();
             #endif // MAX7456_DMA_CHANNEL_TX
         }
         max7456Lock = false;
@@ -521,7 +560,7 @@ void max7456RefreshAll(void)
 #endif
         uint16_t xx;
         max7456Lock = true;
-        ENABLE_MAX7456;
+        ENABLE_MAX7456();
         max7456Send(MAX7456ADD_DMAH, 0);
         max7456Send(MAX7456ADD_DMAL, 0);
         max7456Send(MAX7456ADD_DMM, 1);
@@ -529,12 +568,12 @@ void max7456RefreshAll(void)
         for (xx = 0; xx < maxScreenSize; ++xx)
         {
             max7456Send(MAX7456ADD_DMDI, screenBuffer[xx]);
-            shadowBuffer[xx] = screenBuffer[xx];
         }
+        memset(screenIsDirty, 0, sizeof(screenIsDirty));
 
         max7456Send(MAX7456ADD_DMDI, 0xFF);
         max7456Send(MAX7456ADD_DMM, 0);
-        DISABLE_MAX7456;
+        DISABLE_MAX7456();
         max7456Lock = false;
     }
 }
@@ -549,7 +588,7 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
     while (max7456Lock);
     max7456Lock = true;
 
-    ENABLE_MAX7456;
+    ENABLE_MAX7456();
     // disable display
     fontIsLoading = true;
     max7456Send(MAX7456ADD_VM0, 0);
@@ -572,7 +611,7 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
     // wait until bit 5 in the status register returns to 0 (12ms)
     while ((max7456Send(MAX7456ADD_STAT, 0x00) & STAT_NVR_BUSY) != 0x00);
 
-    DISABLE_MAX7456;
+    DISABLE_MAX7456();
 
     max7456Lock = false;
 }

src/main/drivers/max7456_symbols.h

@@ -88,8 +88,8 @@
 #define SYM_DIRECTION  0x72
 
 // GPS Coordinates and Altitude
-#define SYM_LAT 0xCA
-#define SYM_LON 0xCB
+#define SYM_LAT 0xA6
+#define SYM_LON 0xA7
 #define SYM_ALT 0xAA
 
 // GPS Mode and Autopilot
@@ -202,6 +202,7 @@
 #define SYM_FLY_M 0x9C
 #define SYM_ON_H  0x70
 #define SYM_FLY_H 0x71
+#define SYM_CLOCK 0xBC
 
 // Throttle Position (%)
 #define SYM_THR   0x04

src/main/drivers/rx_spi.c

@@ -104,7 +104,7 @@ void rxSpiDeviceInit(rx_spi_type_e spiType)
     DISABLE_RX();
 
 #ifdef RX_SPI_INSTANCE
-    spiSetDivisor(RX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    spiSetSpeed(RX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
 #endif
     hardwareInitialised = true;
 }

src/main/drivers/sdcard.c

@@ -204,7 +204,7 @@ static void sdcard_reset(void)
     }
 
     if (sdcard.state >= SDCARD_STATE_READY) {
-        spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER);
+        spiSetSpeed(SDCARD_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
     }
 
     sdcard.failureCount++;
@@ -567,7 +567,7 @@ void sdcard_init(bool useDMA)
 #endif // SDCARD_SPI_CS_PIN
 
     // Max frequency is initially 400kHz
-    spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER);
+    spiSetSpeed(SDCARD_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
     // SDCard wants 1ms minimum delay after power is applied to it
     delay(1000);
@@ -727,7 +727,7 @@ bool sdcard_poll(void)
                 }
 
                 // Now we're done with init and we can switch to the full speed clock (<25MHz)
-                spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER);
+                spiSetSpeed(SDCARD_SPI_INSTANCE, SPI_CLOCK_FAST);
 
                 sdcard.multiWriteBlocksRemain = 0;
 

src/main/drivers/serial.h

@@ -31,7 +31,17 @@ typedef enum portOptions_t {
     SERIAL_PARITY_NO     = 0 << 2,
     SERIAL_PARITY_EVEN   = 1 << 2,
     SERIAL_UNIDIR        = 0 << 3,
-    SERIAL_BIDIR         = 1 << 3
+    SERIAL_BIDIR         = 1 << 3,
+
+    /*
+     * Note on SERIAL_BIDIR_PP
+     * With SERIAL_BIDIR_PP, the very first start bit of back-to-back bytes
+     * is lost and the first data byte will be lost by a framing error.
+     * To ensure the first start bit to be sent, prepend a zero byte (0x00)
+     * to actual data bytes.
+     */
+    SERIAL_BIDIR_OD      = 0 << 4,
+    SERIAL_BIDIR_PP      = 1 << 4
 } portOptions_t;
 
 typedef void (*serialReceiveCallbackPtr)(uint16_t data);   // used by serial drivers to return frames to app

src/main/drivers/serial_uart_stm32f10x.c

@@ -128,7 +128,7 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
     // UART1_RX    PA10
     if (options & SERIAL_BIDIR) {
         IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL, RESOURCE_UART_TXRX, 1);
-        IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_OD);
+        IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD);
     } else {
         if (mode & MODE_TX) {
             IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL, RESOURCE_UART_TX, 1);
@@ -205,7 +205,8 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option
     // UART2_RX    PA3
     if (options & SERIAL_BIDIR) {
         IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TXRX, 2);
-        IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), IOCFG_AF_OD);
+        IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TXRX, 2);
+        IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD);
     } else {
         if (mode & MODE_TX) {
             IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TX, 2);
@@ -271,7 +272,7 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     if (options & SERIAL_BIDIR) {
         IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL, RESOURCE_UART_TXRX, 3);
-        IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOCFG_AF_OD);
+        IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD);
     } else {
         if (mode & MODE_TX) {
             IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL, RESOURCE_UART_TX, 3);

src/main/drivers/serial_uart_stm32f30x.c

@@ -115,8 +115,8 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui
 {
     if (options & SERIAL_BIDIR) {
         ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz,
-            (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD,
-            (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP
+            ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_OType_PP : GPIO_OType_OD,
+            ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP
         );
 
         IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, index);

src/main/drivers/serial_uart_stm32f4xx.c

@@ -350,7 +350,10 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
 
     if (options & SERIAL_BIDIR) {
         IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device));
-        IOConfigGPIOAF(tx, IOCFG_AF_OD, uart->af);
+        if (options & SERIAL_BIDIR_PP)
+            IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
+        else
+            IOConfigGPIOAF(tx, IOCFG_AF_OD, uart->af);
     }
     else {
         if (mode & MODE_TX) {

src/main/drivers/system.c

@@ -50,9 +50,9 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
 }
 
 // cycles per microsecond
-STATIC_FASTRAM_UNIT_TESTED timeUs_t usTicks = 0;
+STATIC_UNIT_TESTED  timeUs_t usTicks = 0;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
-STATIC_FASTRAM_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
+STATIC_UNIT_TESTED  volatile timeMs_t sysTickUptime = 0;
 // cached value of RCC->CSR
 uint32_t cachedRccCsrValue;
 
@@ -76,7 +76,7 @@ void cycleCounterInit(void)
 
 // SysTick
 
-STATIC_FASTRAM volatile int sysTickPending = 0;
+static volatile int sysTickPending = 0;
 
 void SysTick_Handler(void)
 {

src/main/drivers/vtx_common.c

@@ -0,0 +1,139 @@
+/*
+ * 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_COMMON)
+
+#include "vtx_common.h"
+
+vtxDevice_t *vtxDevice = NULL;
+
+void vtxCommonInit(void)
+{
+}
+
+// Whatever registered last will win
+
+void vtxCommonRegisterDevice(vtxDevice_t *pDevice)
+{
+    vtxDevice = pDevice;
+}
+
+void vtxCommonProcess(timeUs_t currentTimeUs)
+{
+    if (!vtxDevice)
+        return;
+
+    if (vtxDevice->vTable->process)
+        vtxDevice->vTable->process(currentTimeUs);
+}
+
+vtxDevType_e vtxCommonGetDeviceType(void)
+{
+    if (!vtxDevice || !vtxDevice->vTable->getDeviceType)
+        return VTXDEV_UNKNOWN;
+
+    return vtxDevice->vTable->getDeviceType();
+}
+
+// band and channel are 1 origin
+void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    if (!vtxDevice)
+        return;
+
+    if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount))
+        return;
+    
+    if (vtxDevice->vTable->setBandAndChannel)
+        vtxDevice->vTable->setBandAndChannel(band, channel);
+}
+
+// index is zero origin, zero = power off completely
+void vtxCommonSetPowerByIndex(uint8_t index)
+{
+    if (!vtxDevice)
+        return;
+
+    if (index > vtxDevice->capability.powerCount)
+        return;
+    
+    if (vtxDevice->vTable->setPowerByIndex)
+        vtxDevice->vTable->setPowerByIndex(index);
+}
+
+// on = 1, off = 0
+void vtxCommonSetPitMode(uint8_t onoff)
+{
+    if (!vtxDevice)
+        return;
+
+    if (vtxDevice->vTable->setPitMode)
+        vtxDevice->vTable->setPitMode(onoff);
+}
+
+bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
+{
+    if (!vtxDevice)
+        return false;
+
+    if (vtxDevice->vTable->getBandAndChannel)
+        return vtxDevice->vTable->getBandAndChannel(pBand, pChannel);
+    else
+        return false;
+}
+
+bool vtxCommonGetPowerIndex(uint8_t *pIndex)
+{
+    if (!vtxDevice)
+        return false;
+
+    if (vtxDevice->vTable->getPowerIndex)
+        return vtxDevice->vTable->getPowerIndex(pIndex);
+    else
+        return false;
+}
+
+bool vtxCommonGetPitMode(uint8_t *pOnOff)
+{
+    if (!vtxDevice)
+        return false;
+
+    if (vtxDevice->vTable->getPitMode)
+        return vtxDevice->vTable->getPitMode(pOnOff);
+    else
+        return false;
+}
+
+bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability)
+{
+    if (!vtxDevice)
+        return false;
+
+    memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t));
+    return true;
+}
+#endif

src/main/drivers/vtx_common.h

@@ -0,0 +1,91 @@
+/*
+ * 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 "common/time.h"
+
+typedef enum {
+    VTXDEV_UNSUPPORTED = 0, // reserved for MSP
+    VTXDEV_RTC6705    = 1,
+    // 2 reserved
+    VTXDEV_SMARTAUDIO = 3,
+    VTXDEV_TRAMP      = 4,
+    VTXDEV_UNKNOWN    = 0xFF,
+} vtxDevType_e;
+
+struct vtxVTable_s;
+
+typedef struct vtxDeviceCapability_s {
+    uint8_t bandCount;
+    uint8_t channelCount;
+    uint8_t powerCount;
+} vtxDeviceCapability_t;
+
+typedef struct vtxDevice_s {
+    const struct vtxVTable_s const *vTable;
+
+    vtxDeviceCapability_t capability;
+
+    uint16_t *frequencyTable;  // Array of [bandCount][channelCount]
+    char **bandNames;    // char *bandNames[bandCount]
+    char **channelNames;    // char *channelNames[channelCount]
+    char **powerNames;   // char *powerNames[powerCount]
+
+    uint8_t band; // Band = 1, 1-based
+    uint8_t channel; // CH1 = 1, 1-based
+    uint8_t powerIndex; // Lowest/Off = 0
+    uint8_t pitMode; // 0 = non-PIT, 1 = PIT
+
+} vtxDevice_t;
+
+// {set,get}BandAndChannel: band and channel are 1 origin
+// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
+// {set,get}PitMode: 0 = OFF, 1 = ON
+
+typedef struct vtxVTable_s {
+    void (*process)(uint32_t currentTimeUs);
+    vtxDevType_e (*getDeviceType)(void);
+    bool (*isReady)(void);
+
+    void (*setBandAndChannel)(uint8_t band, uint8_t channel);
+    void (*setPowerByIndex)(uint8_t level);
+    void (*setPitMode)(uint8_t onoff);
+
+    bool (*getBandAndChannel)(uint8_t *pBand, uint8_t *pChannel);
+    bool (*getPowerIndex)(uint8_t *pIndex);
+    bool (*getPitMode)(uint8_t *pOnOff);
+} vtxVTable_t;
+
+// 3.1.0
+// PIT mode is defined as LOWEST POSSIBLE RF POWER.
+// - It can be a dedicated mode, or lowest RF power possible.
+// - It is *NOT* RF on/off control ?
+
+void vtxCommonInit(void);
+void vtxCommonRegisterDevice(vtxDevice_t *pDevice);
+
+// VTable functions
+void vtxCommonProcess(timeUs_t currentTimeUs);
+uint8_t vtxCommonGetDeviceType(void);
+void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel);
+void vtxCommonSetPowerByIndex(uint8_t level);
+void vtxCommonSetPitMode(uint8_t onoff);
+bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel);
+bool vtxCommonGetPowerIndex(uint8_t *pIndex);
+bool vtxCommonGetPitMode(uint8_t *pOnOff);
+bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability);

src/main/drivers/vtx_rtc6705.c

@@ -38,7 +38,6 @@
 
 #include "drivers/vtx_rtc6705.h"
 
-
 #define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400
 #define RTC6705_SET_A1 0x8F3031 //5865
 #define RTC6705_SET_A2 0x8EB1B1 //5845
@@ -204,7 +203,7 @@ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
     band = constrain(band, 0, RTC6705_BAND_COUNT - 1);
     channel = constrain(channel, 0, RTC6705_CHANNEL_COUNT - 1);
 
-    spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
+    spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
 
     rtc6705Transfer(RTC6705_SET_HEAD);
     rtc6705Transfer(channelArray[band][channel]);
@@ -227,7 +226,7 @@ void rtc6705SetFreq(uint16_t frequency)
     val_hex |= (val_a << 5);
     val_hex |= (val_n << 12);
 
-    spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
+    spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
 
     rtc6705Transfer(RTC6705_SET_HEAD);
     delayMicroseconds(10);
@@ -238,7 +237,7 @@ void rtc6705SetRFPower(uint8_t rf_power)
 {
     rf_power = constrain(rf_power, 0, RTC6705_RF_POWER_COUNT - 1);
 
-    spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
+    spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
 
     uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write
     val_hex |= RTC6705_ADDRESS; // address

src/main/fc/fc_init.c

@@ -71,6 +71,12 @@
 #include "drivers/time.h"
 #include "drivers/timer.h"
 #include "drivers/vcd.h"
+#include "drivers/gyro_sync.h"
+#include "drivers/io.h"
+#include "drivers/exti.h"
+#include "drivers/io_pca9685.h"
+#include "drivers/vtx_rtc6705.h"
+#include "drivers/vtx_common.h"
 
 #include "fc/cli.h"
 #include "fc/config.h"
@@ -98,6 +104,10 @@
 #include "io/osd.h"
 #include "io/rcsplit.h"
 #include "io/serial.h"
+#include "io/displayport_msp.h"
+#include "io/vtx_control.h"
+#include "io/vtx_smartaudio.h"
+#include "io/vtx_tramp.h"
 
 #include "msp/msp_serial.h"
 
@@ -619,10 +629,26 @@ void init(void)
 #ifdef BARO
     baroStartCalibration();
 #endif
+
 #ifdef PITOT
     pitotStartCalibration();
 #endif
 
+#ifdef VTX_CONTROL
+    vtxControlInit();
+
+    vtxCommonInit();
+
+#ifdef VTX_SMARTAUDIO
+    vtxSmartAudioInit();
+#endif
+
+#ifdef VTX_TRAMP
+    vtxTrampInit();
+#endif
+
+#endif // VTX_CONTROL
+
     // start all timers
     // TODO - not implemented yet
     timerStart();

src/main/fc/fc_msp.c

@@ -417,7 +417,7 @@ static void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
         if (activeBoxes[activeBoxIds[i]]) {
-            bitArraySet(mspBoxModeFlags, i);
+            bitArraySet(mspBoxModeFlags->bits, i);
         }
     }
 }
@@ -1879,6 +1879,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                     osdConfigMutable()->item_pos[addr] = pos;
                 }
             }
+            // Either a element position change or a units change needs
+            // a full redraw, since an element can change size significantly
+            // and the old position or the now unused space due to the
+            // size change need to be erased.
+            osdStartFullRedraw();
         }
         break;
     case MSP_OSD_CHAR_WRITE:

src/main/fc/fc_tasks.c

@@ -32,6 +32,7 @@
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/stack_check.h"
+#include "drivers/vtx_common.h"
 
 #include "fc/cli.h"
 #include "fc/config.h"
@@ -257,6 +258,19 @@ void taskUpdateOsd(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef VTX_CONTROL
+// Everything that listens to VTX devices
+void taskVtxControl(timeUs_t currentTimeUs)
+{
+    if (ARMING_FLAG(ARMED))
+        return;
+
+#ifdef VTX_COMMON
+    vtxCommonProcess(currentTimeUs);
+#endif
+}
+#endif
+
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -333,6 +347,11 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
 #endif
 #endif
+#ifdef VTX_CONTROL
+#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
+    setTaskEnabled(TASK_VTXCTRL, true);
+#endif
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -512,7 +531,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_OSD] = {
         .taskName = "OSD",
         .taskFunc = taskUpdateOsd,
-        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .desiredPeriod = TASK_PERIOD_HZ(250),
         .staticPriority = TASK_PRIORITY_LOW,
     },
 #endif
@@ -534,4 +553,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif
+
+#ifdef VTX_CONTROL
+    [TASK_VTXCTRL] = {
+        .taskName = "VTXCTRL",
+        .taskFunc = taskVtxControl,
+        .desiredPeriod = TASK_PERIOD_HZ(5),          // 5Hz @200msec
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
 };

src/main/fc/rc_modes.c

@@ -66,7 +66,7 @@ bool isUsingNavigationModes(void)
 
 bool IS_RC_MODE_ACTIVE(boxId_e boxId)
 {
-    return bitArrayGet(&rcModeActivationMask, boxId);
+    return bitArrayGet(rcModeActivationMask.bits, boxId);
 }
 
 void rcModeUpdate(boxBitmask_t *newState)
@@ -124,13 +124,13 @@ void updateActivatedModes(void)
             if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
                 // AND the conditions
                 if (activeConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
-                    bitArraySet(&newMask, modeIndex);
+                    bitArraySet(newMask.bits, modeIndex);
                 }
             }
             else {
                 // OR the conditions
                 if (activeConditionCountPerMode[modeIndex] > 0) {
-                    bitArraySet(&newMask, modeIndex);
+                    bitArraySet(newMask.bits, modeIndex);
                 }
             }
         }

src/main/fc/rc_modes.h

@@ -19,6 +19,8 @@
 
 #include <stdbool.h>
 
+#include "common/bitarray.h"
+
 #include "config/parameter_group.h"
 
 typedef enum {
@@ -58,7 +60,7 @@ typedef enum {
 } boxId_e;
 
 // type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior
-typedef struct boxBitmask_s { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } boxBitmask_t;
+typedef struct boxBitmask_s { BITARRAY_DECLARE(bits, CHECKBOX_ITEM_COUNT); } boxBitmask_t;
 
 #define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
 

src/main/io/osd.c

@@ -42,6 +42,7 @@
 #include "common/axis.h"
 #include "common/printf.h"
 #include "common/string_light.h"
+#include "common/time.h"
 #include "common/utils.h"
 
 #include "config/feature.h"
@@ -104,6 +105,8 @@ static statistic_t stats;
 uint32_t resumeRefreshAt = 0;
 #define REFRESH_1S    (1000*1000)
 
+static bool fullRedraw = false;
+
 static uint8_t armState;
 
 static displayPort_t *osdDisplayPort;
@@ -202,6 +205,39 @@ static void osdFormatVelocityStr(char* buff, int32_t vel)
     }
 }
 
+static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h)
+{
+    uint32_t value = seconds;
+    char sym = sym_m;
+    // Maximum value we can show in minutes is 99 minutes and 59 seconds
+    if (seconds > (99 * 60) + 59) {
+        sym = sym_h;
+        value = seconds / 60;
+    }
+    buff[0] = sym;
+    tfp_sprintf(buff + 1, "%02d:%02d", value / 60, value % 60);
+}
+
+static inline void osdFormatOnTime(char *buff)
+{
+    osdFormatTime(buff, micros() / 1000000, SYM_ON_M, SYM_ON_H);
+}
+
+static inline void osdFormatFlyTime(char *buff)
+{
+    osdFormatTime(buff, flyTime / 1000000, SYM_FLY_M, SYM_FLY_H);
+}
+
+static void osdFormatCoordinate(char *buff, char sym, int32_t val)
+{
+    buff[0] = sym;
+    char wholeDegreeString[5];
+    tfp_sprintf(wholeDegreeString, "%d", val / GPS_DEGREES_DIVIDER);
+    char wholeUnshifted[32];
+    tfp_sprintf(wholeUnshifted, "%d", val);
+    tfp_sprintf(buff + 1, "%s.%s", wholeDegreeString, wholeUnshifted + strlen(wholeDegreeString));
+}
+
 static bool osdDrawSingleElement(uint8_t item)
 {
     if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(osdConfig()->item_pos[item])) {
@@ -255,23 +291,12 @@ static bool osdDrawSingleElement(uint8_t item)
         break;
 
     case OSD_GPS_LAT:
+        osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat);
+        break;
+
     case OSD_GPS_LON:
-        {
-            int32_t val;
-            if (item == OSD_GPS_LAT) {
-                buff[0] = 0xA6;
-                val = gpsSol.llh.lat;
-            } else {
-                buff[0] = 0xA7;
-                val = gpsSol.llh.lon;
-            }
-            char wholeDegreeString[5];
-            tfp_sprintf(wholeDegreeString, "%d", val / GPS_DEGREES_DIVIDER);
-            char wholeUnshifted[32];
-            tfp_sprintf(wholeUnshifted, "%d", val);
-            tfp_sprintf(buff + 1, "%s.%s", wholeDegreeString, wholeUnshifted + strlen(wholeDegreeString));
-            break;
-        }
+        osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon);
+        break;
 
     case OSD_HOME_DIR:
         {
@@ -318,17 +343,23 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_ONTIME:
         {
-            const uint32_t seconds = micros() / 1000000;
-            buff[0] = SYM_ON_M;
-            tfp_sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60);
+            osdFormatOnTime(buff);
             break;
         }
 
     case OSD_FLYTIME:
         {
-            const uint32_t seconds = flyTime / 1000000;
-            buff[0] = SYM_FLY_M;
-            tfp_sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60);
+            osdFormatFlyTime(buff);
+            break;
+        }
+
+    case OSD_ONTIME_FLYTIME:
+        {
+            if (ARMING_FLAG(ARMED)) {
+                osdFormatFlyTime(buff);
+            } else {
+                osdFormatOnTime(buff);
+            }
             break;
         }
 
@@ -565,6 +596,18 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
+    case OSD_RTC_TIME:
+        {
+            // RTC not configured will show 00:00
+            dateTime_t dateTime;
+            if (rtcGetDateTime(&dateTime)) {
+                dateTimeUTCToLocal(&dateTime, &dateTime);
+            }
+            buff[0] = SYM_CLOCK;
+            tfp_sprintf(buff + 1, "%02u:%02u", dateTime.hours, dateTime.minutes);
+            break;
+        }
+
     default:
         return false;
     }
@@ -611,74 +654,6 @@ void osdDrawNextElement(void)
     elementIndex = osdIncElementIndex(elementIndex);
 }
 
-void osdDrawElements(void)
-{
-    displayClearScreen(osdDisplayPort);
-
-#if 0
-    if (currentElement)
-        osdDrawElementPositioningHelp();
-#else
-    if (false)
-        ;
-#endif
-#ifdef CMS
-    else if (sensors(SENSOR_ACC) || displayIsGrabbed(osdDisplayPort))
-#else
-    else if (sensors(SENSOR_ACC))
-#endif
-    {
-        osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
-    }
-
-    osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE);
-    osdDrawSingleElement(OSD_RSSI_VALUE);
-    osdDrawSingleElement(OSD_FLYTIME);
-    osdDrawSingleElement(OSD_ONTIME);
-    osdDrawSingleElement(OSD_FLYMODE);
-    osdDrawSingleElement(OSD_THROTTLE_POS);
-    osdDrawSingleElement(OSD_VTX_CHANNEL);
-    if (feature(FEATURE_CURRENT_METER)) {
-        osdDrawSingleElement(OSD_CURRENT_DRAW);
-        osdDrawSingleElement(OSD_MAH_DRAWN);
-    }
-    osdDrawSingleElement(OSD_CRAFT_NAME);
-    osdDrawSingleElement(OSD_ALTITUDE);
-    osdDrawSingleElement(OSD_ROLL_PIDS);
-    osdDrawSingleElement(OSD_PITCH_PIDS);
-    osdDrawSingleElement(OSD_YAW_PIDS);
-    osdDrawSingleElement(OSD_POWER);
-
-#ifdef GPS
-#ifdef CMS
-    if (sensors(SENSOR_GPS) || displayIsGrabbed(osdDisplayPort))
-#else
-    if (sensors(SENSOR_GPS))
-#endif
-    {
-        osdDrawSingleElement(OSD_GPS_SATS);
-        osdDrawSingleElement(OSD_GPS_SPEED);
-        osdDrawSingleElement(OSD_GPS_LAT);
-        osdDrawSingleElement(OSD_GPS_LON);
-        osdDrawSingleElement(OSD_HOME_DIR);
-        osdDrawSingleElement(OSD_HOME_DIST);
-        osdDrawSingleElement(OSD_HEADING);
-    }
-#endif // GPS
-
-#if defined(BARO) || defined(GPS)
-    osdDrawSingleElement(OSD_VARIO);
-    osdDrawSingleElement(OSD_VARIO_NUM);
-#endif // defined
-
-#ifdef PITOT
-    if (sensors(SENSOR_PITOT)) {
-        osdDrawSingleElement(OSD_AIR_SPEED);
-    }
-#endif
-}
-
-
 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 {
     osdConfig->item_pos[OSD_ALTITUDE] = OSD_POS(1, 0) | VISIBLE_FLAG;
@@ -702,8 +677,11 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[OSD_CRAFT_NAME] = OSD_POS(20, 2);
     osdConfig->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6);
 
-    osdConfig->item_pos[OSD_ONTIME] = OSD_POS(23, 10) | VISIBLE_FLAG;
-    osdConfig->item_pos[OSD_FLYTIME] = OSD_POS(23, 11) | VISIBLE_FLAG;
+    osdConfig->item_pos[OSD_ONTIME] = OSD_POS(23, 8);
+    osdConfig->item_pos[OSD_FLYTIME] = OSD_POS(23, 9);
+    osdConfig->item_pos[OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | VISIBLE_FLAG;
+    osdConfig->item_pos[OSD_RTC_TIME] = OSD_POS(23, 12);
+
     osdConfig->item_pos[OSD_GPS_SATS] = OSD_POS(0, 11) | VISIBLE_FLAG;
 
     osdConfig->item_pos[OSD_GPS_LAT] = OSD_POS(0, 12);
@@ -853,6 +831,7 @@ static void osdUpdateStats(void)
 static void osdShowStats(void)
 {
     uint8_t top = 2;
+    const uint8_t statValuesX = 21;
     char buff[10];
 
     displayClearScreen(osdDisplayPort);
@@ -861,48 +840,80 @@ static void osdShowStats(void)
     if (STATE(GPS_FIX)) {
         displayWrite(osdDisplayPort, 2, top, "MAX SPEED        :");
         osdFormatVelocityStr(buff, stats.max_speed);
-        displayWrite(osdDisplayPort, 22, top++, buff);
+        displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
         displayWrite(osdDisplayPort, 2, top, "MAX DISTANCE     :");
         osdFormatDistanceStr(buff, stats.max_distance*100);
-        displayWrite(osdDisplayPort, 22, top++, buff);
+        displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
         displayWrite(osdDisplayPort, 2, top, "TRAVELED DISTANCE:");
         osdFormatDistanceStr(buff, getTotalTravelDistance());
-        displayWrite(osdDisplayPort, 22, top++, buff);
+        displayWrite(osdDisplayPort, statValuesX, top++, buff);
     }
 
     displayWrite(osdDisplayPort, 2, top, "MIN BATTERY      :");
     tfp_sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10);
-    displayWrite(osdDisplayPort, 22, top++, buff);
+    displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
     displayWrite(osdDisplayPort, 2, top, "MIN RSSI         :");
     itoa(stats.min_rssi, buff, 10);
     strcat(buff, "%");
-    displayWrite(osdDisplayPort, 22, top++, buff);
+    displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
     if (feature(FEATURE_CURRENT_METER)) {
         displayWrite(osdDisplayPort, 2, top, "MAX CURRENT      :");
         itoa(stats.max_current, buff, 10);
         strcat(buff, "A");
-        displayWrite(osdDisplayPort, 22, top++, buff);
+        displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
         displayWrite(osdDisplayPort, 2, top, "USED MAH         :");
         itoa(mAhDrawn, buff, 10);
         strcat(buff, "\x07");
-        displayWrite(osdDisplayPort, 22, top++, buff);
+        displayWrite(osdDisplayPort, statValuesX, top++, buff);
     }
 
     displayWrite(osdDisplayPort, 2, top, "MAX ALTITUDE     :");
     osdFormatDistanceStr(buff, stats.max_altitude);
-    displayWrite(osdDisplayPort, 22, top++, buff);
+    displayWrite(osdDisplayPort, statValuesX, top++, buff);
+
+    displayWrite(osdDisplayPort, 2, top, "FLY TIME         :");
+    uint32_t flySeconds = flyTime / 1000000;
+    uint16_t flyMinutes = flySeconds / 60;
+    flySeconds %= 60;
+    uint16_t flyHours = flyMinutes / 60;
+    flyMinutes %= 60;
+    tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds);
+    displayWrite(osdDisplayPort, statValuesX, top++, buff);
 }
 
 // called when motors armed
 static void osdShowArmed(void)
 {
+    dateTime_t dt;
+    char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)];
+    char *date;
+    char *time;
+    uint8_t y = 7;
+
     displayClearScreen(osdDisplayPort);
-    displayWrite(osdDisplayPort, 12, 7, "ARMED");
+    displayWrite(osdDisplayPort, 12, y, "ARMED");
+    y += 2;
+
+    if (STATE(GPS_FIX)) {
+        osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
+        osdFormatCoordinate(buf, SYM_LON, gpsSol.llh.lon);
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf);
+        y += 3;
+    }
+
+    if (rtcGetDateTime(&dt)) {
+        dateTimeFormatLocal(buf, &dt);
+        dateTimeSplitFormatted(buf, &date, &time);
+
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date);
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time);
+    }
 }
 
 static void osdRefresh(timeUs_t currentTimeUs)
@@ -949,6 +960,10 @@ static void osdRefresh(timeUs_t currentTimeUs)
 
 #ifdef CMS
     if (!displayIsGrabbed(osdDisplayPort)) {
+        if (fullRedraw) {
+            displayClearScreen(osdDisplayPort);
+            fullRedraw = false;
+        }
         osdUpdateAlarms();
         osdDrawNextElement();
         displayHeartbeat(osdDisplayPort);
@@ -972,8 +987,8 @@ void osdUpdate(timeUs_t currentTimeUs)
         return;
     }
 
-#define DRAW_FREQ_DENOM     1
-#define STATS_FREQ_DENOM    20
+#define DRAW_FREQ_DENOM     4
+#define STATS_FREQ_DENOM    50
     counter++;
 
     if ((counter % STATS_FREQ_DENOM) == 0) {
@@ -984,7 +999,7 @@ void osdUpdate(timeUs_t currentTimeUs)
         // redraw values in buffer
         osdRefresh(currentTimeUs);
     } else {
-        // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
+        // rest of time redraw screen
         displayDrawScreen(osdDisplayPort);
     }
 
@@ -997,4 +1012,10 @@ void osdUpdate(timeUs_t currentTimeUs)
     }
 #endif
 }
+
+void osdStartFullRedraw(void)
+{
+    fullRedraw = true;
+}
+
 #endif // OSD

src/main/io/osd.h

@@ -57,6 +57,8 @@ typedef enum {
     OSD_VARIO,
     OSD_VARIO_NUM,
     OSD_AIR_SPEED,
+    OSD_ONTIME_FLYTIME,
+    OSD_RTC_TIME,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -87,3 +89,4 @@ struct displayPort_s;
 void osdInit(struct displayPort_s *osdDisplayPort);
 void osdResetAlarms(void);
 void osdUpdate(timeUs_t currentTimeUs);
+void osdStartFullRedraw(void);

src/main/io/serial.h

@@ -41,7 +41,9 @@ typedef enum {
     FUNCTION_BLACKBOX            = (1 << 7), // 128
     FUNCTION_TELEMETRY_MAVLINK   = (1 << 8), // 256
     FUNCTION_TELEMETRY_IBUS      = (1 << 9), // 512
-    FUNCTION_RCSPLIT             = (1 << 10) // 1024
+    FUNCTION_RCSPLIT             = (1 << 10), // 1024
+    FUNCTION_VTX_SMARTAUDIO      = (1 << 11), // 2048
+    FUNCTION_VTX_TRAMP           = (1 << 12)  // 4096
 } serialPortFunction_e;
 
 typedef enum {

src/main/io/vtx_control.c

@@ -0,0 +1,155 @@
+/*
+ * 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/>.
+ */
+
+
+// Get target build configuration
+#include "platform.h"
+
+#include "common/maths.h"
+
+#include "config/config_eeprom.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/vtx_common.h"
+#include "drivers/light_led.h"
+#include "drivers/system.h"
+
+#include "fc/config.h"
+#include "fc/runtime_config.h"
+
+#include "io/beeper.h"
+#include "io/osd.h"
+#include "io/vtx_control.h"
+
+
+#if defined(VTX_CONTROL) && defined(VTX_COMMON)
+
+PG_REGISTER(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
+
+static uint8_t locked = 0;
+
+void vtxControlInit(void)
+{
+    // NOTHING TO DO
+}
+
+static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep)
+{
+    if (ARMING_FLAG(ARMED)) {
+        locked = 1;
+    }
+
+    if (!locked) {
+        uint8_t band = 0, channel = 0;
+        vtxCommonGetBandAndChannel(&band, &channel);
+        vtxCommonSetBandAndChannel(band + bandStep, channel + channelStep);
+    }
+}
+
+void vtxIncrementBand(void)
+{
+    vtxUpdateBandAndChannel(+1, 0);
+}
+
+void vtxDecrementBand(void)
+{
+    vtxUpdateBandAndChannel(-1, 0);
+}
+
+void vtxIncrementChannel(void)
+{
+    vtxUpdateBandAndChannel(0, +1);
+}
+
+void vtxDecrementChannel(void)
+{
+    vtxUpdateBandAndChannel(0, -1);
+}
+
+void vtxUpdateActivatedChannel(void)
+{
+    if (ARMING_FLAG(ARMED)) {
+        locked = 1;
+    }
+
+    if (!locked) {
+        static uint8_t lastIndex = -1;
+
+        for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) {
+            const vtxChannelActivationCondition_t *vtxChannelActivationCondition = &vtxConfig()->vtxChannelActivationConditions[index];
+
+            if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range)
+                && index != lastIndex) {
+                lastIndex = index;
+
+                vtxCommonSetBandAndChannel(vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel);
+                break;
+            }
+        }
+    }
+}
+
+void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep)
+{
+    uint8_t band = 0, channel = 0;
+    vtxDeviceCapability_t capability;
+
+    bool haveAllNeededInfo = vtxCommonGetBandAndChannel(&band, &channel) && vtxCommonGetDeviceCapability(&capability);
+    if (!haveAllNeededInfo) {
+        return;
+    }
+
+    int newChannel = channel + channelStep;
+    if (newChannel > capability.channelCount) {
+        newChannel = 1;
+    } else if (newChannel < 1) {
+        newChannel = capability.channelCount;
+    }
+
+    int newBand = band + bandStep;
+    if (newBand > capability.bandCount) {
+        newBand = 1;
+    } else if (newBand < 1) {
+        newBand = capability.bandCount;
+    }
+
+    vtxCommonSetBandAndChannel(newBand, newChannel);
+}
+
+void vtxCyclePower(const uint8_t powerStep)
+{
+    uint8_t power = 0;
+    vtxDeviceCapability_t capability;
+
+    bool haveAllNeededInfo = vtxCommonGetPowerIndex(&power) && vtxCommonGetDeviceCapability(&capability);
+    if (!haveAllNeededInfo) {
+        return;
+    }
+
+    int newPower = power + powerStep;
+    if (newPower >= capability.powerCount) {
+        newPower = 0;
+    } else if (newPower < 0) {
+        newPower = capability.powerCount;
+    }
+
+    vtxCommonSetPowerByIndex(newPower);
+}
+
+#endif
+

src/main/io/vtx_control.h

@@ -0,0 +1,49 @@
+/*
+ * 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 "fc/rc_modes.h"
+
+#define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT  10
+
+typedef struct vtxChannelActivationCondition_s {
+    uint8_t auxChannelIndex;
+    uint8_t band;
+    uint8_t channel;
+    channelRange_t range;
+} vtxChannelActivationCondition_t;
+
+typedef struct vtxConfig_s {
+    vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT];
+} vtxConfig_t;
+
+PG_DECLARE(vtxConfig_t, vtxConfig);
+
+void vtxControlInit(void);
+
+void vtxIncrementBand(void);
+void vtxDecrementBand(void);
+void vtxIncrementChannel(void);
+void vtxDecrementChannel(void);
+
+void vtxCyclePower(const uint8_t powerStep);
+void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep);
+
+void vtxUpdateActivatedChannel(void);
+
+void handleVTXControlButton(void);

src/main/io/vtx_smartaudio.h

@@ -0,0 +1,34 @@
+
+// For generic API use, but here for now
+
+bool vtxSmartAudioInit();
+
+#if 0
+#ifdef CMS
+
+uint16_t smartAudioSmartbaud;
+
+uint16_t saerr_badpre;
+uint16_t saerr_badlen;
+uint16_t saerr_crcerr;
+uint16_t saerr_oooresp;
+
+uint8_t smartAudioOpModel;
+uint8_t smartAudioStatus;
+uint8_t smartAudioBand;
+uint8_t smartAudioChan;
+uint16_t smartAudioFreq;
+uint8_t smartAudioPower;
+uint8_t smartAudioTxMode;
+uint8_t smartAudioPitFMode;
+uint16_t smartAudioORFreq;
+
+long smartAudioConfigureOpModelByGvar(displayPort_t *, const void *self);
+long smartAudioConfigurePitFModeByGvar(displayPort_t *, const void *self);
+long smartAudioConfigureBandByGvar(displayPort_t *, const void *self);
+long smartAudioConfigureChanByGvar(displayPort_t *, const void *self);
+long smartAudioConfigurePowerByGvar(displayPort_t *, const void *self);
+long smartAudioSetTxModeByGvar(displayPort_t *, const void *self);
+
+#endif
+#endif

src/main/io/vtx_smartaudio_cms.h

@@ -0,0 +1 @@
+extern CMS_Menu cmsx_menuVtxSmartAudio;

src/main/io/vtx_string.c

@@ -0,0 +1,77 @@
+/*
+ * 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_COMMON)
+
+const uint16_t vtx58frequencyTable[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 *pChannel)
+{
+    int8_t band;
+    uint8_t channel;
+
+    // Use reverse lookup order so that 5880Mhz
+    // get Raceband 7 instead of Fatshark 8.
+    for (band = 4 ; band >= 0 ; band--) {
+        for (channel = 0 ; channel < 8 ; channel++) {
+            if (vtx58frequencyTable[band][channel] == freq) {
+                *pBand = band + 1;
+                *pChannel = channel + 1;
+                return true;
+            }
+        }
+    }
+
+    *pBand = 0;
+    *pChannel = 0;
+
+    return false;
+}
+
+#endif

src/main/io/vtx_string.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <stdint.h>
+
+#if defined(VTX_COMMON)
+
+extern const uint16_t vtx58frequencyTable[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 *pChannel);
+
+#endif

src/main/io/vtx_tramp.c

@@ -0,0 +1,733 @@
+/*
+ * 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 "drivers/vtx_common.h"
+#include "io/vtx_tramp.h"
+#include "io/vtx_string.h"
+
+#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR)
+
+#if defined(CMS) || defined(VTX_COMMON)
+static const uint16_t trampPowerTable[] = {
+    25, 100, 200, 400, 600
+};
+
+static const char * const trampPowerNames[] = {
+    "---", "25 ", "100", "200", "400", "600"
+};
+#endif
+
+#if defined(VTX_COMMON)
+static const vtxVTable_t trampVTable; // forward
+static vtxDevice_t vtxTramp = {
+    .vTable = &trampVTable,
+    .capability.bandCount = 5,
+    .capability.channelCount = 8,
+    .capability.powerCount = sizeof(trampPowerTable),
+    .bandNames = (char **)vtx58BandNames,
+    .channelNames = (char **)vtx58ChannelNames,
+    .powerNames = (char **)trampPowerNames,
+};
+#endif
+
+static serialPort_t *trampSerialPort = NULL;
+
+static uint8_t trampReqBuffer[16];
+static uint8_t trampRespBuffer[16];
+
+typedef enum {
+    TRAMP_STATUS_BAD_DEVICE = -1,
+    TRAMP_STATUS_OFFLINE = 0,
+    TRAMP_STATUS_ONLINE,
+    TRAMP_STATUS_SET_FREQ_PW,
+    TRAMP_STATUS_CHECK_FREQ_PW
+} trampStatus_e;
+
+trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
+
+uint32_t trampRFFreqMin;
+uint32_t trampRFFreqMax;
+uint32_t trampRFPowerMax;
+
+uint32_t trampCurFreq = 0;
+uint8_t trampBand = 0;
+uint8_t trampChannel = 0;
+uint16_t trampPower = 0;       // Actual transmitting power
+uint16_t trampConfiguredPower = 0; // Configured transmitting power
+int16_t trampTemperature = 0;
+uint8_t trampPitMode = 0;
+
+// Maximum number of requests sent to try a config change
+#define TRAMP_MAX_RETRIES 2
+
+uint32_t trampConfFreq = 0;
+uint8_t  trampFreqRetries = 0;
+
+uint16_t trampConfPower = 0;
+uint8_t  trampPowerRetries = 0;
+
+#ifdef CMS
+static void trampCmsUpdateStatusString(void); // Forward
+#endif
+
+static void trampWriteBuf(uint8_t *buf)
+{
+    serialWriteBuf(trampSerialPort, buf, 16);
+}
+
+static uint8_t trampChecksum(uint8_t *trampBuf)
+{
+    uint8_t cksum = 0;
+
+    for (int i = 1 ; i < 14 ; i++)
+        cksum += trampBuf[i];
+
+    return cksum;
+}
+
+void trampCmdU16(uint8_t cmd, uint16_t param)
+{
+    if (!trampSerialPort)
+        return;
+
+    memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
+    trampReqBuffer[0] = 15;
+    trampReqBuffer[1] = cmd;
+    trampReqBuffer[2] = param & 0xff;
+    trampReqBuffer[3] = (param >> 8) & 0xff;
+    trampReqBuffer[14] = trampChecksum(trampReqBuffer);
+    trampWriteBuf(trampReqBuffer);
+}
+
+void trampSetFreq(uint16_t freq)
+{
+    trampConfFreq = freq;
+    if(trampConfFreq != trampCurFreq)
+        trampFreqRetries = TRAMP_MAX_RETRIES;
+}
+
+void trampSendFreq(uint16_t freq)
+{
+    trampCmdU16('F', freq);
+}
+
+void trampSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    trampSetFreq(vtx58frequencyTable[band - 1][channel - 1]);
+}
+
+void trampSetRFPower(uint16_t level)
+{
+    trampConfPower = level;
+    if(trampConfPower != trampPower)
+        trampPowerRetries = TRAMP_MAX_RETRIES;
+}
+
+void trampSendRFPower(uint16_t level)
+{
+    trampCmdU16('P', level);
+}
+
+// return false if error
+bool trampCommitChanges()
+{
+    if(trampStatus != TRAMP_STATUS_ONLINE)
+        return false;
+
+    trampStatus = TRAMP_STATUS_SET_FREQ_PW;
+    return true;
+}
+
+void trampSetPitMode(uint8_t onoff)
+{
+    trampCmdU16('I', onoff ? 0 : 1);
+}
+
+// returns completed response code
+char trampHandleResponse(void)
+{
+    uint8_t respCode = trampRespBuffer[1];
+
+    switch (respCode) {
+    case 'r':
+        {
+            uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+            if(min_freq != 0) {
+                trampRFFreqMin = min_freq;
+                trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
+                trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
+                return 'r';
+            }
+
+            // throw bytes echoed from tx to rx in bidirectional mode away
+        }
+        break;
+
+    case 'v':
+        {
+            uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+            if(freq != 0) {
+                trampCurFreq = freq;
+                trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
+                trampPitMode = trampRespBuffer[7];
+                trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
+                vtx58_Freq2Bandchan(trampCurFreq, &trampBand, &trampChannel);
+
+                if(trampConfFreq == 0)  trampConfFreq  = trampCurFreq;
+                if(trampConfPower == 0) trampConfPower = trampPower;
+                return 'v';
+            }
+
+            // throw bytes echoed from tx to rx in bidirectional mode away
+        }
+        break;
+
+    case 's':
+        {
+            uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
+            if(temp != 0) {
+                trampTemperature = temp;
+                return 's';
+            }
+        }
+        break;
+    }
+
+    return 0;
+}
+
+typedef enum {
+    S_WAIT_LEN = 0,   // Waiting for a packet len
+    S_WAIT_CODE,      // Waiting for a response code
+    S_DATA,           // Waiting for rest of the packet.
+} trampReceiveState_e;
+
+static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
+static int trampReceivePos = 0;
+
+static void trampResetReceiver()
+{
+    trampReceiveState = S_WAIT_LEN;
+    trampReceivePos = 0;
+}
+
+static bool trampIsValidResponseCode(uint8_t code)
+{
+    if (code == 'r' || code == 'v' || code == 's')
+        return true;
+    else
+        return false;
+}
+
+// returns completed response code or 0
+static char trampReceive(uint32_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (!trampSerialPort)
+        return 0;
+
+    while (serialRxBytesWaiting(trampSerialPort)) {
+        uint8_t c = serialRead(trampSerialPort);
+        trampRespBuffer[trampReceivePos++] = c;
+
+        switch(trampReceiveState) {
+        case S_WAIT_LEN:
+            if (c == 0x0F) {
+                trampReceiveState = S_WAIT_CODE;
+            } else {
+                trampReceivePos = 0;
+            }
+            break;
+
+        case S_WAIT_CODE:
+            if (trampIsValidResponseCode(c)) {
+                trampReceiveState = S_DATA;
+            } else {
+                trampResetReceiver();
+            }
+            break;
+
+        case S_DATA:
+            if (trampReceivePos == 16) {
+                uint8_t cksum = trampChecksum(trampRespBuffer);
+
+                trampResetReceiver();
+
+                if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
+                    return trampHandleResponse();
+                }
+            }
+            break;
+
+        default:
+            trampResetReceiver();
+        }
+    }
+
+    return 0;
+}
+
+void trampQuery(uint8_t cmd)
+{
+    trampResetReceiver();
+    trampCmdU16(cmd, 0);
+}
+
+void trampQueryR(void)
+{
+    trampQuery('r');
+}
+
+void trampQueryV(void)
+{
+    trampQuery('v');
+}
+
+void trampQueryS(void)
+{
+    trampQuery('s');
+}
+
+void vtxTrampProcess(uint32_t currentTimeUs)
+{
+    static uint32_t lastQueryTimeUs = 0;
+
+#ifdef TRAMP_DEBUG
+    static uint16_t debugFreqReqCounter = 0;
+    static uint16_t debugPowReqCounter = 0;
+#endif
+
+    if (trampStatus == TRAMP_STATUS_BAD_DEVICE)
+        return;
+
+    char replyCode = trampReceive(currentTimeUs);
+
+#ifdef TRAMP_DEBUG
+    debug[0] = trampStatus;
+#endif
+
+    switch(replyCode) {
+    case 'r':
+        if (trampStatus <= TRAMP_STATUS_OFFLINE)
+            trampStatus = TRAMP_STATUS_ONLINE;
+        break;
+
+    case 'v':
+         if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW)
+             trampStatus = TRAMP_STATUS_SET_FREQ_PW;
+         break;
+    }
+
+    switch(trampStatus) {
+
+    case TRAMP_STATUS_OFFLINE:
+    case TRAMP_STATUS_ONLINE:
+        if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
+
+            if (trampStatus == TRAMP_STATUS_OFFLINE)
+                trampQueryR();
+            else {
+                static unsigned int cnt = 0;
+                if(((cnt++) & 1) == 0)
+                    trampQueryV();
+                else
+                    trampQueryS();
+            }
+
+            lastQueryTimeUs = currentTimeUs;
+        }
+        break;
+
+    case TRAMP_STATUS_SET_FREQ_PW:
+        {
+            bool done = true;
+            if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampCurFreq)) {
+                trampSendFreq(trampConfFreq);
+                trampFreqRetries--;
+#ifdef TRAMP_DEBUG
+                debugFreqReqCounter++;
+#endif
+                done = false;
+            }
+            else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
+                trampSendRFPower(trampConfPower);
+                trampPowerRetries--;
+#ifdef TRAMP_DEBUG
+                debugPowReqCounter++;
+#endif
+                done = false;
+            }
+
+            if(!done) {
+                trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
+
+                // delay next status query by 300ms
+                lastQueryTimeUs = currentTimeUs + 300 * 1000;
+            }
+            else {
+                // everything has been done, let's return to original state
+                trampStatus = TRAMP_STATUS_ONLINE;
+                // reset configuration value in case it failed (no more retries)
+                trampConfFreq  = trampCurFreq;
+                trampConfPower = trampPower;
+                trampFreqRetries = trampPowerRetries = 0;
+            }
+        }
+        break;
+
+    case TRAMP_STATUS_CHECK_FREQ_PW:
+        if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
+            trampQueryV();
+            lastQueryTimeUs = currentTimeUs;
+        }
+        break;
+
+    default:
+        break;
+    }
+
+#ifdef TRAMP_DEBUG
+    debug[1] = debugFreqReqCounter;
+    debug[2] = debugPowReqCounter;
+    debug[3] = 0;
+#endif
+
+#ifdef CMS
+    trampCmsUpdateStatusString();
+#endif
+}
+
+#ifdef CMS
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+
+
+char trampCmsStatusString[31] = "- -- ---- ----";
+//                               m bc ffff tppp
+//                               01234567890123
+
+static void trampCmsUpdateStatusString(void)
+{
+    trampCmsStatusString[0] = '*';
+    trampCmsStatusString[1] = ' ';
+    trampCmsStatusString[2] = vtx58BandLetter[trampBand];
+    trampCmsStatusString[3] = vtx58ChannelNames[trampChannel][0];
+    trampCmsStatusString[4] = ' ';
+
+    if (trampCurFreq)
+        tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq);
+    else
+        tfp_sprintf(&trampCmsStatusString[5], "----");
+
+    if (trampPower) {
+        tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampPower == trampConfiguredPower) ? ' ' : '*', trampPower);
+    }
+    else
+        tfp_sprintf(&trampCmsStatusString[9], " ----");
+}
+
+uint8_t trampCmsPitMode = 0;
+uint8_t trampCmsBand = 1;
+uint8_t trampCmsChan = 1;
+uint16_t trampCmsFreqRef;
+
+static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames };
+
+static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames };
+
+static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
+
+static uint8_t trampCmsPower = 1;
+
+static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames };
+
+static void trampCmsUpdateFreqRef(void)
+{
+    if (trampCmsBand > 0 && trampCmsChan > 0)
+        trampCmsFreqRef = vtx58frequencyTable[trampCmsBand - 1][trampCmsChan - 1];
+}
+
+static long trampCmsConfigBand(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (trampCmsBand == 0)
+        // Bounce back
+        trampCmsBand = 1;
+    else
+        trampCmsUpdateFreqRef();
+
+    return 0;
+}
+
+static long trampCmsConfigChan(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (trampCmsChan == 0)
+        // Bounce back
+        trampCmsChan = 1;
+    else
+        trampCmsUpdateFreqRef();
+
+    return 0;
+}
+
+static long trampCmsConfigPower(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (trampCmsPower == 0)
+        // Bounce back
+        trampCmsPower = 1;
+
+    return 0;
+}
+
+static OSD_INT16_t trampCmsEntTemp = { &trampTemperature, -100, 300, 0 };
+
+static const char * const trampCmsPitModeNames[] = {
+    "---", "OFF", "ON "
+};
+
+static OSD_TAB_t trampCmsEntPitMode = { &trampCmsPitMode, 2, trampCmsPitModeNames };
+
+static long trampCmsSetPitMode(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (trampCmsPitMode == 0) {
+        // Bouce back
+        trampCmsPitMode = 1;
+    } else {
+        trampSetPitMode(trampCmsPitMode - 1);
+    }
+
+    return 0;
+}
+
+static long trampCmsCommence(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    trampSetBandAndChannel(trampCmsBand, trampCmsChan);
+    trampSetRFPower(trampPowerTable[trampCmsPower-1]);
+
+    // If it fails, the user should retry later
+    trampCommitChanges();
+
+
+    return MENU_CHAIN_BACK;
+}
+
+static void trampCmsInitSettings()
+{
+    if(trampBand > 0) trampCmsBand = trampBand;
+    if(trampChannel > 0) trampCmsChan = trampChannel;
+
+    trampCmsUpdateFreqRef();
+    trampCmsPitMode = trampPitMode + 1;
+
+    if (trampConfiguredPower > 0) {
+        for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
+            if (trampConfiguredPower <= trampPowerTable[i]) {
+                trampCmsPower = i + 1;
+                break;
+            }
+        }
+    }
+}
+
+static long trampCmsOnEnter()
+{
+    trampCmsInitSettings();
+    return 0;
+}
+
+static OSD_Entry trampCmsMenuCommenceEntries[] = {
+    { "CONFIRM", OME_Label,   NULL,          NULL, 0 },
+    { "YES",     OME_Funcall, trampCmsCommence, NULL, 0 },
+    { "BACK",    OME_Back, NULL, NULL, 0 },
+    { NULL,      OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu trampCmsMenuCommence = {
+    .GUARD_text = "XVTXTRC",
+    .GUARD_type = OME_MENU,
+    .onEnter = NULL,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = trampCmsMenuCommenceEntries,
+};
+
+static OSD_Entry trampMenuEntries[] =
+{
+    { "- TRAMP -", OME_Label, NULL, NULL, 0 },
+
+    { "",       OME_Label,   NULL,                   trampCmsStatusString,  DYNAMIC },
+    { "PIT",    OME_TAB,     trampCmsSetPitMode,     &trampCmsEntPitMode,   0 },
+    { "BAND",   OME_TAB,     trampCmsConfigBand,     &trampCmsEntBand,      0 },
+    { "CHAN",   OME_TAB,     trampCmsConfigChan,     &trampCmsEntChan,      0 },
+    { "(FREQ)", OME_UINT16,  NULL,                   &trampCmsEntFreqRef,   DYNAMIC },
+    { "POWER",  OME_TAB,     trampCmsConfigPower,    &trampCmsEntPower,     0 },
+    { "T(C)",   OME_INT16,   NULL,                   &trampCmsEntTemp,      DYNAMIC },
+    { "SET",    OME_Submenu, cmsMenuChange,          &trampCmsMenuCommence, 0 },
+
+    { "BACK",   OME_Back, NULL, NULL, 0 },
+    { NULL,     OME_END, NULL, NULL, 0 }
+};
+
+CMS_Menu cmsx_menuVtxTramp = {
+    .GUARD_text = "XVTXTR",
+    .GUARD_type = OME_MENU,
+    .onEnter = trampCmsOnEnter,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = trampMenuEntries,
+};
+#endif
+
+#ifdef VTX_COMMON
+
+// Interface to common VTX API
+
+vtxDevType_e vtxTrampGetDeviceType(void)
+{
+    return VTXDEV_TRAMP;
+}
+
+bool vtxTrampIsReady(void)
+{
+    return trampStatus > TRAMP_STATUS_OFFLINE;
+}
+
+void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    if (band && channel) {
+        trampSetBandAndChannel(band, channel);
+        trampCommitChanges();
+    }
+}
+
+void vtxTrampSetPowerByIndex(uint8_t index)
+{
+    if (index) {
+        trampSetRFPower(trampPowerTable[index - 1]);
+        trampCommitChanges();
+    }
+}
+
+void vtxTrampSetPitMode(uint8_t onoff)
+{
+    trampSetPitMode(onoff);
+}
+
+bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
+{
+    if (!vtxTrampIsReady())
+        return false;
+
+    *pBand = trampBand;
+    *pChannel = trampChannel;
+    return true;
+}
+
+bool vtxTrampGetPowerIndex(uint8_t *pIndex)
+{
+    if (!vtxTrampIsReady())
+        return false;
+
+    if (trampConfiguredPower > 0) {
+        for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
+            if (trampConfiguredPower <= trampPowerTable[i]) {
+                *pIndex = i + 1;
+                break;
+            }
+        }
+    }
+
+    return true;
+}
+
+bool vtxTrampGetPitMode(uint8_t *pOnOff)
+{
+    if (!vtxTrampIsReady())
+        return false;
+
+    *pOnOff = trampPitMode;
+    return true;
+}
+
+static const vtxVTable_t trampVTable = {
+    .process = vtxTrampProcess,
+    .getDeviceType = vtxTrampGetDeviceType,
+    .isReady = vtxTrampIsReady,
+    .setBandAndChannel = vtxTrampSetBandAndChannel,
+    .setPowerByIndex = vtxTrampSetPowerByIndex,
+    .setPitMode = vtxTrampSetPitMode,
+    .getBandAndChannel = vtxTrampGetBandAndChannel,
+    .getPowerIndex = vtxTrampGetPowerIndex,
+    .getPitMode = vtxTrampGetPitMode,
+};
+
+#endif
+
+bool vtxTrampInit(void)
+{
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
+
+    if (portConfig) {
+        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
+    }
+
+    if (!trampSerialPort) {
+        return false;
+    }
+
+#if defined(VTX_COMMON)
+    vtxCommonRegisterDevice(&vtxTramp);
+#endif
+    
+    return true;
+}
+
+#endif // VTX_TRAMP

src/main/io/vtx_tramp.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#if defined(VTX_TRAMP) && defined(VTX_CONTROL)
+
+bool vtxTrampInit();
+
+#ifdef CMS
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+extern CMS_Menu cmsx_menuVtxTramp;
+#endif
+
+#endif

src/main/platform.h

@@ -76,4 +76,4 @@
 
 #include "target/common.h"
 #include "target.h"
-
+#include "target/common_post.h"

src/main/scheduler/scheduler.h

@@ -104,6 +104,10 @@ typedef enum {
 #ifdef USE_RCSPLIT
     TASK_RCSPLIT,
 #endif
+#ifdef VTX_CONTROL
+    TASK_VTXCTRL,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 

src/main/target/ALIENFLIGHTF4/target.h

@@ -76,11 +76,6 @@
 #define SDCARD_SPI_INSTANCE             SPI2
 #define SDCARD_SPI_CS_PIN               PB10
 
-// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream4
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/ALIENFLIGHTNGF7/target.h

@@ -86,11 +86,6 @@
 #define SDCARD_SPI_INSTANCE             SPI2
 #define SDCARD_SPI_CS_PIN               PB10
 
-// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream4
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/ANYFCF7/target.h

@@ -130,10 +130,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI4
 #define SDCARD_SPI_CS_PIN                   SPI4_NSS_PIN
 
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA2_Stream1
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA2

src/main/target/BEEROTORF4/target.h

@@ -65,8 +65,8 @@
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI3
 #define MAX7456_SPI_CS_PIN      SPI3_NSS_PIN
-#define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)
-#define MAX7456_RESTORE_CLK     (SPI_CLOCK_FAST)
+#define MAX7456_SPI_CLK         SPI_CLOCK_STANDARD
+#define MAX7456_RESTORE_CLK     SPI_CLOCK_FAST
 
 //#define MAX7456_DMA_CHANNEL_TX              DMA1_Stream5
 //#define MAX7456_DMA_CHANNEL_RX              DMA1_Stream0
@@ -80,11 +80,6 @@
 #define SDCARD_SPI_INSTANCE     SPI2
 #define SDCARD_SPI_CS_PIN       SPI2_NSS_PIN
 
-// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream4
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/BLUEJAYF4/target.h

@@ -76,11 +76,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI3
 #define SDCARD_SPI_CS_PIN                   PA15
 
-// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/F4BY/target.h

@@ -62,12 +62,6 @@
 #define SDCARD_SPI_INSTANCE     SPI2
 #define SDCARD_SPI_CS_PIN       PE15
 
-// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream3
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF3
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/FISHDRONEF4/target.h

@@ -80,8 +80,8 @@
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI2
 #define MAX7456_SPI_CS_PIN      SPI2_NSS_PIN
-#define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)
-#define MAX7456_RESTORE_CLK     (SPI_CLOCK_FAST)
+#define MAX7456_SPI_CLK         SPI_CLOCK_STANDARD
+#define MAX7456_RESTORE_CLK     SPI_CLOCK_FAST
 
 // *************** TF Support *****************************
 #define USE_SPI_DEVICE_3
@@ -96,11 +96,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI3
 #define SDCARD_SPI_CS_PIN                   PB6
 
-// SPI3 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/FURYF3/target.h

@@ -99,11 +99,6 @@
 #define SDCARD_SPI_CS_GPIO                  SPI2_GPIO
 #define SDCARD_SPI_CS_PIN                   SPI2_NSS_PIN
 
-// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     2
-
 // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx.
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Channel5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5

src/main/target/KFC32F3_INAV/target.h

@@ -75,8 +75,8 @@
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI2
 #define MAX7456_SPI_CS_PIN      PA7
-#define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)
-#define MAX7456_RESTORE_CLK     (SPI_CLOCK_FAST)
+#define MAX7456_SPI_CLK         SPI_CLOCK_STANDARD
+#define MAX7456_RESTORE_CLK     SPI_CLOCK_FAST
 
 //#define RFM_SPI             SPI2
 //#define RFM_SPI_CS_PIN      PC15

src/main/target/KROOZX/target.h

@@ -62,8 +62,6 @@
 #define SDCARD_DETECT_PIN                   PC13
 #define SDCARD_SPI_INSTANCE                 SPI3
 #define SDCARD_SPI_CS_PIN                   PA15
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
 
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5
@@ -77,8 +75,8 @@
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI1
 #define MAX7456_SPI_CS_PIN      PC4
-#define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)
-#define MAX7456_RESTORE_CLK     (SPI_CLOCK_FAST)
+#define MAX7456_SPI_CLK         SPI_CLOCK_STANDARD
+#define MAX7456_RESTORE_CLK     SPI_CLOCK_FAST
 
 #define OSD_CH_SWITCH           PC5
 

src/main/target/MATEKF405/target.h

@@ -62,11 +62,6 @@
 #define SDCARD_SPI_INSTANCE     SPI3
 #define SDCARD_SPI_CS_PIN       PC1
 
-// SPI3 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               	DMA1_Stream7
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG 	DMA_FLAG_TCIF7
 #define SDCARD_DMA_CLK                      	RCC_AHB1Periph_DMA1

src/main/target/OMNIBUS/target.h

@@ -118,11 +118,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI2
 #define SDCARD_SPI_CS_PIN                   SPI2_NSS_PIN
 
-// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     2
-
 // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx.
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Channel5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5
@@ -134,8 +129,8 @@
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI1
 #define MAX7456_SPI_CS_PIN      PB1
-#define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)
-#define MAX7456_RESTORE_CLK     (SPI_CLOCK_FAST)
+#define MAX7456_SPI_CLK         SPI_CLOCK_STANDARD
+#define MAX7456_RESTORE_CLK     SPI_CLOCK_FAST
 //#define MAX7456_DMA_CHANNEL_TX            DMA1_Channel3
 //#define MAX7456_DMA_CHANNEL_RX            DMA1_Channel2
 //#define MAX7456_DMA_IRQ_HANDLER_ID        DMA1_CH3_HANDLER
@@ -159,7 +154,7 @@
 //#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 #define DEFAULT_RX_TYPE         RX_TYPE_PPM
-#define DEFAULT_FEATURES        (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX)
+#define DEFAULT_FEATURES        (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_OSD)
 
 #define BUTTONS
 #define BUTTON_A_PORT           GPIOB // Non-existent (PB1 used for RSSI/MAXCS)

src/main/target/OMNIBUSF4/DYSF4PRO.mk

@@ -0,0 +1 @@
+#DYSF4PRO

src/main/target/OMNIBUSF4/target.h

@@ -21,11 +21,17 @@
 #define TARGET_BOARD_IDENTIFIER "OBSD"
 #elif defined(OMNIBUSF4V3)
 #define TARGET_BOARD_IDENTIFIER "OB43"
+#elif defined(DYSF4PRO)
+#define TARGET_BOARD_IDENTIFIER "DYS4"
 #else
 #define TARGET_BOARD_IDENTIFIER "OBF4"
 #endif
 
+#if defined(DYSF4PRO)
+#define USBD_PRODUCT_STRING "DysF4Pro"
+#else
 #define USBD_PRODUCT_STRING     "Omnibus F4"
+#endif
 
 #define LED0                    PB5
 
@@ -55,6 +61,16 @@
 #define ACC
 #define USE_ACC_SPI_MPU6000
 
+// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000
+#if defined (OMNIBUSF4PRO)
+  #define USE_ACC_SPI_MPU6500
+  #define USE_GYRO_SPI_MPU6500
+  #define MPU6500_CS_PIN          MPU6000_CS_PIN
+  #define MPU6500_SPI_INSTANCE    MPU6000_SPI_INSTANCE
+  #define GYRO_MPU6500_ALIGN      GYRO_MPU6000_ALIGN
+  #define ACC_MPU6500_ALIGN       ACC_MPU6000_ALIGN
+#endif
+
 #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
   #define GYRO_MPU6000_ALIGN      CW270_DEG
   #define ACC_MPU6000_ALIGN       CW270_DEG
@@ -143,8 +159,8 @@
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI3
 #define MAX7456_SPI_CS_PIN      PA15
-#define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)
-#define MAX7456_RESTORE_CLK     (SPI_CLOCK_FAST)
+// #define MAX7456_SPI_CLK         SPI_CLOCK_STANDARD
+// #define MAX7456_RESTORE_CLK     SPI_CLOCK_FAST
 
 #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
   #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
@@ -156,11 +172,6 @@
   #define SDCARD_SPI_INSTANCE             SPI2
   #define SDCARD_SPI_CS_PIN               SPI2_NSS_PIN
 
-  // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-  #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-  // Divide to under 25MHz for normal operation:
-  #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz
-
   #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream4
   #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
   #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1
@@ -176,7 +187,13 @@
 #define USE_ADC
 #define ADC_CHANNEL_1_PIN               PC1
 #define ADC_CHANNEL_2_PIN               PC2
-#define ADC_CHANNEL_3_PIN               PA0
+
+#ifdef DYSF4PRO
+    #define ADC_CHANNEL_3_PIN               PC3
+#else
+    #define ADC_CHANNEL_3_PIN               PA0
+#endif
+
 #define CURRENT_METER_ADC_CHANNEL       ADC_CHN_1
 #define VBAT_ADC_CHANNEL                ADC_CHN_2
 #define RSSI_ADC_CHANNEL                ADC_CHN_3
@@ -192,7 +209,7 @@
 
 #define DEFAULT_RX_TYPE         RX_TYPE_PPM
 #define DISABLE_RX_PWM_FEATURE
-#define DEFAULT_FEATURES        (FEATURE_BLACKBOX | FEATURE_VBAT)
+#define DEFAULT_FEATURES        (FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD)
 
 #define SPEKTRUM_BIND
 #define BIND_PIN                PB11 // USART3 RX

src/main/target/OMNIBUSF4/target.mk

@@ -3,6 +3,8 @@ FEATURES       += VCP ONBOARDFLASH
 
 TARGET_SRC = \
 			drivers/accgyro/accgyro_spi_mpu6000.c \
+			drivers/accgyro/accgyro_spi_mpu6500.c \
+			drivers/accgyro/accgyro_mpu6500.c \
             drivers/barometer/barometer_bmp085.c \
             drivers/barometer/barometer_bmp280.c \
             drivers/barometer/barometer_spi_bmp280.c \

src/main/target/OMNIBUSF7/target.h

@@ -129,8 +129,8 @@
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI2
 #define MAX7456_SPI_CS_PIN      SPI2_NSS_PIN
-#define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)
-#define MAX7456_RESTORE_CLK     (SPI_CLOCK_FAST)
+#define MAX7456_SPI_CLK         SPI_CLOCK_STANDARD
+#define MAX7456_RESTORE_CLK     SPI_CLOCK_FAST
 
 #define USE_SDCARD
 #define SDCARD_DETECT_INVERTED
@@ -143,10 +143,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI4
 #define SDCARD_SPI_CS_PIN                   SPI4_NSS_PIN
 
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 422kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA2_Stream1
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA2

src/main/target/PIXRACER/target.h

@@ -74,11 +74,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI3
 #define SDCARD_SPI_CS_PIN                   PD2
 
-// SPI is on the APB2 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/SPRACINGF3EVO/target.h

@@ -112,11 +112,6 @@
 #define SDCARD_DETECT_PIN       PC14
 #define SDCARD_SPI_INSTANCE     SPI2
 #define SDCARD_SPI_CS_PIN       SPI2_NSS_PIN
-// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     2
-// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx.
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Channel5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5
 

src/main/target/SPRACINGF3MINI/target.h

@@ -106,11 +106,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI2
 #define SDCARD_SPI_CS_PIN                   SPI2_NSS_PIN
 
-// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     2
-
 // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx.
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Channel5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5

src/main/target/SPRACINGF3NEO/target.h

@@ -135,11 +135,6 @@
 #define SDCARD_SPI_INSTANCE     SPI2
 #define SDCARD_SPI_CS_PIN       SPI2_NSS_PIN
 
-// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     2
-
 // Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx.
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Channel5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5

src/main/target/SPRACINGF4EVO/target.h

@@ -125,9 +125,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI2
 #define SDCARD_SPI_CS_PIN                   SPI2_NSS_PIN
 
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream4
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/YUPIF4/target.h

@@ -106,11 +106,6 @@
 #define SDCARD_SPI_INSTANCE                 SPI3
 #define SDCARD_SPI_CS_PIN                   PA15
 
-// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
-#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
-// Divide to under 25MHz for normal operation:
-#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     4 // 21MHz
-
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream5
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1

src/main/target/common.h

@@ -97,6 +97,13 @@
 #define USE_RCSPLIT
 #define PITOT
 #define USE_PITOT_ADC
+
+//Enable VTX controll
+#define VTX_COMMON
+#define VTX_CONTROL
+#define VTX_SMARTAUDIO
+#define VTX_TRAMP
+
 #else
 #define CLI_MINIMAL_VERBOSITY
 #define SKIP_TASK_STATISTICS
@@ -105,4 +112,4 @@
 #define DISABLE_UNCOMMON_MIXERS
 #define NAV_MAX_WAYPOINTS       30
 #define MAX_BOOTLOG_ENTRIES     32
-#endif
+#endif

src/main/target/common_post.h

@@ -0,0 +1,25 @@
+/*
+ * 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/>.
+ */
+
+// Touch up configuration
+
+#pragma once
+
+// Targets with built-in vtx do not need external vtx
+#if defined(VTX) || defined(USE_RTC6705)
+# undef VTX_CONTROL
+#endif

src/test/Makefile

@@ -706,6 +706,20 @@ $(OBJECT_DIR)/time_unittest : \
 
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
+$(OBJECT_DIR)/bitarray_unittest.o : \
+	$(TEST_DIR)/bitarray_unittest.cc \
+	$(USER_DIR)/common/bitarray.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/bitarray_unittest.cc -o $@
+
+$(OBJECT_DIR)/bitarray_unittest : \
+	$(OBJECT_DIR)/common/bitarray.o \
+	$(OBJECT_DIR)/bitarray_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
 
 test: $(TESTS:%=test-%)

src/test/unit/bitarray_unittest.cc

@@ -0,0 +1,66 @@
+#include <cstdint>
+
+extern "C" {
+#include "common/bitarray.h"
+}
+
+#include "gtest/gtest.h"
+
+TEST(BitArrayTest, TestGetSet)
+{
+    uint32_t array[1] = {0};
+    void *p = array;
+
+    bitArraySet(p, 14);
+    EXPECT_EQ(bitArrayGet(p, 14), true);
+    EXPECT_EQ(bitArrayGet(p, 13), false);
+    EXPECT_EQ(bitArrayGet(p, 15), false);
+
+    EXPECT_EQ(bitArrayGet(p, 0), false);
+    bitArraySet(p, 0);
+    EXPECT_EQ(bitArrayGet(p, 0), true);
+}
+
+TEST(BitArrayTest, TestClr)
+{
+    uint32_t array[1] = {0};
+    void *p = array;
+
+    bitArraySet(p, 31);
+    EXPECT_EQ(bitArrayGet(p, 31), true);
+    EXPECT_EQ(bitArrayGet(p, 30), false);
+    EXPECT_EQ(bitArrayGet(p, 0), false);
+    bitArrayClr(p, 31);
+    EXPECT_EQ(bitArrayGet(p, 31), false);
+}
+
+TEST(BitArrayTest, TestFind)
+{
+    uint32_t array[4] = {0, 0, 0, 0};
+    void *p = array;
+
+    EXPECT_EQ(bitArrayFindFirstSet(p, 0, sizeof(array)), -1);
+
+    bitArraySet(p, 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 0, sizeof(array)), 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 16, sizeof(array)), 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 17, sizeof(array)), 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 18, sizeof(array)), -1);
+
+    bitArraySet(p, 44);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 0, sizeof(array)), 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 16, sizeof(array)), 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 17, sizeof(array)), 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 18, sizeof(array)), 44);
+
+    bitArrayClr(p, 17);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 0, sizeof(array)), 44);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 16, sizeof(array)), 44);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 17, sizeof(array)), 44);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 18, sizeof(array)), 44);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 45, sizeof(array)), -1);
+
+    bitArrayClr(p, 44);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 0, sizeof(array)), -1);
+    EXPECT_EQ(bitArrayFindFirstSet(p, 64, sizeof(array)), -1);
+}