diff --git a/Makefile b/Makefile
index fdc04f36c..417fe68d8 100644
--- a/Makefile
+++ b/Makefile
@@ -606,6 +606,7 @@ HIGHEND_SRC = \
             telemetry/ltm.c \
             telemetry/mavlink.c \
             sensors/esc_sensor.c \
+            io/vtx_smartaudio.c
 
 SPEED_OPTIMISED_SRC := ""
 SIZE_OPTIMISED_SRC  := ""
@@ -709,7 +710,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             cms/cms_menu_ledstrip.c \
             cms/cms_menu_misc.c \
             cms/cms_menu_osd.c \
-            cms/cms_menu_vtx.c
+            cms/cms_menu_vtx.c \
+            io/vtx_smartaudio.c
 endif #F3
 
 ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 8ec5c3c3c..33b01b35d 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -230,13 +230,28 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_Submenu:
     case OME_Funcall:
         if (IS_PRINTVALUE(p))  {
-            cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">");
+
+            int colPos = RIGHT_MENU_COLUMN(pDisplay);
+
+            if ((p->type == OME_Submenu) && p->func && (p->flags & OPTSTRING)) {
+
+                // Special case of sub menu entry with optional value display.
+
+                char *str = ((CMSMenuOptFuncPtr)p->func)();
+                cnt = displayWrite(pDisplay, colPos, row, str);
+                colPos += strlen(str);
+            }
+
+            cnt += displayWrite(pDisplay, colPos, row, ">");
+
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_Bool:
         if (IS_PRINTVALUE(p) && p->data) {
             if (*((uint8_t *)(p->data))) {
@@ -247,7 +262,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
-    case OME_TAB: {
+
+    case OME_TAB:
         if (IS_PRINTVALUE(p)) {
             OSD_TAB_t *ptr = p->data;
             //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]);
@@ -255,7 +271,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
-    }
+
 #ifdef OSD
     case OME_VISIBLE:
         if (IS_PRINTVALUE(p) && p->data) {
@@ -273,6 +289,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
         }
         break;
 #endif
+
     case OME_UINT8:
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_UINT8_t *ptr = p->data;
@@ -282,6 +299,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_INT8:
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_INT8_t *ptr = p->data;
@@ -291,6 +309,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_UINT16:
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_UINT16_t *ptr = p->data;
@@ -300,6 +319,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_INT16:
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_UINT16_t *ptr = p->data;
@@ -309,6 +329,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_FLOAT:
         if (IS_PRINTVALUE(p) && p->data) {
             OSD_FLOAT_t *ptr = p->data;
@@ -318,6 +339,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_Label:
         if (IS_PRINTVALUE(p) && p->data) {
             // A label with optional string, immediately following text
@@ -325,10 +347,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
             CLR_PRINTVALUE(p);
         }
         break;
+
     case OME_OSD_Exit:
     case OME_END:
     case OME_Back:
         break;
+
     case OME_MENU:
         // Fall through
     default:
@@ -645,17 +669,34 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
 
     switch (p->type) {
         case OME_Submenu:
-        case OME_Funcall:
+            if (key == KEY_RIGHT) {
+                cmsMenuChange(pDisplay, p->data);
+                res = BUTTON_PAUSE;
+            }
+            break;
+
+        case OME_Funcall:;
+            long retval;
+            if (p->func && key == KEY_RIGHT) {
+                retval = p->func(pDisplay, p->data);
+                if (retval == MENU_CHAIN_BACK)
+                    cmsMenuBack(pDisplay);
+                res = BUTTON_PAUSE;
+            }
+            break;
+
         case OME_OSD_Exit:
             if (p->func && key == KEY_RIGHT) {
                 p->func(pDisplay, p->data);
                 res = BUTTON_PAUSE;
             }
             break;
+
         case OME_Back:
             cmsMenuBack(pDisplay);
             res = BUTTON_PAUSE;
             break;
+
         case OME_Bool:
             if (p->data) {
                 uint8_t *val = p->data;
@@ -666,6 +707,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
                 SET_PRINTVALUE(p);
             }
             break;
+
 #ifdef OSD
         case OME_VISIBLE:
             if (p->data) {
@@ -682,6 +724,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
             }
             break;
 #endif
+
         case OME_UINT8:
         case OME_FLOAT:
             if (p->data) {
@@ -700,6 +743,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
                 }
             }
             break;
+
         case OME_TAB:
             if (p->type == OME_TAB) {
                 OSD_TAB_t *ptr = p->data;
@@ -717,6 +761,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
                 SET_PRINTVALUE(p);
             }
             break;
+
         case OME_INT8:
             if (p->data) {
                 OSD_INT8_t *ptr = p->data;
@@ -734,6 +779,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
                 }
             }
             break;
+
         case OME_UINT16:
             if (p->data) {
                 OSD_UINT16_t *ptr = p->data;
@@ -751,6 +797,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
                 }
             }
             break;
+
         case OME_INT16:
             if (p->data) {
                 OSD_INT16_t *ptr = p->data;
@@ -768,11 +815,14 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
                 }
             }
             break;
+
         case OME_String:
             break;
+
         case OME_Label:
         case OME_END:
             break;
+
         case OME_MENU:
             // Shouldn't happen
             break;
diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c
index 50a85a4c0..a0dc6b19e 100644
--- a/src/main/cms/cms_menu_builtin.c
+++ b/src/main/cms/cms_menu_builtin.c
@@ -45,6 +45,9 @@
 #include "cms/cms_menu_ledstrip.h"
 #include "cms/cms_menu_misc.h"
 
+// User supplied menus
+
+#include "io/vtx_smartaudio_cms.h"
 
 // Info
 
@@ -93,6 +96,9 @@ static OSD_Entry menuFeaturesEntries[] =
 #if defined(VTX) || defined(USE_RTC6705)
     {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0},
 #endif // VTX || USE_RTC6705
+#if defined(VTX_SMARTAUDIO)
+    {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0},
+#endif
 #ifdef LED_STRIP
     {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0},
 #endif // LED_STRIP
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index f95560a2c..26cce4558 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -362,14 +362,13 @@ static OSD_Entry cmsx_menuImuEntries[] =
 
     {"PID PROF",  OME_UINT8,   cmsx_profileIndexOnChange,     &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1},    0},
     {"PID",       OME_Submenu, cmsMenuChange,                 &cmsx_menuPid,                                                 0},
-    {"OTHER PP",  OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther,                                        0},
+    {"MISC PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther,                                        0},
+    {"FILT PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterPerProfile,                                    0},
 
     {"RATE PROF", OME_UINT8,   cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0},
     {"RATE",      OME_Submenu, cmsMenuChange,                 &cmsx_menuRateProfile,                                         0},
 
-    {"FLT PP",    OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterPerProfile,                                    0},
-
-    {"FLT GLB",   OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal,                                        0},
+    {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal,                                        0},
 
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h
index 427e1d38e..4bea82e62 100644
--- a/src/main/cms/cms_types.h
+++ b/src/main/cms/cms_types.h
@@ -66,6 +66,7 @@ typedef struct
 #define PRINT_VALUE    0x01  // Value has been changed, need to redraw
 #define PRINT_LABEL    0x02  // Text label should be printed
 #define DYNAMIC        0x04  // Value should be updated dynamically
+#define OPTSTRING      0x08  // (Temporary) Flag for OME_Submenu, indicating func should be called to get a string to display.
 
 #define IS_PRINTVALUE(p) ((p)->flags & PRINT_VALUE)
 #define SET_PRINTVALUE(p) { (p)->flags |= PRINT_VALUE; }
@@ -77,9 +78,11 @@ typedef struct
 
 #define IS_DYNAMIC(p) ((p)->flags & DYNAMIC)
 
-
 typedef long (*CMSMenuFuncPtr)(void);
 
+// Special return value(s) for function chaining by CMSMenuFuncPtr
+#define MENU_CHAIN_BACK  (-1) // Causes automatic cmsMenuBack
+
 /*
 onExit function is called with self:
 (1) Pointer to an OSD_Entry when cmsMenuBack() was called.
@@ -153,3 +156,7 @@ typedef struct
 {
     char *val;
 } OSD_String_t;
+
+// This is a function used in the func member if the type is OME_Submenu.
+
+typedef char * (*CMSMenuOptFuncPtr)(void);
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index e2e51979d..cbe9a26c0 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -198,7 +198,7 @@ typedef struct master_s {
     uint8_t transponderData[6];
 #endif
 
-#ifdef USE_RTC6705
+#if defined(USE_RTC6705)
     uint8_t vtx_channel;
     uint8_t vtx_power;
 #endif
diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 3e4d541d8..d1d1a7188 100755
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -37,9 +37,8 @@
 #include "max7456.h"
 #include "max7456_symbols.h"
 
-
-
 //MAX7456 opcodes
+//XXX These are not opcodes but reg addrs. Substitute with MAX7456ADD_xxx.
 #define DMM_REG   0x04
 #define DMAH_REG  0x05
 #define DMAL_REG  0x06
@@ -52,15 +51,21 @@
 #define MAX7456_RESET               0x02
 #define VERTICAL_SYNC_NEXT_VSYNC    0x04
 #define OSD_ENABLE                  0x08
+
 #define SYNC_MODE_AUTO              0x00
 #define SYNC_MODE_INTERNAL          0x30
 #define SYNC_MODE_EXTERNAL          0x20
+
 #define VIDEO_MODE_PAL              0x40
 #define VIDEO_MODE_NTSC             0x00
+#define VIDEO_MODE_MASK             0x40
+#define VIDEO_MODE_IS_PAL(val)      (((val) & VIDEO_MODE_MASK) == VIDEO_MODE_PAL)
+#define VIDEO_MODE_IS_NTSC(val)     (((val) & VIDEO_MODE_MASK) == VIDEO_MODE_NTSC)
+
+#define VIDEO_SIGNAL_DEBOUNCE_MS    100 // Time to wait for input to stabilize
 
 // video mode register 1 bits
 
-
 // duty cycle is on_off
 #define BLINK_DUTY_CYCLE_50_50 0x00
 #define BLINK_DUTY_CYCLE_33_66 0x01
@@ -85,9 +90,33 @@
 
 #define BACKGROUND_MODE_GRAY 0x40
 
-//MAX7456 commands
+// STAT register bits
+
+#define STAT_PAL      0x01
+#define STAT_NTSC     0x02
+#define STAT_LOS      0x04
+#define STAT_NVR_BUSY 0x20
+
+#define STAT_IS_PAL(val)  ((val) & STAT_PAL)
+#define STAT_IS_NTSC(val) ((val) & STAT_NTSC)
+#define STAT_IS_LOS(val)  ((val) & STAT_LOS)
+
+#define VIN_IS_PAL(val)  (!STAT_IS_LOS(val) && STAT_IS_PAL(val))
+#define VIN_IS_NTSC(val)  (!STAT_IS_LOS(val) && STAT_IS_NTSC(val))
+
+// Kluege warning!
+// There are occasions that NTSC is not detected even with !LOS (AB7456 specific?)
+// When this happens, lower 3 bits of STAT register is read as zero.
+// To cope with this case, this macro defines !LOS && !PAL as NTSC.
+// Should be compatible with MAX7456 and non-problematic case.
+
+#define VIN_IS_NTSC_alt(val)  (!STAT_IS_LOS(val) && !STAT_IS_PAL(val))
+
+// DMM special bits
 #define CLEAR_DISPLAY 0x04
 #define CLEAR_DISPLAY_VERT 0x06
+
+// Special address for terminating incremental write
 #define END_STRING 0xff
 
 
@@ -125,7 +154,6 @@
 
 #define NVM_RAM_SIZE            54
 #define WRITE_NVR               0xA0
-#define STATUS_REG_NVR_BUSY     0x20
 
 /** Line multiples, for convenience & one less op at runtime **/
 #define LINE      30
@@ -147,12 +175,6 @@
 #define LINE16    450
 
 
-
-
-
-
-
-
 //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);}
@@ -315,8 +337,12 @@ uint8_t max7456GetRowsCount(void)
     return (videoSignalReg & VIDEO_MODE_PAL) ? VIDEO_LINES_PAL : VIDEO_LINES_NTSC;
 }
 
+#if 0
+// XXX Remove this comment, too. 
 //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
+#endif
+
 void max7456ReInit(void)
 {
     uint8_t maxScreenRows;
@@ -324,9 +350,13 @@ void max7456ReInit(void)
     uint16_t x;
     static bool firstInit = true;
 
+#if 0
+// XXX We don't have to wait for the cam any more.
+// XXX Remove this when everything is stable.
     //do not init MAX before camera power up correctly
     if (millis() < 1500)
         return;
+#endif
 
     ENABLE_MAX7456;
 
@@ -334,15 +364,22 @@ void max7456ReInit(void)
         case VIDEO_SYSTEM_PAL:
             videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
             break;
+
         case VIDEO_SYSTEM_NTSC:
             videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
             break;
+
         case VIDEO_SYSTEM_AUTO:
             srdata = max7456Send(MAX7456ADD_STAT, 0x00);
-            if ((0x02 & srdata) == 0x02)
+
+            if (VIN_IS_NTSC(srdata)) {
                 videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
-            else
+            } else if (VIN_IS_PAL(srdata)) {
                 videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
+            } else {
+                // No valid input signal, fallback to default (XXX NTSC for now)
+                videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
+            }
             break;
     }
 
@@ -436,22 +473,60 @@ bool max7456DmaInProgres(void)
 }
 #endif
 
+#include "build/debug.h"
+
 void max7456DrawScreen(void)
 {
-    uint8_t check;
+    uint8_t stallCheck;
+    uint8_t videoSense;
+    static uint32_t videoDetectTimeMs = 0;
     static uint16_t pos = 0;
     int k = 0, buff_len=0;
 
     if (!max7456Lock && !fontIsLoading) {
-        //-----------------detect MAX7456 fail, or initialize it at startup when it is ready--------
+
+        // Detect MAX7456 fail, or initialize it at startup when it is ready
+
         max7456Lock = true;
         ENABLE_MAX7456;
-        check = max7456Send(VM0_REG | 0x80, 0x00);
+        stallCheck = max7456Send(VM0_REG | 0x80, 0x00);
         DISABLE_MAX7456;
 
-        if ( check != videoSignalReg)
+        if (stallCheck != videoSignalReg) {
             max7456ReInit();
 
+        } else if (videoSignalCfg == VIDEO_SYSTEM_AUTO) {
+
+            // Adjust output format based on the current input format.
+
+            ENABLE_MAX7456;
+            videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
+            DISABLE_MAX7456;
+
+            debug[0] = videoSignalReg & VIDEO_MODE_MASK;
+            debug[1] = videoSense & 0x7;
+            debug[3] = max7456GetRowsCount();
+
+            if (videoSense & STAT_LOS) {
+                videoDetectTimeMs = 0;
+            } else {
+                // There is a case that NTSC is not detected for some reason (AB7456 specific?)
+                // Here we force NTSC detection if not LOS and not PAL.
+                if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(videoSignalReg))
+                  || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(videoSignalReg))) {
+                    if (videoDetectTimeMs) {
+                        if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
+                            max7456ReInit();
+                            debug[2]++;
+                        }
+                    } else {
+                        // Wait for signal to stabilize
+                        videoDetectTimeMs = millis();
+                    }
+                }
+            }
+        }
+
         //------------   end of (re)init-------------------------------------
 
         for (k=0; k< MAX_CHARS2UPDATE; k++) {
@@ -545,12 +620,11 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
     max7456Send(MAX7456ADD_CMM, WRITE_NVR);
 
     // wait until bit 5 in the status register returns to 0 (12ms)
-    while ((max7456Send(MAX7456ADD_STAT, 0x00) & STATUS_REG_NVR_BUSY) != 0x00);
+    while ((max7456Send(MAX7456ADD_STAT, 0x00) & STAT_NVR_BUSY) != 0x00);
 
     DISABLE_MAX7456;
 
     max7456Lock = false;
 }
 
-
 #endif
diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h
index 5d337a729..bfad7e2aa 100644
--- a/src/main/drivers/serial.h
+++ b/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
diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c
index 599087f5d..493146644 100644
--- a/src/main/drivers/serial_uart_stm32f10x.c
+++ b/src/main/drivers/serial_uart_stm32f10x.c
@@ -123,7 +123,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_TX, 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_TX, 1);
@@ -194,7 +194,7 @@ 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_TX, 2);
-        IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), IOCFG_AF_OD);
+        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_TX, 2);
@@ -256,7 +256,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_TX, 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_TX, 3);
diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c
index 7b9464990..51f256038 100644
--- a/src/main/drivers/serial_uart_stm32f30x.c
+++ b/src/main/drivers/serial_uart_stm32f30x.c
@@ -117,8 +117,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_TX, index);
diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c
index 79d3e18ad..ccfedb397 100644
--- a/src/main/drivers/serial_uart_stm32f4xx.c
+++ b/src/main/drivers/serial_uart_stm32f4xx.c
@@ -340,7 +340,10 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
 
     if (options & SERIAL_BIDIR) {
         IOInit(tx, OWNER_SERIAL_TX, 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) {
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 0d63281d2..1678353ca 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -51,6 +51,7 @@
 #include "io/serial.h"
 #include "io/serial_cli.h"
 #include "io/transponder_ir.h"
+#include "io/vtx_smartaudio.h"
 
 #include "msp/msp_serial.h"
 
@@ -203,6 +204,19 @@ static void taskTelemetry(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef VTX_CONTROL
+// Everything that listens to VTX devices
+void taskVtxControl(uint32_t currentTime)
+{
+    if (ARMING_FLAG(ARMED))
+        return;
+
+#ifdef VTX_SMARTAUDIO
+    smartAudioProcess(currentTime);
+#endif
+}
+#endif
+
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -281,6 +295,11 @@ void fcTasksInit(void)
 #ifdef STACK_CHECK
     setTaskEnabled(TASK_STACK_CHECK, true);
 #endif
+#ifdef VTX_CONTROL
+#ifdef VTX_SMARTAUDIO
+    setTaskEnabled(TASK_VTXCTRL, true);
+#endif
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -467,4 +486,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+
+#ifdef VTX_CONTROL
+    [TASK_VTXCTRL] = {
+        .taskName = "VTXCTRL",
+        .taskFunc = taskVtxControl,
+        .desiredPeriod = TASK_PERIOD_HZ(5),          // 5Hz @200msec
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
 };
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 02f00d823..c8c9a6667 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -37,7 +37,8 @@ typedef enum {
     FUNCTION_BLACKBOX            = (1 << 7), // 128
 
     FUNCTION_TELEMETRY_MAVLINK   = (1 << 9), // 512
-    FUNCTION_ESC_SENSOR          = (1 << 10) // 1024
+    FUNCTION_ESC_SENSOR          = (1 << 10),// 1024
+    FUNCTION_VTX_CONTROL         = (1 << 11),// 2048
 } serialPortFunction_e;
 
 typedef enum {
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index be7408bfc..f831d8281 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -739,7 +739,7 @@ const clivalue_t valueTable[] = {
 #ifdef LED_STRIP
     { "ledstrip_visual_beeper",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &ledStripConfig()->ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
 #endif
-#ifdef USE_RTC6705
+#if defined(USE_RTC6705)
     { "vtx_channel",                VAR_UINT8  | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0,  39 } },
     { "vtx_power",                  VAR_UINT8  | MASTER_VALUE, &masterConfig.vtx_power,   .config.minmax = { 0,  1 } },
 #endif
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
new file mode 100644
index 000000000..2fbe94743
--- /dev/null
+++ b/src/main/io/vtx_smartaudio.c
@@ -0,0 +1,1293 @@
+/*
+ * 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 "platform.h"
+
+#ifdef VTX_SMARTAUDIO
+
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+
+#include "string.h"
+#include "common/printf.h"
+#include "common/utils.h"
+#include "drivers/system.h"
+#include "drivers/serial.h"
+#include "io/serial.h"
+#include "io/vtx_smartaudio.h"
+
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "flight/pid.h"
+#include "config/config_master.h"
+
+#include "build/build_config.h"
+
+//#define SMARTAUDIO_DPRINTF
+//#define SMARTAUDIO_DEBUG_MONITOR
+
+#ifdef SMARTAUDIO_DPRINTF
+
+#ifdef OMNIBUSF4
+#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
+#else
+#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1
+#endif
+
+serialPort_t *debugSerialPort = NULL;
+#define dprintf(x) if (debugSerialPort) printf x
+#else
+#define dprintf(x)
+#endif
+
+#include "build/debug.h"
+
+#ifdef CMS
+static void saUpdateStatusString(void); // Forward
+#endif
+
+static serialPort_t *smartAudioSerialPort = NULL;
+
+// SmartAudio command and response codes
+enum {
+    SA_CMD_NONE = 0x00,
+    SA_CMD_GET_SETTINGS = 0x01,
+    SA_CMD_SET_POWER,
+    SA_CMD_SET_CHAN,
+    SA_CMD_SET_FREQ,
+    SA_CMD_SET_MODE,
+    SA_CMD_GET_SETTINGS_V2 = 0x09        // Response only
+} smartAudioCommand_e;
+
+// This is not a good design; can't distinguish command from response this way.
+#define SACMD(cmd) (((cmd) << 1) | 1)
+
+// opmode flags, GET side
+#define SA_MODE_GET_FREQ_BY_FREQ            1
+#define SA_MODE_GET_PITMODE                 2
+#define SA_MODE_GET_IN_RANGE_PITMODE        4
+#define SA_MODE_GET_OUT_RANGE_PITMODE       8
+#define SA_MODE_GET_UNLOCK                 16
+#define SA_MODE_GET_DEFERRED_FREQ          32
+
+#define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE)
+#define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE))
+#define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE))
+
+// opmode flags, SET side
+#define SA_MODE_SET_IN_RANGE_PITMODE        1 // Immediate
+#define SA_MODE_SET_OUT_RANGE_PITMODE	    2 // Immediate
+#define SA_MODE_CLR_PITMODE                 4 // Immediate
+#define SA_MODE_SET_UNLOCK                  8
+#define SA_MODE_SET_LOCK                    0 // ~UNLOCK
+#define SA_MODE_SET_DEFERRED_FREQ          16
+
+// SetFrequency flags, for pit mode frequency manipulation
+#define SA_FREQ_GETPIT                      (1 << 14)
+#define SA_FREQ_SETPIT                      (1 << 15)
+#define SA_FREQ_MASK                        (~(SA_FREQ_GETPIT|SA_FREQ_SETPIT))
+
+// Statistical counters, for user side trouble shooting.
+
+typedef struct smartAudioStat_s {
+    uint16_t pktsent;
+    uint16_t pktrcvd;
+    uint16_t badpre;
+    uint16_t badlen;
+    uint16_t crc;
+    uint16_t ooopresp;
+    uint16_t badcode;
+} smartAudioStat_t;
+
+static smartAudioStat_t saStat = {
+    .pktsent = 0,
+    .pktrcvd = 0,
+    .badpre = 0,
+    .badlen = 0,
+    .crc = 0,
+    .ooopresp = 0,
+    .badcode = 0,
+};
+
+// The band/chan to frequency table
+// XXX Should really be consolidated among different vtx drivers
+static const uint16_t saFreqTable[5][8] =
+{
+    { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boacam A
+    { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B
+    { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E
+    { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark
+    { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand
+};
+// XXX Should frequencies not usable in locked state be detected?
+
+typedef struct saPowerTable_s {
+    int rfpower;
+    int16_t valueV1;
+    int16_t valueV2;
+} saPowerTable_t;
+
+static saPowerTable_t saPowerTable[] = {
+    {  25,   7,   0 },
+    { 200,  16,   1 },
+    { 500,  25,   2 },
+    { 800,  40,   3 },
+};
+
+// Last received device ('hard') states
+
+typedef struct smartAudioDevice_s {
+    int8_t version;
+    int8_t chan;
+    int8_t power;
+    int8_t mode;
+    uint16_t freq;
+    uint16_t orfreq;
+} smartAudioDevice_t;
+
+static smartAudioDevice_t saDevice = {
+    .version = 0,
+    .chan = -1,
+    .power = -1,
+    .mode = 0,
+    .freq = 0,
+    .orfreq = 0,
+};
+
+static smartAudioDevice_t saDevicePrev = {
+    .version = 0,
+};
+
+// XXX Possible compliance problem here. Need LOCK/UNLOCK menu?
+static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable?
+
+// XXX Should be configurable by user?
+static bool saDeferred = true; // saCms variable?
+
+// Receive frame reassembly buffer
+#define SA_MAX_RCVLEN 11
+static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard
+
+//
+// CRC8 computations
+//
+
+#define POLYGEN 0xd5
+
+static uint8_t CRC8(const uint8_t *data, const int8_t len)
+{
+    uint8_t crc = 0; /* start with 0 so first byte can be 'xored' in */
+    uint8_t currByte;
+
+    for (int i = 0 ; i < len ; i++) {
+        currByte = data[i];
+
+        crc ^= currByte; /* XOR-in the next input byte */
+
+        for (int i = 0; i < 8; i++) {
+            if ((crc & 0x80) != 0) {
+                crc = (uint8_t)((crc << 1) ^ POLYGEN);
+            } else {
+                crc <<= 1;
+            }
+        }
+    }
+    return crc;
+}
+
+
+static void saPrintSettings(void)
+{
+#ifdef SMARTAUDIO_DPRINTF
+    dprintf(("Current status: version: %d\r\n", saDevice.version));
+    dprintf(("  mode(0x%x): fmode=%s", saDevice.mode,  (saDevice.mode & 1) ? "freq" : "chan"));
+    dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off"));
+    dprintf((" inb=%s", (saDevice.mode & 4) ? "on " : "off"));
+    dprintf((" outb=%s", (saDevice.mode & 8) ? "on " : "off"));
+    dprintf((" lock=%s", (saDevice.mode & 16) ? "unlocked" : "locked"));
+    dprintf((" deferred=%s\r\n", (saDevice.mode & 32) ? "on" : "off"));
+    dprintf(("  chan: %d ", saDevice.chan));
+    dprintf(("freq: %d ", saDevice.freq));
+    dprintf(("power: %d ", saDevice.power));
+    dprintf(("pitfreq: %d ", saDevice.orfreq));
+    dprintf(("\r\n"));
+#endif
+}
+
+static int saDacToPowerIndex(int dac)
+{
+    int idx;
+
+    for (idx = 0 ; idx < 4 ; idx++) {
+        if (saPowerTable[idx].valueV1 <= dac)
+            return(idx);
+    }
+    return(3);
+}
+
+//
+// Autobauding
+//
+
+#define SMARTBAUD_MIN 4800
+#define SMARTBAUD_MAX 4950
+uint16_t sa_smartbaud = SMARTBAUD_MIN;
+static int sa_adjdir = 1; // -1=going down, 1=going up
+static int sa_baudstep = 50;
+
+#define SMARTAUDIO_CMD_TIMEOUT    120
+
+static void saAutobaud(void)
+{
+    if (saStat.pktsent < 10)
+        // Not enough samples collected
+        return;
+
+#if 0
+    dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
+        sa_smartbaud, saStat.pktrcvd, saStat.pktsent, ((saStat.pktrcvd * 100) / saStat.pktsent)));
+#endif
+
+    if (((saStat.pktrcvd * 100) / saStat.pktsent) >= 70) {
+        // This is okay
+        saStat.pktsent = 0; // Should be more moderate?
+        saStat.pktrcvd = 0;
+        return;
+    }
+
+    dprintf(("autobaud: adjusting\r\n"));
+
+    if ((sa_adjdir == 1) && (sa_smartbaud == SMARTBAUD_MAX)) {
+       sa_adjdir = -1;
+       dprintf(("autobaud: now going down\r\n"));
+    } else if ((sa_adjdir == -1 && sa_smartbaud == SMARTBAUD_MIN)) {
+       sa_adjdir = 1;
+       dprintf(("autobaud: now going up\r\n"));
+    }
+
+    sa_smartbaud += sa_baudstep * sa_adjdir;
+
+    dprintf(("autobaud: %d\r\n", sa_smartbaud));
+
+    smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud);
+
+    saStat.pktsent = 0;
+    saStat.pktrcvd = 0;
+}
+
+// Transport level variables
+
+static uint32_t sa_lastTransmission = 0;
+static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
+static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
+static int sa_oslen;         // And associate length
+
+#ifdef CMS
+void saCmsUpdate(void);
+#endif
+
+static void saProcessResponse(uint8_t *buf, int len)
+{
+    uint8_t resp = buf[0];
+
+    if (resp == sa_outstanding) {
+        sa_outstanding = SA_CMD_NONE;
+    } else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) {
+        sa_outstanding = SA_CMD_NONE;
+    } else {
+        saStat.ooopresp++;
+        dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp));
+    }
+
+    switch(resp) {
+    case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
+    case SA_CMD_GET_SETTINGS:    // Version 1 Get Settings
+        if (len < 7)
+            break;
+
+        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
+        saDevice.chan = buf[2];
+        saDevice.power = buf[3];
+        saDevice.mode = buf[4];
+        saDevice.freq = (buf[5] << 8)|buf[6];
+
+#ifdef SMARTAUDIO_DEBUG_MONITOR
+        debug[0] = saDevice.version * 100 + saDevice.mode;
+        debug[1] = saDevice.chan;
+        debug[2] = saDevice.freq;
+        debug[3] = saDevice.power;
+#endif
+        break;
+
+    case SA_CMD_SET_POWER: // Set Power
+        break;
+
+    case SA_CMD_SET_CHAN: // Set Channel
+        break;
+
+    case SA_CMD_SET_FREQ: // Set Frequency
+        if (len < 5)
+            break;
+
+        uint16_t freq = (buf[2] << 8)|buf[3];
+
+        if (freq & SA_FREQ_GETPIT) {
+            saDevice.orfreq = freq & SA_FREQ_MASK;
+            dprintf(("saProcessResponse: GETPIT freq %d\r\n", saDevice.orfreq));
+        } else if (freq & SA_FREQ_SETPIT) {
+            saDevice.orfreq = freq & SA_FREQ_MASK;
+            dprintf(("saProcessResponse: SETPIT freq %d\r\n", saDevice.orfreq));
+        } else {
+            saDevice.freq = freq;
+            dprintf(("saProcessResponse: SETFREQ freq %d\r\n", freq));
+        }
+        break;
+
+    case SA_CMD_SET_MODE: // Set Mode
+        dprintf(("saProcessResponse: SET_MODE 0x%x\r\n", buf[2]));
+        break;
+
+    default:
+        saStat.badcode++;
+        return;
+    }
+
+    // Debug
+    if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t)))
+        saPrintSettings();
+    saDevicePrev = saDevice;
+
+#ifdef CMS
+    // Export current device status for CMS
+    saCmsUpdate();
+    saUpdateStatusString();
+#endif
+}
+
+//
+// Datalink
+//
+
+static void saReceiveFramer(uint8_t c)
+{
+
+    static enum saFramerState_e {
+        S_WAITPRE1, // Waiting for preamble 1 (0xAA)
+        S_WAITPRE2, // Waiting for preamble 2 (0x55)
+        S_WAITRESP, // Waiting for response code
+        S_WAITLEN,  // Waiting for length
+        S_DATA,     // Receiving data
+        S_WAITCRC,  // Waiting for CRC
+    } state = S_WAITPRE1;
+
+    static int len;
+    static int dlen;
+
+    switch(state) {
+    case S_WAITPRE1:
+        if (c == 0xAA) {
+            state = S_WAITPRE2;
+        } else {
+            state = S_WAITPRE1; // Don't need this (no change)
+        }
+        break;
+
+    case S_WAITPRE2:
+        if (c == 0x55) {
+            state = S_WAITRESP;
+        } else {
+            saStat.badpre++;
+            state = S_WAITPRE1;
+        }
+        break;
+
+    case S_WAITRESP:
+        sa_rbuf[0] = c;
+        state = S_WAITLEN;
+        break;
+
+    case S_WAITLEN:
+        sa_rbuf[1] = c;
+        len = c;
+
+        if (len > SA_MAX_RCVLEN - 2) {
+            saStat.badlen++;
+            state = S_WAITPRE1;
+        } else if (len == 0) {
+            state = S_WAITCRC;
+        } else {
+            dlen = 0;
+            state = S_DATA;
+        }
+        break;
+
+    case S_DATA:
+        // XXX Should check buffer overflow (-> saerr_overflow)
+        sa_rbuf[2 + dlen] = c;
+        if (++dlen == len) {
+            state = S_WAITCRC;
+        }
+        break;
+
+    case S_WAITCRC:
+        if (CRC8(sa_rbuf, 2 + len) == c) {
+            // Got a response
+            saProcessResponse(sa_rbuf, len + 2);
+            saStat.pktrcvd++;
+        } else if (sa_rbuf[0] & 1) {
+            // Command echo
+            // XXX There is an exceptional case (V2 response)
+            // XXX Should check crc in the command format?
+        } else {
+            saStat.crc++;
+        }
+        state = S_WAITPRE1;
+        break;
+    }
+}
+
+static void saSendFrame(uint8_t *buf, int len)
+{
+    int i;
+
+    serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
+
+    for (i = 0 ; i < len ; i++)
+        serialWrite(smartAudioSerialPort, buf[i]);
+
+    serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
+
+    sa_lastTransmission = millis();
+    saStat.pktsent++;
+}
+
+/*
+ * Retransmission and command queuing
+ *
+ *   The transport level support includes retransmission on response timeout
+ * and command queueing.
+ *
+ * Resend buffer:
+ *   The smartaudio returns response for valid command frames in no less
+ * than 60msec, which we can't wait. So there's a need for a resend buffer.
+ *
+ * Command queueing:
+ *   The driver autonomously sends GetSettings command for auto-bauding,
+ * asynchronous to user initiated commands; commands issued while another
+ * command is outstanding must be queued for later processing.
+ *   The queueing also handles the case in which multiple commands are
+ * required to implement a user level command.
+ */
+
+// Retransmission
+
+static void saResendCmd(void)
+{
+    saSendFrame(sa_osbuf, sa_oslen);
+}
+
+static void saSendCmd(uint8_t *buf, int len)
+{
+    int i;
+
+    for (i = 0 ; i < len ; i++)
+        sa_osbuf[i] = buf[i];
+
+    sa_oslen = len;
+    sa_outstanding = (buf[2] >> 1);
+
+    saSendFrame(sa_osbuf, sa_oslen);
+}
+
+// Command queue management
+
+typedef struct saCmdQueue_s {
+    uint8_t *buf;
+    int len;
+} saCmdQueue_t;
+
+#define SA_QSIZE 4     // 1 heartbeat (GetSettings) + 2 commands + 1 slack
+static saCmdQueue_t sa_queue[SA_QSIZE];
+static uint8_t sa_qhead = 0;
+static uint8_t sa_qtail = 0;
+
+#ifdef DPRINTF_SMARTAUDIO
+static int saQueueLength()
+{
+    if (sa_qhead >= sa_qtail) {
+        return sa_qhead - sa_qtail;
+    } else {
+        return SA_QSIZE + sa_qhead - sa_qtail;
+    }
+}
+#endif
+
+static bool saQueueEmpty()
+{
+    return sa_qhead == sa_qtail;
+}
+
+static bool saQueueFull()
+{
+    return ((sa_qhead + 1) % SA_QSIZE) == sa_qtail;
+}
+
+static void saQueueCmd(uint8_t *buf, int len)
+{
+    if (saQueueFull())
+         return;
+
+    sa_queue[sa_qhead].buf = buf;
+    sa_queue[sa_qhead].len = len;
+    sa_qhead = (sa_qhead + 1) % SA_QSIZE;
+}
+
+static void saSendQueue(void)
+{
+    if (saQueueEmpty())
+         return;
+
+    saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
+    sa_qtail = (sa_qtail + 1) % SA_QSIZE;
+}
+
+// Individual commands
+
+static void saGetSettings(void)
+{
+    static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
+
+    saQueueCmd(bufGetSettings, 5);
+}
+
+static void saSetFreq(uint16_t freq)
+{
+    static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
+
+    if (freq & SA_FREQ_GETPIT) {
+        dprintf(("smartAudioSetFreq: GETPIT\r\n"));
+    } else if (freq & SA_FREQ_SETPIT) {
+        dprintf(("smartAudioSetFreq: SETPIT %d\r\n", freq & SA_FREQ_MASK));
+    } else {
+        dprintf(("smartAudioSetFreq: SET %d\r\n", freq));
+    }
+
+    buf[4] = (freq >> 8) & 0xff;
+    buf[5] = freq & 0xff;
+    buf[6] = CRC8(buf, 6);
+
+    saQueueCmd(buf, 7);
+}
+
+#if 0
+static void saSetPitFreq(uint16_t freq)
+{
+    saSetFreq(freq | SA_FREQ_SETPIT);
+}
+
+static void saGetPitFreq(void)
+{
+    saSetFreq(SA_FREQ_GETPIT);
+}
+#endif
+
+void saSetBandChan(uint8_t band, uint8_t chan)
+{
+    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
+
+    buf[4] = band * 8 + chan;
+    buf[5] = CRC8(buf, 5);
+
+    saQueueCmd(buf, 6);
+}
+
+static void saSetMode(int mode)
+{
+    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
+
+    buf[4] = (mode & 0x3f)|saLockMode;
+    buf[5] = CRC8(buf, 5);
+
+    saQueueCmd(buf, 6);
+}
+
+void saSetPowerByIndex(uint8_t index)
+{
+    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+
+    dprintf(("saSetPowerByIndex: index %d\r\n", index));
+
+    if (saDevice.version == 0) {
+        // Unknown or yet unknown version.
+        return;
+    }
+
+    if (index > 3)
+        return;
+
+    buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2;
+    buf[5] = CRC8(buf, 5);
+    saQueueCmd(buf, 6);
+}
+
+bool smartAudioInit()
+{
+#ifdef SMARTAUDIO_DPRINTF
+    // Setup debugSerialPort
+
+    debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0);
+    if (debugSerialPort) {
+        setPrintfSerialPort(debugSerialPort);
+        dprintf(("smartAudioInit: OK\r\n"));
+    }
+#endif
+
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL);
+    if (portConfig) {
+        smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP);
+    }
+
+    if (!smartAudioSerialPort) {
+        return false;
+    }
+
+    return true;
+}
+
+void smartAudioProcess(uint32_t now)
+{
+    static bool initialSent = false;
+
+    if (smartAudioSerialPort == NULL)
+        return;
+
+    while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
+        uint8_t c = serialRead(smartAudioSerialPort);
+        saReceiveFramer((uint16_t)c);
+    }
+
+    // Re-evaluate baudrate after each frame reception
+    saAutobaud();
+
+    if (!initialSent) {
+        saGetSettings();
+        saSetFreq(SA_FREQ_GETPIT);
+        saSendQueue();
+        initialSent = true;
+        return;
+    }
+
+    if ((sa_outstanding != SA_CMD_NONE)
+            && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
+        // Last command timed out
+        // dprintf(("process: resending 0x%x\r\n", sa_outstanding));
+        // XXX Todo: Resend termination and possible offline transition
+        saResendCmd();
+    } else if (!saQueueEmpty()) {
+        // Command pending. Send it.
+        // dprintf(("process: sending queue\r\n"));
+        saSendQueue();
+    } else if (now - sa_lastTransmission >= 1000) {
+        // Heart beat for autobauding
+        //dprintf(("process: sending heartbeat\r\n"));
+        saGetSettings();
+        saSendQueue();
+    }
+}
+
+#ifdef CMS
+
+// Interface to CMS
+
+// Operational Model and RF modes (CMS)
+
+#define SACMS_OPMODEL_UNDEF        0 // Not known yet
+#define SACMS_OPMODEL_FREE         1 // Freestyle model: Power up transmitting
+#define SACMS_OPMODEL_RACE         2 // Race model: Power up in pit mode
+
+uint8_t  saCmsOpmodel = SACMS_OPMODEL_UNDEF;
+
+#define SACMS_TXMODE_NODEF         0
+#define SACMS_TXMODE_PIT_OUTRANGE  1
+#define SACMS_TXMODE_PIT_INRANGE   2
+#define SACMS_TXMODE_ACTIVE        3
+
+uint8_t  saCmsRFState;          // RF state; ACTIVE, PIR, POR XXX Not currently used
+
+uint8_t  saCmsBand = 0;
+uint8_t  saCmsChan = 0;
+uint8_t  saCmsPower = 0;
+
+// Frequency derived from channel table (used for reference in band/chan mode)
+uint16_t saCmsFreqRef = 0;
+
+uint16_t saCmsDeviceFreq = 0;
+
+uint8_t  saCmsDeviceStatus = 0;
+uint8_t  saCmsPower;
+uint8_t  saCmsPitFMode;         // In-Range or Out-Range
+uint8_t  saCmsFselMode;          // Channel(0) or User defined(1)
+
+uint16_t saCmsORFreq = 0;       // POR frequency
+uint16_t saCmsORFreqNew;        // POR frequency
+
+uint16_t saCmsUserFreq = 0;     // User defined frequency
+uint16_t saCmsUserFreqNew;      // User defined frequency
+
+void saCmsUpdate(void)
+{
+// XXX Take care of pit mode update somewhere???
+
+    if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) {
+        // This is a first valid response to GET_SETTINGS.
+        saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE;
+
+        saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0;
+
+        saCmsBand = (saDevice.chan / 8) + 1;
+        saCmsChan = (saDevice.chan % 8) + 1;
+        saCmsFreqRef = saFreqTable[saDevice.chan / 8][saDevice.chan % 8];
+
+        saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8];
+
+        if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) {
+            saCmsRFState = SACMS_TXMODE_ACTIVE;
+        } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
+            saCmsRFState = SACMS_TXMODE_PIT_INRANGE;
+        } else {
+            saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE;
+        }
+
+        if (saDevice.version == 2) {
+            saCmsPower = saDevice.power + 1; // XXX Take care V1
+        } else {
+            saCmsPower = saDacToPowerIndex(saDevice.power) + 1;
+        }
+    }
+
+    saUpdateStatusString();
+}
+
+char saCmsStatusString[31] = "- -- ---- ---";
+//                            m bc ffff ppp
+//                            0123456789012
+
+static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self);
+static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self);
+static long saCmsConfigBandByGvar(displayPort_t *, const void *self);
+static long saCmsConfigChanByGvar(displayPort_t *, const void *self);
+static long saCmsConfigPowerByGvar(displayPort_t *, const void *self);
+
+static void saUpdateStatusString(void)
+{
+    if (saDevice.version == 0)
+        return;
+
+// XXX These should be done somewhere else
+if (saCmsDeviceStatus == 0 && saDevice.version != 0)
+    saCmsDeviceStatus = saDevice.version;
+if (saCmsORFreq == 0 && saDevice.orfreq != 0)
+    saCmsORFreq = saDevice.orfreq;
+if (saCmsUserFreq == 0 && saDevice.freq != 0)
+    saCmsUserFreq = saDevice.freq;
+
+if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
+    saCmsPitFMode = 1;
+else
+    saCmsPitFMode = 0;
+
+    saCmsStatusString[0] = "-FP"[(saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE];
+    saCmsStatusString[2] = "ABEFR"[saDevice.chan / 8];
+    saCmsStatusString[3] = '1' + (saDevice.chan % 8);
+
+    if ((saDevice.mode & SA_MODE_GET_PITMODE)
+       && (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE))
+        tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.orfreq);
+    else if (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ)
+        tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.freq);
+    else
+        tfp_sprintf(&saCmsStatusString[5], "%4d",
+            saFreqTable[saDevice.chan / 8][saDevice.chan % 8]);
+
+    saCmsStatusString[9] = ' ';
+
+    if (saDevice.mode & SA_MODE_GET_PITMODE) {
+        saCmsStatusString[10] = 'P';
+        if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
+            saCmsStatusString[11] = 'I';
+        } else {
+            saCmsStatusString[11] = 'O';
+        }
+        saCmsStatusString[12] = 'R';
+        saCmsStatusString[13] = 0;
+    } else {
+        tfp_sprintf(&saCmsStatusString[10], "%3d", (saDevice.version == 2) ?  saPowerTable[saDevice.power].rfpower : saPowerTable[saDacToPowerIndex(saDevice.power)].rfpower);
+    }
+}
+
+static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (saDevice.version == 0) {
+        // Bounce back; not online yet
+        saCmsBand = 0;
+        return 0;
+    }
+
+dprintf(("saCmsConfigBand: band req %d ", saCmsBand));
+
+    if (saCmsBand == 0) {
+        // Bouce back, no going back to undef state
+        saCmsBand = 1;
+        return 0;
+    }
+
+    if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred))
+        saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+
+    saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1];
+
+    return 0;
+}
+
+static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (saDevice.version == 0) {
+        // Bounce back; not online yet
+        saCmsChan = 0;
+        return 0;
+    }
+
+    if (saCmsChan == 0) {
+        // Bounce back; no going back to undef state
+        saCmsChan = 1;
+        return 0;
+    }
+
+    if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred))
+        saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+
+    saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1];
+
+    return 0;
+}
+
+static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (saDevice.version == 0) {
+        // Bounce back; not online yet
+        saCmsPower = 0;
+        return 0;
+    }
+
+    if (saCmsPower == 0) {
+        // Bouce back; no going back to undef state
+        saCmsPower = 1;
+        return 0;
+    }
+
+    saSetPowerByIndex(saCmsPower - 1);
+
+    return 0;
+}
+
+static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode));
+
+    if (saCmsPitFMode == 0) {
+        saSetMode(SA_MODE_SET_IN_RANGE_PITMODE);
+    } else {
+        saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE);
+    }
+
+    return 0;
+}
+
+static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    uint8_t opmodel = saCmsOpmodel;
+
+    dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel));
+
+    if (opmodel == SACMS_OPMODEL_FREE) {
+        // VTX should power up transmitting.
+        // Turn off In-Range and Out-Range bits
+        saSetMode(0);
+    } else if (opmodel == SACMS_OPMODEL_RACE) {
+        // VTX should power up in pit mode.
+        // Default PitFMode is in-range to prevent users without
+        // out-range receivers from getting blinded.
+        saCmsPitFMode = 0;
+        saCmsConfigPitFModeByGvar(pDisp, self);
+    } else {
+        // Trying to go back to unknown state; bounce back
+        saCmsOpmodel = SACMS_OPMODEL_UNDEF + 1;
+    }
+
+    return 0;
+}
+
+static const char * const saCmsDeviceStatusNames[] = {
+    "OFFL",
+    "ONL V1",
+    "ONL V2",
+};
+
+static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames };
+
+static OSD_Entry saCmsMenuStatsEntries[] = {
+    { "- SA STATS -", OME_Label, NULL, NULL, 0 },
+    { "STATUS",   OME_TAB,    NULL, &saCmsEntOnline,                              DYNAMIC },
+    { "BAUDRATE", OME_UINT16, NULL, &(OSD_UINT16_t){ &sa_smartbaud, 0, 0, 0 },    DYNAMIC },
+    { "SENT",     OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktsent, 0, 0, 0 },  DYNAMIC },
+    { "RCVD",     OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktrcvd, 0, 0, 0 },  DYNAMIC },
+    { "BADPRE",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badpre, 0, 0, 0 },   DYNAMIC },
+    { "BADLEN",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badlen, 0, 0, 0 },   DYNAMIC },
+    { "CRCERR",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.crc, 0, 0, 0 },      DYNAMIC },
+    { "OOOERR",   OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.ooopresp, 0, 0, 0 }, DYNAMIC },
+    { "BACK",     OME_Back,   NULL, NULL, 0 },
+    { NULL,       OME_END,    NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuStats = {
+    .GUARD_text = "XSAST",
+    .GUARD_type = OME_MENU,
+    .onEnter = NULL,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = saCmsMenuStatsEntries
+};
+
+static const char * const saCmsBandNames[] = {
+    "--------",
+    "BOSCAM A",
+    "BOSCAM B",
+    "BOSCAM E",
+    "FATSHARK",
+    "RACEBAND",
+};
+
+static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, &saCmsBandNames[0], NULL };
+
+static const char * const saCmsChanNames[] = {
+    "-", "1", "2", "3", "4", "5", "6", "7", "8",
+};
+
+static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &saCmsChanNames[0], NULL };
+
+static const char * const saCmsPowerNames[] = {
+    "---",
+    "25 ",
+    "200",
+    "500",
+    "800",
+};
+
+static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames};
+
+static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 };
+
+static const char * const saCmsOpmodelNames[] = {
+    "----",
+    "FREE",
+    "RACE",
+};
+
+static const char * const saCmsFselModeNames[] = {
+    "CHAN",
+    "USER"
+};
+
+static const char * const saCmsPitFModeNames[] = {
+    "PIR",
+    "POR"
+};
+
+static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames };
+
+static long sacms_SetupTopMenu(void); // Forward
+
+static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (saCmsFselMode == 0) {
+        // CHAN
+        saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+    } else {
+        // USER: User frequency mode is only available in FREE opmodel.
+        if (saCmsOpmodel == SACMS_OPMODEL_FREE) {
+            saSetFreq(saCmsUserFreq);
+        } else {
+            // Bounce back
+            saCmsFselMode = 0;
+        }
+    }
+
+    sacms_SetupTopMenu();
+
+    return 0;
+}
+
+static long saCmsCommence(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
+        if (saCmsPitFMode == 0)
+            saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE);
+        else
+            saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE);
+    } else {
+        if (saCmsFselMode == 0)
+            saSetBandChan(saCmsBand - 1, saCmsChan - 1);
+        else
+            saSetFreq(saCmsUserFreq);
+    }
+
+    return MENU_CHAIN_BACK;
+}
+
+static long saCmsSetPORFreqOnEnter(void)
+{
+    saCmsORFreqNew = saCmsORFreq;
+
+    return 0;
+}
+
+static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT);
+
+    return 0;
+}
+
+static char *saCmsORFreqGetString(void)
+{
+    static char pbuf[5];
+
+    tfp_sprintf(pbuf, "%4d", saCmsORFreq);
+
+    return pbuf;
+}
+
+static char *saCmsUserFreqGetString(void)
+{
+    static char pbuf[5];
+
+    tfp_sprintf(pbuf, "%4d", saCmsUserFreq);
+
+    return pbuf;
+}
+
+static long saCmsSetUserFreqOnEnter(void)
+{
+    saCmsUserFreqNew = saCmsUserFreq;
+
+    return 0;
+}
+
+static long saCmsSetUserFreq(displayPort_t *pDisp, const void *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    saCmsUserFreq = saCmsUserFreqNew;
+    saSetFreq(saCmsUserFreq);
+
+    return 0;
+}
+
+static OSD_Entry saCmsMenuPORFreqEntries[] = {
+    { "- POR FREQ -", OME_Label,   NULL,             NULL,                                                 0 },
+
+    { "CUR FREQ",     OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsORFreq, 5000, 5900, 0 },       DYNAMIC },
+    { "NEW FREQ",     OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsORFreqNew, 5000, 5900, 1 },    0 },
+    { "SET",          OME_Funcall, saCmsSetPORFreq,  NULL,                                                 0 },
+
+    { "BACK",         OME_Back,    NULL,             NULL,                                                 0 },
+    { NULL,           OME_END,     NULL,             NULL,                                                 0 }
+};
+
+static CMS_Menu saCmsMenuPORFreq =
+{
+    .GUARD_text = "XSAPOR",
+    .GUARD_type = OME_MENU,
+    .onEnter = saCmsSetPORFreqOnEnter,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = saCmsMenuPORFreqEntries,
+};
+
+static OSD_Entry saCmsMenuUserFreqEntries[] = {
+    { "- USER FREQ -", OME_Label,   NULL,             NULL,                                                0 },
+
+    { "CUR FREQ",      OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsUserFreq, 5000, 5900, 0 },    DYNAMIC },
+    { "NEW FREQ",      OME_UINT16,  NULL,             &(OSD_UINT16_t){ &saCmsUserFreqNew, 5000, 5900, 1 }, 0 },
+    { "SET",           OME_Funcall, saCmsSetUserFreq, NULL,                                                0 },
+
+    { "BACK",          OME_Back,    NULL,             NULL,                                                0 },
+    { NULL,            OME_END,     NULL,             NULL,                                                0 }
+};
+
+static CMS_Menu saCmsMenuUserFreq =
+{
+    .GUARD_text = "XSAUFQ",
+    .GUARD_type = OME_MENU,
+    .onEnter = saCmsSetUserFreqOnEnter,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = saCmsMenuUserFreqEntries,
+};
+
+static OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames };
+
+static OSD_Entry saCmsMenuConfigEntries[] = {
+    { "- SA CONFIG -", OME_Label, NULL, NULL, 0 },
+
+    { "OP MODEL",  OME_TAB,     saCmsConfigOpmodelByGvar,              &(OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, 0 },
+    { "FSEL MODE", OME_TAB,     saCmsConfigFreqModeByGvar,             &saCmsEntFselMode,                                   0 },
+    { "PIT FMODE", OME_TAB,     saCmsConfigPitFModeByGvar,             &saCmsEntPitFMode,                                   0 },
+    { "POR FREQ",  OME_Submenu, (CMSEntryFuncPtr)saCmsORFreqGetString, &saCmsMenuPORFreq,                                   OPTSTRING },
+    { "STATX",     OME_Submenu, cmsMenuChange,                         &saCmsMenuStats,                                     0 },
+
+    { "BACK", OME_Back, NULL, NULL, 0 },
+    { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuConfig = {
+    .GUARD_text = "XSACFG",
+    .GUARD_type = OME_MENU,
+    .onEnter = NULL,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = saCmsMenuConfigEntries
+};
+
+static OSD_Entry saCmsMenuCommenceEntries[] = {
+    { "CONFIRM", OME_Label,   NULL,          NULL, 0 },
+
+    { "YES",     OME_Funcall, saCmsCommence, NULL, 0 },
+
+    { "BACK",    OME_Back, NULL, NULL, 0 },
+    { NULL,      OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu saCmsMenuCommence = {
+    .GUARD_text = "XVTXCOM",
+    .GUARD_type = OME_MENU,
+    .onEnter = NULL,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = saCmsMenuCommenceEntries,
+};
+
+static OSD_Entry saCmsMenuFreqModeEntries[] = {
+    { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
+
+    { "",       OME_Label,   NULL,                                     saCmsStatusString,  DYNAMIC },
+    { "FREQ",   OME_Submenu, (CMSEntryFuncPtr)saCmsUserFreqGetString,  &saCmsMenuUserFreq, OPTSTRING },
+    { "POWER",  OME_TAB,     saCmsConfigPowerByGvar,                   &saCmsEntPower,     0 },
+    { "SET",    OME_Submenu, cmsMenuChange,                            &saCmsMenuCommence, 0 },
+    { "CONFIG", OME_Submenu, cmsMenuChange,                            &saCmsMenuConfig,   0 },
+
+    { "BACK", OME_Back, NULL, NULL, 0 },
+    { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static OSD_Entry saCmsMenuChanModeEntries[] =
+{
+    { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
+
+    { "",       OME_Label,   NULL,                   saCmsStatusString,  DYNAMIC },
+    { "BAND",   OME_TAB,     saCmsConfigBandByGvar,  &saCmsEntBand,      0 },
+    { "CHAN",   OME_TAB,     saCmsConfigChanByGvar,  &saCmsEntChan,      0 },
+    { "(FREQ)", OME_UINT16,  NULL,                   &saCmsEntFreqRef,   DYNAMIC },
+    { "POWER",  OME_TAB,     saCmsConfigPowerByGvar, &saCmsEntPower,     0 },
+    { "SET",    OME_Submenu, cmsMenuChange,          &saCmsMenuCommence, 0 },
+    { "CONFIG", OME_Submenu, cmsMenuChange,          &saCmsMenuConfig,   0 },
+
+    { "BACK",   OME_Back, NULL, NULL, 0 },
+    { NULL,     OME_END, NULL, NULL, 0 }
+};
+
+static OSD_Entry saCmsMenuOfflineEntries[] =
+{
+    { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 },
+
+    { "",      OME_Label,   NULL,          saCmsStatusString, DYNAMIC },
+    { "STATX", OME_Submenu, cmsMenuChange, &saCmsMenuStats,   0 },
+
+    { "BACK",  OME_Back, NULL, NULL, 0 },
+    { NULL,    OME_END, NULL, NULL, 0 }
+};
+
+CMS_Menu cmsx_menuVtxSmartAudio; // Forward
+
+static long sacms_SetupTopMenu(void)
+{
+    if (saCmsDeviceStatus) {
+        if (saCmsFselMode == 0)
+            cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
+        else
+            cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries;
+    } else {
+        cmsx_menuVtxSmartAudio.entries = saCmsMenuOfflineEntries;
+    }
+
+    return 0;
+}
+
+CMS_Menu cmsx_menuVtxSmartAudio = {
+    .GUARD_text = "XVTXSA",
+    .GUARD_type = OME_MENU,
+    .onEnter = sacms_SetupTopMenu,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = saCmsMenuOfflineEntries,
+};
+
+#endif // CMS
+
+#endif // VTX_SMARTAUDIO
diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h
new file mode 100644
index 000000000..d11ba621a
--- /dev/null
+++ b/src/main/io/vtx_smartaudio.h
@@ -0,0 +1,35 @@
+
+// For generic API use, but here for now
+
+bool smartAudioInit();
+void smartAudioProcess(uint32_t);
+
+#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
diff --git a/src/main/io/vtx_smartaudio_cms.h b/src/main/io/vtx_smartaudio_cms.h
new file mode 100644
index 000000000..93a72ba1e
--- /dev/null
+++ b/src/main/io/vtx_smartaudio_cms.h
@@ -0,0 +1 @@
+extern CMS_Menu cmsx_menuVtxSmartAudio;
diff --git a/src/main/main.c b/src/main/main.c
index 78bed43c6..ada641775 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -30,6 +30,7 @@
 #include "common/printf.h"
 
 #include "cms/cms.h"
+#include "cms/cms_types.h"
 
 #include "drivers/nvic.h"
 #include "drivers/sensor.h"
@@ -90,6 +91,7 @@
 #include "io/osd.h"
 #include "io/displayport_msp.h"
 #include "io/vtx.h"
+#include "io/vtx_smartaudio.h"
 
 #include "scheduler/scheduler.h"
 
@@ -517,6 +519,10 @@ void init(void)
     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 #endif
 
+#ifdef VTX_SMARTAUDIO
+    smartAudioInit();
+#endif
+
     // start all timers
     // TODO - not implemented yet
     timerStart();
diff --git a/src/main/platform.h b/src/main/platform.h
index 0f6439cbc..9e2c68e0d 100644
--- a/src/main/platform.h
+++ b/src/main/platform.h
@@ -73,4 +73,4 @@
 
 #include "target/common.h"
 #include "target.h"
-
+#include "target/common_post.h"
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 133943a3f..06014f01b 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -100,6 +100,9 @@ typedef enum {
 #ifdef CMS
     TASK_CMS,
 #endif
+#ifdef VTX_CONTROL
+    TASK_VTXCTRL,
+#endif
 
     /* Count of real tasks */
     TASK_COUNT,
diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index 1071cdd00..e0407debb 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -162,7 +162,7 @@
 #define BUTTON_A_PIN            PB1
 #define BUTTON_B_PIN            PB0
 
-#define AVOID_UART3_FOR_PWM_PPM
+//#define AVOID_UART3_FOR_PWM_PPM // Disable this for using UART3
 
 #define SPEKTRUM_BIND
 // USART3,
diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk
index f39c26840..db1c65b89 100644
--- a/src/main/target/OMNIBUSF4/target.mk
+++ b/src/main/target/OMNIBUSF4/target.mk
@@ -8,4 +8,3 @@ TARGET_SRC = \
             drivers/barometer_spi_bmp280.c \
             drivers/compass_hmc5883l.c \
             drivers/max7456.c
-
diff --git a/src/main/target/common.h b/src/main/target/common.h
index ed8d70942..66d17911a 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -101,6 +101,8 @@
 #define TELEMETRY_MAVLINK
 #define USE_RX_MSP
 #define USE_SERIALRX_JETIEXBUS
+#define VTX_CONTROL
+#define VTX_SMARTAUDIO
 #else
 #define SKIP_CLI_COMMAND_HELP
 #endif
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
new file mode 100644
index 000000000..193124c07
--- /dev/null
+++ b/src/main/target/common_post.h
@@ -0,0 +1,26 @@
+/*
+ * 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
+# undef VTX_SMARTAUDIO
+#endif