From 400faee58842a1fd3304ddaab02768ebcc38ed73 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Mon, 1 Aug 2016 20:25:07 +0200
Subject: [PATCH] Refactoring

---
 radio/src/functions.cpp                       |  13 +-
 radio/src/gui/128x64/gui.h                    |  27 +-
 radio/src/gui/128x64/lcd.h                    |   2 +
 radio/src/gui/128x64/menus.h                  |   4 +-
 radio/src/gui/128x64/popups.cpp               |  10 +-
 radio/src/gui/128x64/view_main.cpp            |   2 +-
 radio/src/gui/212x64/bmp.cpp                  |  68 +--
 radio/src/gui/212x64/gui.h                    |   8 +-
 radio/src/gui/212x64/lcd.h                    |  10 +
 radio/src/gui/212x64/menus.h                  |   6 +-
 radio/src/gui/212x64/popups.cpp               |  42 +-
 radio/src/gui/480x272/gui.h                   |   4 +
 radio/src/gui/480x272/menus.h                 |   2 +-
 radio/src/gui/480x272/popups.cpp              |   2 +-
 radio/src/gui/gui_helpers.h                   |   8 +-
 radio/src/lua/interface.cpp                   |   2 +-
 radio/src/main_arm.cpp                        |   4 +-
 radio/src/main_avr.cpp                        |   2 +-
 radio/src/opentx.cpp                          |  21 -
 radio/src/targets/taranis/CMakeLists.txt      |  25 +-
 radio/src/targets/taranis/adc_driver.cpp      |  33 +-
 radio/src/targets/taranis/aspi.c              | 161 -------
 radio/src/targets/taranis/aspi.h              |  53 ---
 radio/src/targets/taranis/audio_driver.cpp    |  10 +-
 radio/src/targets/taranis/board.h             |  33 --
 .../targets/taranis/bootloader/CMakeLists.txt |   5 +-
 radio/src/targets/taranis/configure_pins.cpp  |  61 ---
 radio/src/targets/taranis/hal.h               |  67 ++-
 radio/src/targets/taranis/lcd_driver.cpp      | 447 ------------------
 radio/src/targets/taranis/lcd_driver_spi.cpp  | 291 ++++++++++++
 radio/src/targets/taranis/trainer_driver.cpp  |  18 +-
 31 files changed, 491 insertions(+), 950 deletions(-)
 delete mode 100644 radio/src/targets/taranis/aspi.c
 delete mode 100644 radio/src/targets/taranis/aspi.h
 delete mode 100644 radio/src/targets/taranis/configure_pins.cpp
 delete mode 100644 radio/src/targets/taranis/lcd_driver.cpp
 create mode 100644 radio/src/targets/taranis/lcd_driver_spi.cpp

diff --git a/radio/src/functions.cpp b/radio/src/functions.cpp
index a514262b0..e6ce00f46 100644
--- a/radio/src/functions.cpp
+++ b/radio/src/functions.cpp
@@ -323,18 +323,7 @@ void evalFunctions()
           case FUNC_INSTANT_TRIM:
             newActiveFunctions |= (1 << FUNCTION_INSTANT_TRIM);
             if (!isFunctionActive(FUNCTION_INSTANT_TRIM)) {
-#if defined(GUI)
-              if (menuHandlers[0] == menuMainView
-#if defined(FRSKY) && !defined(PCBFLAMENCO) && !defined(PCBHORUS)
-                || menuHandlers[0] == menuTelemetryFrsky
-#endif
-#if defined(PCBTARANIS)
-                || menuHandlers[0] == menuMainViewChannelsMonitor
-                || menuHandlers[0] == menuChannelsView
-#endif
-              )
-#endif
-              {
+              if (IS_INSTANT_TRIM_ALLOWED()) {
                 instantTrim();
               }
             }
diff --git a/radio/src/gui/128x64/gui.h b/radio/src/gui/128x64/gui.h
index 657dc48a4..ba93147e5 100644
--- a/radio/src/gui/128x64/gui.h
+++ b/radio/src/gui/128x64/gui.h
@@ -39,21 +39,38 @@ extern const pm_uchar sticks[] PROGMEM;
 #define X0                             (LCD_W-WCHART-2)
 #define Y0                             (LCD_H/2)
 
-#if LCD_W >= 212
-  #define MIXES_2ND_COLUMN             (18*FW)
-#else
-  #define MIXES_2ND_COLUMN             (12*FW)
-#endif
+#define MIXES_2ND_COLUMN               (12*FW)
 
 void drawSplash();
 void drawScreenIndex(uint8_t index, uint8_t count, uint8_t attr);
 void drawStick(coord_t centrex, int16_t xval, int16_t yval);
+void drawProgressBar(const char * label)
+{
+  // TODO
+}
+void updateProgressBar(int num, int den)
+{
+  // TODO
+}
+void drawSleepBitmap()
+{
+  // TODO
+}
 
 #if !defined(CPUM64)
   void drawVerticalScrollbar(coord_t x, coord_t y, coord_t h, uint16_t offset, uint16_t count, uint8_t visible);
 #endif
 
+
 #define SET_SCROLLBAR_X(x)
 #define LOAD_MODEL_BITMAP()
 
+#define IS_MAIN_VIEW_DISPLAYED()       menuHandlers[0] == menuMainView
+#if defined(FRSKY)
+#define IS_TELEMETRY_VIEW_DISPLAYED()  menuHandlers[0] == menuTelemetryFrsky
+#else
+#define IS_TELEMETRY_VIEW_DISPLAYED()  false
+#endif
+#define IS_OTHER_VIEW_DISPLAYED()      false
+
 #endif // _GUI_H_
\ No newline at end of file
diff --git a/radio/src/gui/128x64/lcd.h b/radio/src/gui/128x64/lcd.h
index 0490bdacc..c6a1a7a0b 100644
--- a/radio/src/gui/128x64/lcd.h
+++ b/radio/src/gui/128x64/lcd.h
@@ -268,4 +268,6 @@ uint8_t	lcdRefresh_ST7920(uint8_t full);
   #define BLINK_ON_PHASE (g_blinkTmr10ms & (1<<6))
 #endif
 
+const char * writeScreenshot();
+
 #endif // _LCD_H_
diff --git a/radio/src/gui/128x64/menus.h b/radio/src/gui/128x64/menus.h
index 484f3bdff..aca277530 100644
--- a/radio/src/gui/128x64/menus.h
+++ b/radio/src/gui/128x64/menus.h
@@ -445,7 +445,7 @@ extern uint8_t s_curveChan;
 #define WARNING_LINE_X                 16
 #define WARNING_LINE_Y                 3*FH
 
-void displayBox();
+void drawMessageBox();
 void displayPopup(const pm_char * pstr);
 void displayWarning(uint8_t event);
 
@@ -496,7 +496,7 @@ void displayWarning(uint8_t event);
   extern uint16_t popupMenuNoItems;
   extern uint8_t popupMenuFlags;
   extern uint16_t popupMenuOffset;
-  const char * displayPopupMenu(uint8_t event);
+  const char * runPopupMenu(uint8_t event);
   extern void (*popupMenuHandler)(const char *result);
 #else
   #define popupMenuNoItems             0
diff --git a/radio/src/gui/128x64/popups.cpp b/radio/src/gui/128x64/popups.cpp
index 0c0b96cab..db330c344 100644
--- a/radio/src/gui/128x64/popups.cpp
+++ b/radio/src/gui/128x64/popups.cpp
@@ -18,7 +18,7 @@
  * GNU General Public License for more details.
  */
 
-#include "../../opentx.h"
+#include "opentx.h"
 
 const pm_char * warningText = NULL;
 const pm_char * warningInfoText;
@@ -33,7 +33,7 @@ int16_t         warningInputValueMin;
 int16_t         warningInputValueMax;
 #endif
 
-void displayBox()
+void drawMessageBox()
 {
   lcdDrawFilledRect(10, 16, LCD_W-20, 40, SOLID, ERASE);
   lcdDrawRect(10, 16, LCD_W-20, 40);
@@ -48,7 +48,7 @@ void displayBox()
 void displayPopup(const pm_char * pstr)
 {
   warningText = pstr;
-  displayBox();
+  drawMessageBox();
   warningText = NULL;
   lcdRefresh();
 }
@@ -89,7 +89,7 @@ void message(const pm_char *title, const pm_char *t, const char *last MESSAGE_SO
 void displayWarning(uint8_t event)
 {
   warningResult = false;
-  displayBox();
+  drawMessageBox();
   if (warningInfoText) {
     lcdDrawSizedText(WARNING_LINE_X, WARNING_LINE_Y+FH, warningInfoText, warningInfoLength, WARNING_INFO_FLAGS);
   }
@@ -133,7 +133,7 @@ uint16_t popupMenuNoItems = 0;
 uint8_t popupMenuFlags = 0;
 uint16_t popupMenuOffset = 0;
 void (*popupMenuHandler)(const char *result);
-const char * displayPopupMenu(uint8_t event)
+const char * runPopupMenu(uint8_t event)
 {
   const char * result = NULL;
 
diff --git a/radio/src/gui/128x64/view_main.cpp b/radio/src/gui/128x64/view_main.cpp
index 187ddd1fc..ef732b570 100644
--- a/radio/src/gui/128x64/view_main.cpp
+++ b/radio/src/gui/128x64/view_main.cpp
@@ -554,7 +554,7 @@ void menuMainView(uint8_t event)
   if (gvarDisplayTimer > 0) {
     gvarDisplayTimer--;
     warningText = STR_GLOBAL_VAR;
-    displayBox();
+    drawMessageBox();
     lcdDrawSizedText(16, 5*FH, g_model.gvars[gvarLastChanged].name, LEN_GVAR_NAME, ZCHAR);
     lcdDrawText(16+7*FW, 5*FH, PSTR("[\010]"), BOLD);
 
diff --git a/radio/src/gui/212x64/bmp.cpp b/radio/src/gui/212x64/bmp.cpp
index b664def4b..180567b59 100644
--- a/radio/src/gui/212x64/bmp.cpp
+++ b/radio/src/gui/212x64/bmp.cpp
@@ -188,65 +188,21 @@ uint8_t * lcdLoadBitmap(uint8_t * bmp, const char * filename, uint16_t width, ui
   return bmp;
 }
 
-const uint8_t bmpHeader[] = {
-  0x42, 0x4d, 0xF8, 0x1A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x76, 0x00, 0x00, 0x00, 0x28, 0x00,
-  0x00, 0x00, 212,  0x00, 0x00, 0x00, 64,   0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0xbc, 0x38, 0x00, 0x00, 0xbc, 0x38, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 0xee, 0xee, 0xee, 0x00, 0xdd, 0xdd,
-  0xdd, 0x00, 0xcc, 0xcc, 0xcc, 0x00, 0xbb, 0xbb, 0xbb, 0x00, 0xaa, 0xaa, 0xaa, 0x00, 0x99, 0x99,
-  0x99, 0x00, 0x88, 0x88, 0x88, 0x00, 0x77, 0x77, 0x77, 0x00, 0x66, 0x66, 0x66, 0x00, 0x55, 0x55,
-  0x55, 0x00, 0x44, 0x44, 0x44, 0x00, 0x33, 0x33, 0x33, 0x00, 0x22, 0x22, 0x22, 0x00, 0x11, 0x11,
-  0x11, 0x00, 0x00, 0x00, 0x00, 0x00
-};
+uint8_t modelBitmap[MODEL_BITMAP_SIZE] __DMA;
 
-inline display_t getPixel(unsigned int x, unsigned int y)
+bool loadModelBitmap(char * name, uint8_t * bitmap)
 {
-  if (x>=LCD_W || y>=LCD_H)
-    return 0;
-  display_t * p = &displayBuf[y / 2 * LCD_W + x];
-  return (y & 1) ? (*p >> 4) : (*p & 0x0F);
-}
-
-const char * writeScreenshot()
-{
-  FIL bmpFile;
-  UINT written;
-  char filename[42]; // /SCREENSHOTS/screen-2013-01-01-123540.bmp
-
-  // check and create folder here
-  strcpy_P(filename, SCREENSHOTS_PATH);
-  const char * error = sdCheckAndCreateDirectory(filename);
-  if (error) {
-    return error;
-  }
-
-  char * tmp = strAppend(&filename[sizeof(SCREENSHOTS_PATH)-1], "/screen");
-  tmp = strAppendDate(tmp, true);
-  strcpy(tmp, BMP_EXT);
-
-  FRESULT result = f_open(&bmpFile, filename, FA_CREATE_ALWAYS | FA_WRITE);
-  if (result != FR_OK) {
-    return SDCARD_ERROR(result);
-  }
-
-  result = f_write(&bmpFile, bmpHeader, sizeof(bmpHeader), &written);
-  if (result != FR_OK || written != sizeof(bmpHeader)) {
-    f_close(&bmpFile);
-    return SDCARD_ERROR(result);
-  }
-
-  for (int y=LCD_H-1; y>=0; y-=1) {
-    for (int x=0; x<8*((LCD_W+7)/8); x+=2) {
-      uint8_t byte = getPixel(x+1, y) + (getPixel(x, y) << 4);
-      f_write(&bmpFile, &byte, 1, &written);
-      if (result != FR_OK || written != 1) {
-        f_close(&bmpFile);
-        return SDCARD_ERROR(result);
-      }
+  uint8_t len = zlen(name, LEN_BITMAP_NAME);
+  if (len > 0) {
+    char lfn[] = BITMAPS_PATH "/xxxxxxxxxx.bmp";
+    strncpy(lfn+sizeof(BITMAPS_PATH), name, len);
+    strcpy(lfn+sizeof(BITMAPS_PATH)+len, BMP_EXT);
+    if (lcdLoadBitmap(bitmap, lfn, MODEL_BITMAP_WIDTH, MODEL_BITMAP_HEIGHT)) {
+      return true;
     }
   }
 
-  f_close(&bmpFile);
-
-  return NULL;
+  // In all error cases, we set the default logo
+  memcpy(bitmap, logo_taranis, MODEL_BITMAP_SIZE);
+  return false;
 }
diff --git a/radio/src/gui/212x64/gui.h b/radio/src/gui/212x64/gui.h
index 5f6ed2749..a7209446c 100644
--- a/radio/src/gui/212x64/gui.h
+++ b/radio/src/gui/212x64/gui.h
@@ -61,8 +61,8 @@ struct MenuItem {
 void drawSplash();
 void drawScreenIndex(uint8_t index, uint8_t count, uint8_t attr);
 void drawVerticalScrollbar(coord_t x, coord_t y, coord_t h, uint16_t offset, uint16_t count, uint8_t visible);
-void displayMenuBar(const MenuItem *menu, int index);
-void drawProgressBar(const char *label);
+void displayMenuBar(const MenuItem * menu, int index);
+void drawProgressBar(const char * label);
 void updateProgressBar(int num, int den);
 void drawGauge(coord_t x, coord_t y, coord_t w, coord_t h, int32_t val, int32_t max);
 void drawColumnHeader(const char * const * headers, uint8_t index);
@@ -80,4 +80,8 @@ FlightModesType editFlightModes(coord_t x, coord_t y, uint8_t event, FlightModes
   #define displayFlightModes(...)
 #endif
 
+#define IS_MAIN_VIEW_DISPLAYED()       menuHandlers[0] == menuMainView
+#define IS_TELEMETRY_VIEW_DISPLAYED()  menuHandlers[0] == menuTelemetryFrsky
+#define IS_OTHER_VIEW_DISPLAYED()      (menuHandlers[0] == menuMainViewChannelsMonitor || menuHandlers[0] == menuChannelsView)
+
 #endif // _GUI_H_
\ No newline at end of file
diff --git a/radio/src/gui/212x64/lcd.h b/radio/src/gui/212x64/lcd.h
index 83639474a..8b8a79fc5 100644
--- a/radio/src/gui/212x64/lcd.h
+++ b/radio/src/gui/212x64/lcd.h
@@ -218,4 +218,14 @@ const char * writeScreenshot();
   #define BLINK_ON_PHASE (g_blinkTmr10ms & (1<<6))
 #endif
 
+inline display_t getPixel(unsigned int x, unsigned int y)
+{
+  if (x>=LCD_W || y>=LCD_H) {
+    return 0;
+  }
+
+  display_t * p = &displayBuf[y / 2 * LCD_W + x];
+  return (y & 1) ? (*p >> 4) : (*p & 0x0F);
+}
+
 #endif // _LCD_H_
diff --git a/radio/src/gui/212x64/menus.h b/radio/src/gui/212x64/menus.h
index 006b11112..bc4e1db2f 100644
--- a/radio/src/gui/212x64/menus.h
+++ b/radio/src/gui/212x64/menus.h
@@ -362,8 +362,8 @@ void insertMix(uint8_t idx);
 #define WARNING_LINE_X                 16
 #define WARNING_LINE_Y                 3*FH
 
-void displayBox(const char *title);
-void displayPopup(const char *title);
+void drawMessageBox(const char * title);
+void displayPopup(const char * title);
 void displayWarning(uint8_t event);
 
 extern void (*popupFunc)(uint8_t event);
@@ -395,7 +395,7 @@ enum {
   MENU_OFFSET_EXTERNAL
 };
 extern uint8_t popupMenuOffsetType;
-const char * displayPopupMenu(uint8_t event);
+const char * runPopupMenu(uint8_t event);
 extern void (*popupMenuHandler)(const char *result);
 
 #define STATUS_LINE_LENGTH             32
diff --git a/radio/src/gui/212x64/popups.cpp b/radio/src/gui/212x64/popups.cpp
index 6f73b01ed..e45046752 100644
--- a/radio/src/gui/212x64/popups.cpp
+++ b/radio/src/gui/212x64/popups.cpp
@@ -20,24 +20,24 @@
 
 #include "opentx.h"
 
-const char *warningText = NULL;
-const char *warningInfoText;
-uint8_t     warningInfoLength;
-uint8_t     warningType;
-uint8_t     warningResult = 0;
-uint8_t     warningInfoFlags = ZCHAR;
-int16_t     warningInputValue;
-int16_t     warningInputValueMin;
-int16_t     warningInputValueMax;
-void        (*popupFunc)(uint8_t event) = NULL;
-const char *popupMenuItems[POPUP_MENU_MAX_LINES];
-uint8_t     s_menu_item = 0;
-uint16_t    popupMenuNoItems = 0;
-uint16_t    popupMenuOffset = 0;
-uint8_t     popupMenuOffsetType = MENU_OFFSET_INTERNAL;
-void        (*popupMenuHandler)(const char * result);
+const char * warningText = NULL;
+const char * warningInfoText;
+uint8_t      warningInfoLength;
+uint8_t      warningType;
+uint8_t      warningResult = 0;
+uint8_t      warningInfoFlags = ZCHAR;
+int16_t      warningInputValue;
+int16_t      warningInputValueMin;
+int16_t      warningInputValueMax;
+void         (*popupFunc)(uint8_t event) = NULL;
+const char * popupMenuItems[POPUP_MENU_MAX_LINES];
+uint8_t      s_menu_item = 0;
+uint16_t     popupMenuNoItems = 0;
+uint16_t     popupMenuOffset = 0;
+uint8_t      popupMenuOffsetType = MENU_OFFSET_INTERNAL;
+void         (*popupMenuHandler)(const char * result);
 
-void displayBox(const char * title)
+void drawMessageBox(const char * title)
 {
   lcdDrawFilledRect(10, 16, LCD_W-20, 40, SOLID, ERASE);
   lcdDrawRect(10, 16, LCD_W-20, 40);
@@ -47,7 +47,7 @@ void displayBox(const char * title)
 
 void displayPopup(const char * title)
 {
-  displayBox(title);
+  drawMessageBox(title);
   lcdRefresh();
 }
 
@@ -95,7 +95,7 @@ void message(const pm_char * title, const pm_char * text, const char * action, u
 void displayWarning(uint8_t event)
 {
   warningResult = false;
-  displayBox(warningText);
+  drawMessageBox(warningText);
   if (warningInfoText) {
     lcdDrawSizedText(WARNING_LINE_X, WARNING_LINE_Y+FH, warningInfoText, warningInfoLength, WARNING_INFO_FLAGS);
   }
@@ -119,11 +119,11 @@ void displayWarning(uint8_t event)
   }
 }
 
-const char * displayPopupMenu(uint8_t event)
+const char * runPopupMenu(uint8_t event)
 {
   const char * result = NULL;
 
-  uint8_t display_count = min<unsigned int>(popupMenuNoItems, MENU_MAX_DISPLAY_LINES);
+  uint8_t display_count = min<uint8_t>(popupMenuNoItems, MENU_MAX_DISPLAY_LINES);
   uint8_t y = (display_count >= 5 ? MENU_Y - FH - 1 : MENU_Y);
   lcdDrawFilledRect(MENU_X, y, MENU_W, display_count * (FH+1) + 2, SOLID, ERASE);
   lcdDrawRect(MENU_X, y, MENU_W, display_count * (FH+1) + 2);
diff --git a/radio/src/gui/480x272/gui.h b/radio/src/gui/480x272/gui.h
index 326c5a892..67a00ba9b 100644
--- a/radio/src/gui/480x272/gui.h
+++ b/radio/src/gui/480x272/gui.h
@@ -91,4 +91,8 @@ void drawCurvePoint(int x, int y, LcdFlags color);
 extern Layout * customScreens[MAX_CUSTOM_SCREENS];
 extern Topbar * topbar;
 
+#define IS_MAIN_VIEW_DISPLAYED()       menuHandlers[0] == menuMainView
+#define IS_TELEMETRY_VIEW_DISPLAYED()  false
+#define IS_OTHER_VIEW_DISPLAYED()      false
+
 #endif // _GUI_H_
diff --git a/radio/src/gui/480x272/menus.h b/radio/src/gui/480x272/menus.h
index 67162675c..c3185e97b 100644
--- a/radio/src/gui/480x272/menus.h
+++ b/radio/src/gui/480x272/menus.h
@@ -503,7 +503,7 @@ enum {
   MENU_OFFSET_EXTERNAL
 };
 extern uint8_t popupMenuOffsetType;
-const char * displayPopupMenu(evt_t event);
+const char * runPopupMenu(evt_t event);
 extern void (*popupMenuHandler)(const char * result);
 
 #define TEXT_FILENAME_MAXLEN           40
diff --git a/radio/src/gui/480x272/popups.cpp b/radio/src/gui/480x272/popups.cpp
index cc44d7a7d..ecf29f8cb 100644
--- a/radio/src/gui/480x272/popups.cpp
+++ b/radio/src/gui/480x272/popups.cpp
@@ -99,7 +99,7 @@ void displayWarning(evt_t event)
   }
 }
 
-const char * displayPopupMenu(evt_t event)
+const char * runPopupMenu(evt_t event)
 {
   const char * result = NULL;
 
diff --git a/radio/src/gui/gui_helpers.h b/radio/src/gui/gui_helpers.h
index 6301b4929..6f7984b5b 100644
--- a/radio/src/gui/gui_helpers.h
+++ b/radio/src/gui/gui_helpers.h
@@ -62,6 +62,12 @@ bool modelHasNotes();
 
 #if defined(COLORLCD)
 bool isSwitchWarningStateAvailable(int state);
-#endif // #if defined(COLORLCD)
+#endif
+
+#if defined(GUI)
+#define IS_INSTANT_TRIM_ALLOWED()      (IS_MAIN_VIEW_DISPLAYED() || IS_TELEMETRY_VIEW_DISPLAYED() || IS_OTHER_VIEW_DISPLAYED())
+#else
+#define IS_INSTANT_TRIM_ALLOWED()      true
+#endif
 
 #endif // _GUI_HELPERS_H_
diff --git a/radio/src/lua/interface.cpp b/radio/src/lua/interface.cpp
index 02c58e789..549b3a5cf 100644
--- a/radio/src/lua/interface.cpp
+++ b/radio/src/lua/interface.cpp
@@ -426,7 +426,7 @@ void luaLoadPermanentScripts()
 void displayLuaError(const char * title)
 {
 #if !defined(COLORLCD)
-  displayBox(title);
+  drawMessageBox(title);
 #endif
   if (lua_warning_info[0]) {
     char * split = strstr(lua_warning_info, ": ");
diff --git a/radio/src/main_arm.cpp b/radio/src/main_arm.cpp
index 724cb73c6..048004145 100644
--- a/radio/src/main_arm.cpp
+++ b/radio/src/main_arm.cpp
@@ -231,7 +231,7 @@ void guiMain(evt_t evt)
           popupDisplayed = lcdRestoreBackupBuffer();
           if (warn) DISPLAY_WARNING(evt);
           if (menu) {
-            const char * result = displayPopupMenu(evt);
+            const char * result = runPopupMenu(evt);
             if (result) {
               popupMenuHandler(result);
               if (menuEvent == 0) {
@@ -376,7 +376,7 @@ void guiMain(evt_t evt)
       TRACE("Popup Menu started");
       inPopupMenu = true;
     }
-    const char * result = displayPopupMenu(evt);
+    const char * result = runPopupMenu(evt);
     if (result) {
       TRACE("popupMenuHandler(%s)", result);
       popupMenuHandler(result);
diff --git a/radio/src/main_avr.cpp b/radio/src/main_avr.cpp
index 069b7df20..25940bdd3 100644
--- a/radio/src/main_avr.cpp
+++ b/radio/src/main_avr.cpp
@@ -129,7 +129,7 @@ void perMain()
     if (warn) DISPLAY_WARNING(evt);
 #if defined(NAVIGATION_MENUS)
     if (popupMenuActive) {
-      const char * result = displayPopupMenu(evt);
+      const char * result = runPopupMenu(evt);
       if (result) {
         popupMenuHandler(result);
       }
diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp
index 2d3fda029..5d2161490 100644
--- a/radio/src/opentx.cpp
+++ b/radio/src/opentx.cpp
@@ -27,27 +27,6 @@ ModelData  g_model;
 Clipboard clipboard;
 #endif
 
-#if defined(PCBTARANIS)
-uint8_t modelBitmap[MODEL_BITMAP_SIZE] __DMA;
-
-bool loadModelBitmap(char * name, uint8_t * bitmap)
-{
-  uint8_t len = zlen(name, LEN_BITMAP_NAME);
-  if (len > 0) {
-    char lfn[] = BITMAPS_PATH "/xxxxxxxxxx.bmp";
-    strncpy(lfn+sizeof(BITMAPS_PATH), name, len);
-    strcpy(lfn+sizeof(BITMAPS_PATH)+len, BMP_EXT);
-    if (lcdLoadBitmap(bitmap, lfn, MODEL_BITMAP_WIDTH, MODEL_BITMAP_HEIGHT)) {
-      return true;
-    }
-  }
-
-  // In all error cases, we set the default logo
-  memcpy(bitmap, logo_taranis, MODEL_BITMAP_SIZE);
-  return false;
-}
-#endif
-
 #if !defined(CPUARM)
 uint8_t g_tmr1Latency_max;
 uint8_t g_tmr1Latency_min;
diff --git a/radio/src/targets/taranis/CMakeLists.txt b/radio/src/targets/taranis/CMakeLists.txt
index 5f57f9984..07ed5ab90 100644
--- a/radio/src/targets/taranis/CMakeLists.txt
+++ b/radio/src/targets/taranis/CMakeLists.txt
@@ -24,7 +24,7 @@ if(PCB STREQUAL X9E)
   set(GUI_DIR 212x64)
   set(GUI_SRC ${GUI_SRC} bmp.cpp)
   set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} taranis_bitmaps)
-  set(VIRTUAL_INPUTS YES)
+  set(LCD_DRIVER lcd_driver_spi.cpp)
 elseif(PCB STREQUAL X9D+)
   set(CPU_TYPE STM32F2)
   set(LINKER_SCRIPT targets/taranis/stm32f2_flash.ld)
@@ -36,7 +36,7 @@ elseif(PCB STREQUAL X9D+)
   set(GUI_DIR 212x64)
   set(GUI_SRC ${GUI_SRC} bmp.cpp)
   set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} taranis_bitmaps)
-  set(VIRTUAL_INPUTS YES)
+  set(LCD_DRIVER lcd_driver_spi.cpp)
 elseif(PCB STREQUAL X9D)
   set(CPU_TYPE STM32F2)
   set(LINKER_SCRIPT targets/taranis/stm32f2_flash.ld)
@@ -47,7 +47,7 @@ elseif(PCB STREQUAL X9D)
   set(GUI_DIR 212x64)
   set(GUI_SRC ${GUI_SRC} bmp.cpp)
   set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} taranis_bitmaps)
-  set(VIRTUAL_INPUTS YES)
+  set(LCD_DRIVER lcd_driver_aspi.cpp)
 elseif(PCB STREQUAL X7D)
   set(CPU_TYPE STM32F2)
   set(LINKER_SCRIPT targets/taranis/stm32f2_flash.ld)
@@ -58,16 +58,19 @@ elseif(PCB STREQUAL X7D)
   add_definitions(-DEEPROM_VARIANT=0)
   set(GUI_DIR 128x64)
   set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} 9x_bitmaps)
-  set(VIRTUAL_INPUTS NO)
+  set(LCD_DRIVER lcd_driver_spi2.cpp)
 endif()
 
 set(HSE_VALUE 12000000)
 set(SDCARD YES)
 set(EEPROM EEPROM_RLC)
+set(VIRTUAL_INPUTS YES)
 set(TARGET_DIR taranis)
+
 add_definitions(-DPCBTARANIS -DPPM_PIN_TIMER)
 add_definitions(-DAUDIO -DVOICE -DRTCLOCK)
-add_definitions(-DVIRTUALINPUTS -DLUAINPUTS -DXCURVES -DVARIO)
+add_definitions(-DLUAINPUTS -DXCURVES -DVARIO)
+
 set(GUI_SRC ${GUI_SRC}
   model_inputs.cpp
   model_mixes.cpp
@@ -79,33 +82,39 @@ set(GUI_SRC ${GUI_SRC}
   view_telemetry.cpp
   view_text.cpp
   view_about.cpp
+  ../screenshot.cpp
   )
+
 set(TARGET_SRC
   ${TARGET_SRC}
   board.cpp
   extmodule_driver.cpp
   ../common/arm/stm32/rtc_driver.cpp
   )
+
 set(FIRMWARE_SRC
   ${FIRMWARE_SRC}
   loadboot.cpp
   )
+
 set(FIRMWARE_TARGET_SRC
   ${FIRMWARE_TARGET_SRC}
-  lcd_driver.cpp
+  ${LCD_DRIVER}
+  backlight_driver.cpp
   delays.c
   i2c_driver.cpp
   pwr_driver.c
-  configure_pins.cpp
   flash_driver.cpp
-  aspi.c
   )
+
 if(LCD_DUAL_BUFFER)
   add_definitions(-DLCD_DUAL_BUFFER)
 endif()
+
 if(TARANIS_INTERNAL_PPM)
   add_definitions(-DTARANIS_INTERNAL_PPM)
 endif()
+
 if(MIXERS_MONITOR)
   add_definitions(-DMIXERS_MONITOR)
 endif()
diff --git a/radio/src/targets/taranis/adc_driver.cpp b/radio/src/targets/taranis/adc_driver.cpp
index bc6b9de31..1fa501302 100644
--- a/radio/src/targets/taranis/adc_driver.cpp
+++ b/radio/src/targets/taranis/adc_driver.cpp
@@ -20,11 +20,6 @@
 
 #include "opentx.h"
 
-#define PIN_ANALOG  0x0003
-#define PIN_PORTA   0x0000
-#define PIN_PORTB   0x0100
-#define PIN_PORTC   0x0200
-
 // Sample time should exceed 1uS
 #define SAMPTIME       2   // sample time = 28 cycles
 #define SAMPTIME_LONG  3   // sample time = 56 cycles
@@ -57,19 +52,23 @@ uint16_t adcValues[NUMBER_ANALOG] __DMA;
 
 void adcInit()
 {
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = ADC_GPIOA_PINS;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+  GPIO_Init(GPIOA, &GPIO_InitStructure);
+
+  GPIO_InitStructure.GPIO_Pin = ADC_GPIOB_PINS;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  GPIO_InitStructure.GPIO_Pin = ADC_GPIOC_PINS;
+  GPIO_Init(GPIOC, &GPIO_InitStructure);
+
 #if defined(PCBX9E)
-  configure_pins(ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV | ADC_GPIO_PIN_SLIDER3, PIN_ANALOG | PIN_PORTA);
-  configure_pins(ADC_GPIO_PIN_POT2 | ADC_GPIO_PIN_SLIDER4, PIN_ANALOG | PIN_PORTB);
-  configure_pins(ADC_GPIO_PIN_POT3 | ADC_GPIO_PIN_POT4 | ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2 | ADC_GPIO_PIN_BATT, PIN_ANALOG | PIN_PORTC);
-  configure_pins(ADC_GPIO_PIN_POT1 | ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2, PIN_ANALOG | PIN_PORTF);
-#elif defined(PCBX9DP)
-  configure_pins(ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV | ADC_GPIO_PIN_POT1, PIN_ANALOG | PIN_PORTA);
-  configure_pins(ADC_GPIO_PIN_POT2 | ADC_GPIO_PIN_POT3, PIN_ANALOG | PIN_PORTB);
-  configure_pins(ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2 | ADC_GPIO_PIN_BATT, PIN_ANALOG | PIN_PORTC);
-#else
-  configure_pins(ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV | ADC_GPIO_PIN_POT1, PIN_ANALOG | PIN_PORTA);
-  configure_pins(ADC_GPIO_PIN_POT2, PIN_ANALOG|PIN_PORTB);
-  configure_pins(ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2 | ADC_GPIO_PIN_BATT, PIN_ANALOG | PIN_PORTC);
+  GPIO_InitStructure.GPIO_Pin = ADC_GPIOF_PINS;
+  GPIO_Init(GPIOF, &GPIO_InitStructure);
 #endif
 
   ADC1->CR1 = ADC_CR1_SCAN;
diff --git a/radio/src/targets/taranis/aspi.c b/radio/src/targets/taranis/aspi.c
deleted file mode 100644
index d379a3138..000000000
--- a/radio/src/targets/taranis/aspi.c
+++ /dev/null
@@ -1,161 +0,0 @@
-/*
- * Copyright (C) OpenTX
- *
- * Based on code named
- *   th9x - http://code.google.com/p/th9x 
- *   er9x - http://code.google.com/p/er9x
- *   gruvin9x - http://code.google.com/p/gruvin9x
- *
- * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program 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.
- */
-
-#include "board.h"
-
-/**
-**********send command to lcd**************
-*/
-//hardware spi
-/*u16 spiCmd(u8 addr)
-{
-	u8 readValue;
-	
-	SPI_RS_LOW();
-    SPI_NCS_LOW();
-	
-	SPI_WRITE_BYTE(addr);
-	SPI_WAIT_DONE();
-
-	readValue=SPI_READ_BYTE();
-	SPI_NCS_HIGH();
-    return	readValue;
-}*/
-
-#if defined(PCBX9E) || defined(PCBX9DP)
-void AspiCmd(uint8_t Command_Byte)
-{
-  LCD_A0_LOW() ;
-  LCD_NCS_LOW() ;  
-  while ( ( SPI3->SR & SPI_SR_TXE ) == 0 ) {
-    // wait
-  }
-  (void)SPI3->DR ;		// Clear receive
-  SPI3->DR = Command_Byte ;
-  while ( ( SPI3->SR & SPI_SR_RXNE ) == 0 ) {
-    // wait
-  }
-  LCD_NCS_HIGH() ;
-}
-#else
-// Analog SPI
-void AspiCmd(u8 Command_Byte)
-{
-    int i=8; 
-    LCD_A0_LOW();
-
-    LCD_CLK_HIGH();
-    LCD_CLK_HIGH();
-    LCD_NCS_LOW();
- 
-    while (i--) {
-      LCD_CLK_LOW();
-
-      if (Command_Byte&0x80)
-        LCD_MOSI_HIGH();
-      else
-        LCD_MOSI_LOW();
-
-      Command_Byte <<= 1;
-      LCD_CLK_LOW(); \
-      LCD_CLK_LOW(); \
-
-      LCD_CLK_HIGH();
-      LCD_CLK_HIGH();
-    }
-
-    LCD_NCS_HIGH();  
-    LCD_A0_HIGH();
-}
-#endif
-
-/**
-
-********send data to lcd**************
-
-*/
-
-//hardware spi
-/*
-u16 spiData(u8 addr)
-{
-	u16 readValue;
-      	OLED_RS_HIGH();
-	SPI_NCS_LOW();
-	
-	SPI_WRITE_BYTE(addr);
-	SPI_WAIT_DONE();
-
-	readValue=SPI_READ_BYTE();
-	SPI_NCS_HIGH();
-      	return	readValue;
-}
-*/
-
-//Anolog spi
-void AspiData(u8 Para_data)
-{
-    int i=8;
-    LCD_CLK_HIGH();
-    LCD_CLK_HIGH();
-    LCD_A0_HIGH();
-    LCD_NCS_LOW();
-    while (i--) {
-    	LCD_CLK_LOW();
-        if (Para_data&0x80)
-          LCD_MOSI_HIGH();
-        else
-          LCD_MOSI_LOW();
-        Para_data <<= 1;
-        LCD_CLK_LOW();
-        LCD_CLK_LOW();
-
-        LCD_CLK_HIGH();
-        LCD_CLK_HIGH();
-    }
-    LCD_NCS_HIGH();
-    LCD_A0_HIGH();  
-}
-
-/***
-**********read one byte in a register*******
-*/
-
-/*
-u16 spiRegAccess(u8 addrByte, u8 writeValue)
-{
-	u8 readValue;
-
-	SPI_NCS_LOW();
-	
-	SPI_WRITE_BYTE(addrByte);
-	SPI_WAIT_DONE();
-
-	SPI_WRITE_BYTE(writeValue);
-	SPI_WAIT_DONE();
-
-	readValue=SPI_READ_BYTE();
-	SPI_NCS_HIGH();
-	return	readValue;
-}
-
-*/
-
-
diff --git a/radio/src/targets/taranis/aspi.h b/radio/src/targets/taranis/aspi.h
deleted file mode 100644
index 2f9b33e3e..000000000
--- a/radio/src/targets/taranis/aspi.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * Copyright (C) OpenTX
- *
- * Based on code named
- *   th9x - http://code.google.com/p/th9x 
- *   er9x - http://code.google.com/p/er9x
- *   gruvin9x - http://code.google.com/p/gruvin9x
- *
- * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program 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.
- */
-
-#ifndef _ASPI_H_
-#define _ASPI_H_
-
-#if defined(__cplusplus) && !defined(SIMU)
-extern "C" {
-#endif
-
-#define __no_operation     __NOP
-
-#define LCD_NCS_HIGH()    LCD_GPIO_NCS->BSRRL = LCD_GPIO_PIN_NCS
-#define LCD_NCS_LOW()     LCD_GPIO_NCS->BSRRH = LCD_GPIO_PIN_NCS
-
-#define LCD_A0_HIGH()     LCD_GPIO_SPI->BSRRL = LCD_GPIO_PIN_A0
-#define LCD_A0_LOW()      LCD_GPIO_SPI->BSRRH = LCD_GPIO_PIN_A0
-
-#define LCD_RST_HIGH()    LCD_GPIO_RST->BSRRL = LCD_GPIO_PIN_RST
-#define LCD_RST_LOW()     LCD_GPIO_RST->BSRRH = LCD_GPIO_PIN_RST
-
-#define LCD_CLK_HIGH()    LCD_GPIO_SPI->BSRRL = LCD_GPIO_PIN_CLK
-#define LCD_CLK_LOW()     LCD_GPIO_SPI->BSRRH = LCD_GPIO_PIN_CLK
-
-#define LCD_MOSI_HIGH()   LCD_GPIO_SPI->BSRRL = LCD_GPIO_PIN_MOSI
-#define LCD_MOSI_LOW()    LCD_GPIO_SPI->BSRRH = LCD_GPIO_PIN_MOSI
-
-void AspiCmd(u8 Command_Byte);
-void AspiData(u8 Para_data);
-
-#if defined(__cplusplus) && !defined(SIMU)
-}
-#endif
-
-#endif // _ASPI_H_
-
diff --git a/radio/src/targets/taranis/audio_driver.cpp b/radio/src/targets/taranis/audio_driver.cpp
index e81fea863..0269d0f17 100644
--- a/radio/src/targets/taranis/audio_driver.cpp
+++ b/radio/src/targets/taranis/audio_driver.cpp
@@ -18,7 +18,7 @@
  * GNU General Public License for more details.
  */
 
-#include "../../opentx.h"
+#include "opentx.h"
 
 #if !defined(SIMU)
 bool dacIdle = true;
@@ -50,7 +50,13 @@ void dacInit()
 {
   dacTimerInit();
 
-  configure_pins( GPIO_Pin_4, PIN_ANALOG | PIN_PORTA ) ;
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = AUDIO_OUTPUT_GPIO_PIN;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+  GPIO_Init(AUDIO_OUTPUT_GPIO, &GPIO_InitStructure);
 
   // Chan 7, 16-bit wide, Medium priority, memory increments
   AUDIO_DMA_Stream->CR &= ~DMA_SxCR_EN ;              // Disable DMA
diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h
index 36f8d28f8..50e5a6bee 100644
--- a/radio/src/targets/taranis/board.h
+++ b/radio/src/targets/taranis/board.h
@@ -70,7 +70,6 @@ extern "C" {
 #endif
 
 #include "hal.h"
-#include "aspi.h"
 
 #if defined(__cplusplus) && !defined(SIMU)
 }
@@ -91,38 +90,6 @@ extern "C" {
 #define TIMER_MULT_APB1    2
 #define TIMER_MULT_APB2    2
 
-#define PIN_MODE_MASK      0x0003
-#define PIN_INPUT          0x0000
-#define PIN_OUTPUT         0x0001
-#define PIN_PERIPHERAL     0x0002
-#define PIN_ANALOG         0x0003
-
-#define PIN_PULL_MASK      0x000C
-#define PIN_PULLUP         0x0004
-
-#define PIN_PORT_MASK      0x0700
-#define PIN_PORTA          0x0000
-#define PIN_PORTB          0x0100
-#define PIN_PORTC          0x0200
-#define PIN_PORTD          0x0300
-#define PIN_PORTE          0x0400
-#define PIN_PORTF          0x0500
-
-#define PIN_PERI_MASK      0x00F0
-#define PIN_PER_1          0x0010
-#define PIN_PER_2          0x0020
-#define PIN_PER_3          0x0030
-#define PIN_PER_5          0x0050
-#define PIN_PER_6          0x0060
-#define PIN_PER_8          0x0080
-
-#define PIN_SPEED_MASK     0x6000
-#define PIN_OS25           0x2000
-#define PIN_OS50           0x4000
-#define PIN_OS100          0x6000
-
-void configure_pins( uint32_t pins, uint16_t config );
-
 #define strcpy_P strcpy
 #define strcat_P strcat
 
diff --git a/radio/src/targets/taranis/bootloader/CMakeLists.txt b/radio/src/targets/taranis/bootloader/CMakeLists.txt
index 0b95b945d..2f9dced44 100644
--- a/radio/src/targets/taranis/bootloader/CMakeLists.txt
+++ b/radio/src/targets/taranis/bootloader/CMakeLists.txt
@@ -44,8 +44,8 @@ set(BOOTLOADER_SRC
   ../../../${STM32USB_DIR}/STM32_USB_Device_Library/Class/msc/src/usbd_msc_core.c
   ../../../${FATFS_DIR}/ff.c
   ../../../${FATFS_DIR}/option/ccsbcs.c
-  ../lcd_driver.cpp
-  ../configure_pins.cpp
+  ../${LCD_DRIVER}
+  ../backlight_driver.cpp
   ../keys_driver.cpp
   ../i2c_driver.cpp
   ../flash_driver.cpp
@@ -54,7 +54,6 @@ set(BOOTLOADER_SRC
   ../../common/arm/stm32/usbd_storage_msd.cpp
   ../delays.c
   ../../common/arm/stm32/usbd_desc.c
-  ../aspi.c
   ../../common/arm/stm32/usb_bsp.c
   ../../common/arm/stm32/usb_driver.c
   ../pwr_driver.c
diff --git a/radio/src/targets/taranis/configure_pins.cpp b/radio/src/targets/taranis/configure_pins.cpp
deleted file mode 100644
index 3b84106f6..000000000
--- a/radio/src/targets/taranis/configure_pins.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * Copyright (C) OpenTX
- *
- * Based on code named
- *   th9x - http://code.google.com/p/th9x 
- *   er9x - http://code.google.com/p/er9x
- *   gruvin9x - http://code.google.com/p/gruvin9x
- *
- * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program 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.
- */
-
-#include "../../opentx.h"
-
-void configure_pins( uint32_t pins, uint16_t config )
-{
-  uint32_t address ;
-  GPIO_TypeDef *pgpio ;
-  uint32_t thispin ;
-  uint32_t pos ;
-
-  address = ( config & PIN_PORT_MASK ) >> 8 ;
-  address *= (GPIOB_BASE-GPIOA_BASE) ;
-  address += GPIOA_BASE ;
-  pgpio = (GPIO_TypeDef* ) address ;
-
-  /* -------------------------Configure the port pins---------------- */
-  /*-- GPIO Mode Configuration --*/
-  for (thispin = 0x0001, pos = 0; thispin < 0x10000; thispin <<= 1, pos +=1 )
-  {
-    if ( pins & thispin)
-    {
-      pgpio->MODER  &= ~(GPIO_MODER_MODER0 << (pos * 2)) ;
-      pgpio->MODER |= (config & PIN_MODE_MASK) << (pos * 2) ;
-
-      if ( ( (config & PIN_MODE_MASK ) == PIN_OUTPUT) || ( (config & PIN_MODE_MASK) == PIN_PERIPHERAL) )
-      {
-        /* Speed mode configuration */
-        pgpio->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pos * 2)) ;
-        pgpio->OSPEEDR |= ((config & PIN_SPEED_MASK) >> 13 ) << (pos * 2) ;
-
-        /* Output mode configuration*/
-        pgpio->OTYPER  &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pos)) ;
-      }
-      /* Pull-up Pull down resistor configuration*/
-      pgpio->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pos * 2));
-      pgpio->PUPDR |= ((config & PIN_PULL_MASK) >> 2) << (pos * 2) ;
-
-      pgpio->AFR[pos >> 3] &= ~(0x000F << ((pos & 7)*4)) ;
-      pgpio->AFR[pos >> 3] |= ((config & PIN_PERI_MASK) >> 4) << ((pos & 7)*4) ;
-    }
-  }
-}
diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h
index 3c15e3e31..6ab5cfac9 100644
--- a/radio/src/targets/taranis/hal.h
+++ b/radio/src/targets/taranis/hal.h
@@ -283,6 +283,10 @@
   #define ADC_GPIO_PIN_SLIDER3          GPIO_Pin_6  // PA.06
   #define ADC_GPIO_PIN_SLIDER4          GPIO_Pin_1  // PB.01
   #define ADC_GPIO_PIN_BATT             GPIO_Pin_0  // PC.00
+  #define ADC_GPIOA_PINS (ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV | ADC_GPIO_PIN_SLIDER3)
+  #define ADC_GPIOB_PINS (ADC_GPIO_PIN_POT2 | ADC_GPIO_PIN_SLIDER4)
+  #define ADC_GPIOC_PINS (ADC_GPIO_PIN_POT3 | ADC_GPIO_PIN_POT4 | ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2 | ADC_GPIO_PIN_BATT)
+  #define ADC_GPIOF_PINS (ADC_GPIO_PIN_POT1 | ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2)
   #define ADC_CHANNEL_POT1              ADC_Channel_6  // ADC3_IN6
   #define ADC_CHANNEL_POT2              ADC_Channel_8  // ADC1_IN8
   #define ADC_CHANNEL_POT3              ADC_Channel_15 // ADC1_IN15
@@ -303,6 +307,9 @@
   #define ADC_GPIO_PIN_SLIDER1          GPIO_Pin_4  // PC.04
   #define ADC_GPIO_PIN_SLIDER2          GPIO_Pin_5  // PC.05
   #define ADC_GPIO_PIN_BATT             GPIO_Pin_0  // PC.00
+  #define ADC_GPIOA_PINS                (ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV | ADC_GPIO_PIN_POT1)
+  #define ADC_GPIOB_PINS                (ADC_GPIO_PIN_POT2 | ADC_GPIO_PIN_POT3)
+  #define ADC_GPIOC_PINS                (ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2 | ADC_GPIO_PIN_BATT)
   #define ADC_CHANNEL_POT1              ADC_Channel_6
   #define ADC_CHANNEL_POT2              ADC_Channel_8
   #define ADC_CHANNEL_POT3              ADC_Channel_9
@@ -315,6 +322,9 @@
   #define ADC_GPIO_PIN_SLIDER1          GPIO_Pin_4  // PC.04
   #define ADC_GPIO_PIN_SLIDER2          GPIO_Pin_5  // PC.05
   #define ADC_GPIO_PIN_BATT             GPIO_Pin_0  // PC.00
+  #define ADC_GPIOA_PINS                (ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV | ADC_GPIO_PIN_POT1)
+  #define ADC_GPIOB_PINS                (ADC_GPIO_PIN_POT2)
+  #define ADC_GPIOC_PINS                (ADC_GPIO_PIN_SLIDER1 | ADC_GPIO_PIN_SLIDER2 | ADC_GPIO_PIN_BATT)
   #define ADC_CHANNEL_POT1              ADC_Channel_6
   #define ADC_CHANNEL_POT2              ADC_Channel_8
   #define ADC_CHANNEL_POT3              ADC_Channel_9
@@ -433,9 +443,11 @@
 // Heartbeat
 #define HEARTBEAT_RCC_AHB1Periph        RCC_AHB1Periph_GPIOC
 #define HEARTBEAT_RCC_APB2Periph        RCC_APB2Periph_USART6
+#define HEARTBEAT_GPIO                  GPIOC
 #define HEARTBEAT_GPIO_PIN              GPIO_Pin_7  // PC.07
 #define HEARTBEAT_GPIO_PinSource        GPIO_PinSource7
-#define HEARTBEAT_GPIO_AF               GPIO_AF_USART6
+#define HEARTBEAT_GPIO_AF_SBUS          GPIO_AF_USART6
+#define HEARTBEAT_GPIO_AF_CAPTURE       GPIO_AF_TIM3
 #define HEARTBEAT_USART                 USART6
 #define HEARTBEAT_USART_IRQHandler      USART6_IRQHandler
 #define HEARTBEAT_USART_IRQn            USART6_IRQn
@@ -492,14 +504,14 @@
 #if defined(PCBX9E)
   #define LCD_RCC_AHB1Periph            (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_DMA1)
   #define LCD_RCC_APB1Periph            RCC_APB1Periph_SPI3
-  #define LCD_GPIO_SPI                  GPIOC
-  #define LCD_GPIO_NCS                  GPIOA
-  #define LCD_GPIO_RST                  GPIOD
-  #define LCD_GPIO_PIN_MOSI             GPIO_Pin_12 // PC.12
-  #define LCD_GPIO_PIN_CLK              GPIO_Pin_10 // PC.10
-  #define LCD_GPIO_PIN_NCS              GPIO_Pin_15 // PA.15
-  #define LCD_GPIO_PIN_A0               GPIO_Pin_11 // PC.11
-  #define LCD_GPIO_PIN_RST              GPIO_Pin_15 // PD.15
+  #define LCD_SPI_GPIO                  GPIOC
+  #define LCD_MOSI_GPIO_PIN             GPIO_Pin_12 // PC.12
+  #define LCD_CLK_GPIO_PIN              GPIO_Pin_10 // PC.10
+  #define LCD_A0_GPIO_PIN               GPIO_Pin_11 // PC.11
+  #define LCD_NCS_GPIO                  GPIOA
+  #define LCD_NCS_GPIO_PIN              GPIO_Pin_15 // PA.15
+  #define LCD_RST_GPIO                  GPIOD
+  #define LCD_RST_GPIO_PIN              GPIO_Pin_15 // PD.15
   #define LCD_DMA                       DMA1
   #define LDC_DMA_Stream                DMA1_Stream7
   #define LCD_DMA_Stream_IRQn           DMA1_Stream7_IRQn
@@ -510,14 +522,16 @@
 #elif defined(PCBX9DP)
   #define LCD_RCC_AHB1Periph            (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_DMA1)
   #define LCD_RCC_APB1Periph            RCC_APB1Periph_SPI3
-  #define LCD_GPIO_SPI                  GPIOC
-  #define LCD_GPIO_NCS                  GPIOA
-  #define LCD_GPIO_RST                  GPIOD
-  #define LCD_GPIO_PIN_MOSI             GPIO_Pin_12 // PC.12
-  #define LCD_GPIO_PIN_CLK              GPIO_Pin_10 // PC.10
-  #define LCD_GPIO_PIN_NCS              GPIO_Pin_15 // PA.15
-  #define LCD_GPIO_PIN_A0               GPIO_Pin_11 // PC.11
-  #define LCD_GPIO_PIN_RST              GPIO_Pin_12 // PD.12
+  #define LCD_SPI_GPIO                  GPIOC
+  #define LCD_MOSI_GPIO_PIN             GPIO_Pin_12 // PC.12
+  #define LCD_MOSI_GPIO_PinSource       GPIO_PinSource12
+  #define LCD_CLK_GPIO_PIN              GPIO_Pin_10 // PC.10
+  #define LCD_CLK_GPIO_PinSource        GPIO_PinSource10
+  #define LCD_A0_GPIO_PIN               GPIO_Pin_11 // PC.11
+  #define LCD_NCS_GPIO                  GPIOA
+  #define LCD_NCS_GPIO_PIN              GPIO_Pin_15 // PA.15
+  #define LCD_RST_GPIO                  GPIOD
+  #define LCD_RST_GPIO_PIN              GPIO_Pin_12 // PD.12
   #define LCD_DMA                       DMA1
   #define LDC_DMA_Stream                DMA1_Stream7
   #define LCD_DMA_Stream_IRQn           DMA1_Stream7_IRQn
@@ -525,17 +539,18 @@
   #define LCD_DMA_FLAGS                 (DMA_HIFCR_CTCIF7 | DMA_HIFCR_CHTIF7 | DMA_HIFCR_CTEIF7 | DMA_HIFCR_CDMEIF7 | DMA_HIFCR_CFEIF7)
   #define LCD_DMA_FLAG_INT              DMA_HIFCR_CTCIF7
   #define LCD_SPI                       SPI3
+  #define LCD_GPIO_AF                   GPIO_AF_SPI3
 #else
   #define LCD_RCC_AHB1Periph            RCC_AHB1Periph_GPIOD
   #define LCD_RCC_APB1Periph            0
-  #define LCD_GPIO_SPI                  GPIOD
-  #define LCD_GPIO_NCS                  GPIOD
-  #define LCD_GPIO_RST                  GPIOD
-  #define LCD_GPIO_PIN_MOSI             GPIO_Pin_10 // PD.10
-  #define LCD_GPIO_PIN_CLK              GPIO_Pin_11 // PD.11
-  #define LCD_GPIO_PIN_NCS              GPIO_Pin_14 // PD.14
-  #define LCD_GPIO_PIN_A0               GPIO_Pin_13 // PD.13
-  #define LCD_GPIO_PIN_RST              GPIO_Pin_12 // PD.12
+  #define LCD_SPI_GPIO                  GPIOD
+  #define LCD_MOSI_GPIO_PIN             GPIO_Pin_10 // PD.10
+  #define LCD_CLK_GPIO_PIN              GPIO_Pin_11 // PD.11
+  #define LCD_A0_GPIO_PIN               GPIO_Pin_13 // PD.13
+  #define LCD_NCS_GPIO                  GPIOD
+  #define LCD_NCS_GPIO_PIN              GPIO_Pin_14 // PD.14
+  #define LCD_RST_GPIO                  GPIOD
+  #define LCD_RST_GPIO_PIN              GPIO_Pin_12 // PD.12
 #endif
 
 // I2C Bus: EEPROM and CAT5137 digital pot for volume control
@@ -588,6 +603,8 @@
 // Audio
 #define AUDIO_RCC_AHB1Periph            (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_DMA1)
 #define AUDIO_RCC_APB1Periph            (RCC_APB1Periph_TIM6 | RCC_APB1Periph_DAC)
+#define AUDIO_OUTPUT_GPIO               GPIOA
+#define AUDIO_OUTPUT_GPIO_PIN           GPIO_Pin_4  // PA.04
 #define AUDIO_DMA_Stream                DMA1_Stream5
 #define AUDIO_DMA_Stream_IRQn           DMA1_Stream5_IRQn
 #define AUDIO_TIM_IRQn                  TIM6_DAC_IRQn
diff --git a/radio/src/targets/taranis/lcd_driver.cpp b/radio/src/targets/taranis/lcd_driver.cpp
deleted file mode 100644
index 5e059f208..000000000
--- a/radio/src/targets/taranis/lcd_driver.cpp
+++ /dev/null
@@ -1,447 +0,0 @@
-/*
- * Copyright (C) OpenTX
- *
- * Based on code named
- *   th9x - http://code.google.com/p/th9x 
- *   er9x - http://code.google.com/p/er9x
- *   gruvin9x - http://code.google.com/p/gruvin9x
- *
- * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program 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.
- */
-
-#include "opentx.h"
-
-#define	WriteData(x)	 AspiData(x)
-#define	WriteCommand(x)	 AspiCmd(x)
-
-#if defined(PCBX9E) || defined(PCBX9DP)
-  #define CONTRAST_OFS 160
-  #define RESET_WAIT_DELAY_MS      300        //wait time after LCD reset before first command
-  #define WAIT_FOR_DMA_END()       { while(lcd_busy) {}; }
-#else
-  #define CONTRAST_OFS 5
-  #define RESET_WAIT_DELAY_MS     1300        //wait time after LCD reset before first command
-  #define WAIT_FOR_DMA_END()
-#endif
-
-bool lcdInitFinished = false;
-void lcdInitFinish();
-
-#if defined(PCBX9E) || defined(PCBX9DP)
-
-// New hardware SPI driver for LCD
-void initLcdSpi()
-{
-  // APB1 clock / 2 = 133nS per clock
-  LCD_SPI->CR1 = 0 ;		// Clear any mode error
-  LCD_SPI->CR1 = SPI_CR1_SSM | SPI_CR1_SSI | SPI_CR1_CPOL | SPI_CR1_CPHA ;
-  LCD_SPI->CR2 = 0 ;
-  LCD_SPI->CR1 |= SPI_CR1_MSTR ;	// Make sure in case SSM/SSI needed to be set first
-  LCD_SPI->CR1 |= SPI_CR1_SPE ;
-
-  configure_pins( LCD_GPIO_PIN_NCS, PIN_OUTPUT | PIN_PORTA | PIN_OS25) ;
-  configure_pins( LCD_GPIO_PIN_RST, PIN_OUTPUT | PIN_PORTD | PIN_OS25) ;
-  configure_pins( LCD_GPIO_PIN_A0,  PIN_OUTPUT | PIN_PORTC | PIN_OS50) ;
-  configure_pins( LCD_GPIO_PIN_MOSI|LCD_GPIO_PIN_CLK, PIN_PORTC | PIN_OS50 | PIN_PER_6 | PIN_PERIPHERAL ) ;
-
-  LDC_DMA_Stream->CR &= ~DMA_SxCR_EN ;    // Disable DMA
-  LCD_DMA->HIFCR = LCD_DMA_FLAGS ; // Write ones to clear bits
-  LDC_DMA_Stream->CR =  DMA_SxCR_PL_0 | DMA_SxCR_MINC | DMA_SxCR_DIR_0 ;
-  LDC_DMA_Stream->PAR = (uint32_t) &LCD_SPI->DR ;
-  LDC_DMA_Stream->M0AR = (uint32_t)displayBuf;
-  LDC_DMA_Stream->FCR = 0x05 ; //DMA_SxFCR_DMDIS | DMA_SxFCR_FTH_0 ;
-  LDC_DMA_Stream->NDTR = LCD_W*LCD_H/8*4 ;
-
-  NVIC_EnableIRQ(LCD_DMA_Stream_IRQn) ;
-}
-
-static void LCD_Init()
-{
-  WriteCommand(0x2F);   //Internal pump control
-  delay_ms(20);
-  WriteCommand(0x24);   //Temperature compensation
-  WriteCommand(0xE9);   //set bias=1/10
-  WriteCommand(0x81);   //Set Vop
-#if defined(BOOT)
-  AspiCmd(CONTRAST_OFS+25);
-#else
-  AspiCmd(CONTRAST_OFS+g_eeGeneral.contrast);
-#endif
-  WriteCommand(0xA2);   //set line rate:28KLPS
-  WriteCommand(0x28);   //set pannel loading
-  WriteCommand(0x40);   //scroll line LSB
-  WriteCommand(0x50);   //SCROLL LINE MSB
-  WriteCommand(0x89);   //ram address control
-  WriteCommand(0xC0);   //LCD mapping control
-  WriteCommand(0x04);   //MX=0,MY=1
-  WriteCommand(0xD0);   //DISPLAY PATTERN = 16-SCALE GRAY
-  WriteCommand(0xF1);   //SET COM end
-  WriteCommand(0x3F);   //64
-
-  WriteCommand(0xF8);   //Set Window Program Disable.
-
-  WriteCommand(0xF5);   //starting row address of RAM program window.PAGE1
-  WriteCommand(0x00);
-  WriteCommand(0xF7);   //end row address of RAM program window.PAGE32
-  WriteCommand(0x1F);
-  WriteCommand(0xF4);   //start column address of RAM program window.
-  WriteCommand(0x00);
-  WriteCommand(0xF6);   //end column address of RAM program window.SEG212
-  WriteCommand(0xD3);
-}
-#else
-static void LCD_Init()
-{	
-  AspiCmd(0x2B);   //Panel loading set ,Internal VLCD.
-  delay_ms(20);
-  AspiCmd(0x25);   //Temperature compensation curve definition: 0x25 = -0.05%/oC
-  AspiCmd(0xEA);	//set bias=1/10 :Command table NO.27
-  AspiCmd(0x81);	//Set Vop
-#if defined(BOOT)
-  AspiCmd(CONTRAST_OFS+25);
-#else
-  AspiCmd(CONTRAST_OFS+g_eeGeneral.contrast);
-#endif
-  AspiCmd(0xA6);	//inverse display off
-  AspiCmd(0xD1);	//SET RGB:Command table NO.21 .SET RGB or BGR.  D1=RGB
-  AspiCmd(0xD5);	//set color mode 4K and 12bits  :Command table NO.22
-  AspiCmd(0xA0);	//line rates,25.2 Klps
-  AspiCmd(0xC8);	//SET N-LINE INVERSION
-  AspiCmd(0x1D);	//Disable NIV
-  AspiCmd(0xF1);	//Set CEN
-  AspiCmd(0x3F);	// 1/64DUTY
-  AspiCmd(0x84);	//Disable Partial Display
-  AspiCmd(0xC4);	//MY=1,MX=0
-  AspiCmd(0x89);	//WA=1,column (CA) increment (+1) first until CA reaches CA boundary, then RA will increment by (+1).
-
-  AspiCmd(0xF8);	//Set Window Program Enable  ,inside modle
-  AspiCmd(0xF4);   //starting column address of RAM program window.
-  AspiCmd(0x00);
-  AspiCmd(0xF5);   //starting row address of RAM program window.
-  AspiCmd(0x60);
-  AspiCmd(0xF6);   //ending column address of RAM program window.
-  AspiCmd(0x47);
-  AspiCmd(0xF7);   //ending row address of RAM program window.
-  AspiCmd(0x9F);
-}
-#endif
-
-void Set_Address(u8 x, u8 y)
-{
-  WriteCommand(x&0x0F);	//Set Column Address LSB CA[3:0]
-  WriteCommand((x>>4)|0x10);	//Set Column Address MSB CA[7:4]
-    
-  WriteCommand((y&0x0F)|0x60);	//Set Row Address LSB RA [3:0]
-  WriteCommand(((y>>4)&0x0F)|0x70);    //Set Row Address MSB RA [7:4]
-}
-
-#define LCD_WRITE_BIT(bit) \
-  if (bit) \
-    LCD_MOSI_HIGH(); \
-  else \
-    LCD_MOSI_LOW(); \
-  LCD_CLK_LOW(); \
-  LCD_CLK_LOW(); \
-  LCD_CLK_LOW(); \
-  LCD_CLK_HIGH(); \
-  LCD_CLK_HIGH();
-
-#if defined(PCBX9E) || defined(PCBX9DP)
-
-volatile bool lcd_busy;
-
-#if !defined(LCD_DUAL_BUFFER)
-void lcdRefreshWait() 
-{
-  WAIT_FOR_DMA_END();
-}
-#endif
-
-void lcdRefresh(bool wait)
-{
-  if (!lcdInitFinished) {
-    lcdInitFinish();
-  }
-
-  //wait if previous DMA transfer still active
-  WAIT_FOR_DMA_END();
-  lcd_busy = true;
-
-  Set_Address(0, 0);
-	
-  LCD_NCS_LOW();
-  LCD_A0_HIGH();
-
-  LDC_DMA_Stream->CR &= ~DMA_SxCR_EN ;    // Disable DMA
-  LCD_DMA->HIFCR = LCD_DMA_FLAGS ; // Write ones to clear bits
-
-#if defined(LCD_DUAL_BUFFER)
-  //switch LCD buffer
-  LDC_DMA_Stream->M0AR = (uint32_t)displayBuf;
-  displayBuf = (displayBuf == displayBuf1) ? displayBuf2 : displayBuf1;
-#endif
-
-  LDC_DMA_Stream->CR |= DMA_SxCR_EN | DMA_SxCR_TCIE;		// Enable DMA & TC interrupts
-  LCD_SPI->CR2 |= SPI_CR2_TXDMAEN ;
-}
-
-extern "C" void LCD_DMA_Stream_IRQHandler()
-{
-  DEBUG_INTERRUPT(INT_LCD);
-  //clear interrupt flag
-  LDC_DMA_Stream->CR &= ~DMA_SxCR_TCIE ;  // Stop interrupt
-  LCD_DMA->HIFCR |= LCD_DMA_FLAG_INT;     // Clear interrupt flag
-  LCD_SPI->CR2 &= ~SPI_CR2_TXDMAEN ;
-  LDC_DMA_Stream->CR &= ~DMA_SxCR_EN ;    // Disable DMA
-
-  while ( LCD_SPI->SR & SPI_SR_BSY ) {
-    /* Wait for SPI to finish sending data 
-    The DMA TX End interrupt comes two bytes before the end of SPI transmission,
-    therefore we have to wait here.
-    */
-  }
-  LCD_NCS_HIGH();
-  lcd_busy = false;
-}
-
-#else // #if defined(PCBX9E) || defined(PCBX9DP)
-void lcdRefresh()
-{  
-  if (!lcdInitFinished) {
-    lcdInitFinish();
-  }
-
-  for (uint32_t y=0; y<LCD_H; y++) {
-    uint8_t *p = &displayBuf[y/2 * LCD_W];
-
-    Set_Address(0, y);
-    AspiCmd(0xAF);
-
-    LCD_CLK_HIGH();
-    LCD_A0_HIGH();
-    LCD_NCS_LOW();
-
-    for (uint32_t x=0; x<LCD_W; x++) {
-      uint8_t b = p[x];
-      if (y & 1)
-        b >>= 4;
-      LCD_WRITE_BIT(b & 0x08);
-      LCD_WRITE_BIT(b & 0x04);
-      LCD_WRITE_BIT(b & 0x02);
-      LCD_WRITE_BIT(b & 0x01);
-    }
-
-    LCD_NCS_HIGH();
-    LCD_A0_HIGH();
-
-    WriteData(0);
-  }
-}
-#endif
-
-void backlightInit()
-{
-  GPIO_InitTypeDef GPIO_InitStructure;
-  
-#if defined(PCBX9E)
-  GPIO_InitStructure.GPIO_Pin = BACKLIGHT_GPIO_PIN_1|BACKLIGHT_GPIO_PIN_2;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
-  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
-  GPIO_Init(BACKLIGHT_GPIO, &GPIO_InitStructure);
-  GPIO_PinAFConfig(BACKLIGHT_GPIO, BACKLIGHT_GPIO_PinSource_1, BACKLIGHT_GPIO_AF_1);
-  GPIO_PinAFConfig(BACKLIGHT_GPIO, BACKLIGHT_GPIO_PinSource_2, BACKLIGHT_GPIO_AF_1);
-  BACKLIGHT_TIMER->ARR = 100 ;
-  BACKLIGHT_TIMER->PSC = (PERI2_FREQUENCY * TIMER_MULT_APB2) / 50000 - 1;  // 20us * 100 = 2ms => 500Hz
-  BACKLIGHT_TIMER->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2 ; // PWM
-  BACKLIGHT_TIMER->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E ;
-  BACKLIGHT_TIMER->CCR2 = 0 ;
-  BACKLIGHT_TIMER->CCR1 = 100 ;
-  BACKLIGHT_TIMER->EGR = 0 ;
-  BACKLIGHT_TIMER->CR1 = TIM_CR1_CEN ;            // Counter enable
-#elif defined(PCBX9DP)
-  GPIO_InitStructure.GPIO_Pin = BACKLIGHT_GPIO_PIN_1|BACKLIGHT_GPIO_PIN_2;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
-  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
-  GPIO_Init(BACKLIGHT_GPIO, &GPIO_InitStructure);
-  GPIO_PinAFConfig(BACKLIGHT_GPIO, BACKLIGHT_GPIO_PinSource_1, BACKLIGHT_GPIO_AF_1);
-  GPIO_PinAFConfig(BACKLIGHT_GPIO, BACKLIGHT_GPIO_PinSource_2, BACKLIGHT_GPIO_AF_1);
-  BACKLIGHT_TIMER->ARR = 100 ;
-  BACKLIGHT_TIMER->PSC = (PERI1_FREQUENCY * TIMER_MULT_APB1) / 50000 - 1;  // 20us * 100 = 2ms => 500Hz
-  BACKLIGHT_TIMER->CCMR1 = TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2 ; // PWM
-  BACKLIGHT_TIMER->CCMR2 = TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2 ; // PWM
-  BACKLIGHT_TIMER->CCER = TIM_CCER_CC4E | TIM_CCER_CC2E ;
-  BACKLIGHT_TIMER->CCR2 = 0 ;
-  BACKLIGHT_TIMER->CCR4 = 100 ;
-  BACKLIGHT_TIMER->EGR = 0 ;
-  BACKLIGHT_TIMER->CR1 = TIM_CR1_CEN ;            // Counter enable
-#else
-  GPIO_InitStructure.GPIO_Pin = BACKLIGHT_GPIO_PIN_1;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
-  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
-  GPIO_Init(BACKLIGHT_GPIO, &GPIO_InitStructure);
-  GPIO_PinAFConfig(BACKLIGHT_GPIO, BACKLIGHT_GPIO_PinSource_1, BACKLIGHT_GPIO_AF_1);
-  BACKLIGHT_TIMER->ARR = 100 ;
-  BACKLIGHT_TIMER->PSC = (PERI2_FREQUENCY * TIMER_MULT_APB2) / 50000 - 1;  // 20us * 100 = 2ms => 500Hz
-  BACKLIGHT_TIMER->CCMR1 = 0x60 ;    // PWM
-  BACKLIGHT_TIMER->CCER = 1 ;
-  BACKLIGHT_TIMER->CCR1 = 80;
-  BACKLIGHT_TIMER->EGR = 0 ;
-  BACKLIGHT_TIMER->CR1 = 1 ;
-#endif
-}
-
-/** Init the analog SPI GPIO
-*/
-static void LCD_Hardware_Init()
-{
-  GPIO_InitTypeDef GPIO_InitStructure;
-  
-  /*!< Configure lcd CLK\ MOSI\ A0pin in output push-pull mode *************/
-  GPIO_InitStructure.GPIO_Pin = LCD_GPIO_PIN_MOSI | LCD_GPIO_PIN_CLK | LCD_GPIO_PIN_A0;
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
-  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
-  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
-  GPIO_Init(LCD_GPIO_SPI, &GPIO_InitStructure);
-  
-  LCD_NCS_HIGH();
-  
-  /*!< Configure lcd NCS pin in output push-pull mode ,PULLUP *************/
-  GPIO_InitStructure.GPIO_Pin = LCD_GPIO_PIN_NCS; 
-  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
-  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
-  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
-  GPIO_Init(LCD_GPIO_NCS, &GPIO_InitStructure);
-  
-  /*!< Configure lcd RST pin in output pushpull mode ,PULLUP *************/
-  GPIO_InitStructure.GPIO_Pin = LCD_GPIO_PIN_RST; 
-  GPIO_Init(LCD_GPIO_RST, &GPIO_InitStructure);
-}
-
-/*
-  Proper method for turning of LCD module. It must be used,
-  otherwise we might damage LCD crystals in the long run!
-*/
-void lcdOff()
-{
-  WAIT_FOR_DMA_END();
-  /* 
-  LCD Sleep mode is also good for draining capacitors and enables us
-  to re-init LCD without any delay
-  */
-  AspiCmd(0xAE);    //LCD sleep
-  delay_ms(3);	        //wait for caps to drain
-}
-
-/*
-  Starts LCD initialization routine. It should be called as
-  soon as possible after the reset because LCD takes a lot of
-  time to properly power-on.
-
-  Make sure that delay_ms() is functional before calling this function!
-*/
-void lcdInit()
-{
-  LCD_Hardware_Init();
-
-  if (WAS_RESET_BY_WATCHDOG_OR_SOFTWARE()) return;    //no need to reset LCD module
-
-  //reset LCD module
-  LCD_RST_LOW();
-  delay_ms(1);       // only 3 us needed according to data-sheet, we use 1 ms
-  LCD_RST_HIGH();
-}
-
-/*
-  Finishes LCD initialization. It is called auto-magically when first LCD command is 
-  issued by the other parts of the code.
-*/
-void lcdInitFinish()
-{
-  lcdInitFinished = true;
-
-#if defined(PCBX9E) || defined(PCBX9DP)
-  initLcdSpi();
-#endif
-  
-  /*
-    LCD needs longer time to initialize in low temperatures. The data-sheet 
-    mentions a time of at least 150 ms. The delay of 1300 ms was obtained 
-    experimentally. It was tested down to -10 deg Celsius.
-
-    The longer initialization time seems to only be needed for regular Taranis, 
-    the Taranis Plus (9XE) has been tested to work without any problems at -18 deg Celsius.
-    Therefore the delay for T+ is lower.
-    
-    If radio is reset by watchdog or boot-loader the wait is skipped, but the LCD
-    is initialized in any case. 
-
-    This initialization is needed in case the user moved power switch to OFF and 
-    then immediately to ON position, because lcdOff() was called. In any case the LCD 
-    initialization (without reset) is also recommended by the data sheet.
-  */
-
-  if (!WAS_RESET_BY_WATCHDOG_OR_SOFTWARE()) {
-#if !defined(BOOT)
-    while(g_tmr10ms < (RESET_WAIT_DELAY_MS/10)) {};    //wait measured from the power-on
-#else
-    delay_ms(RESET_WAIT_DELAY_MS);
-#endif
-  }
-  
-  LCD_Init();
-  AspiCmd(0xAF);	//dc2=1, IC into exit SLEEP MODE, dc3=1 gray=ON, dc4=1 Green Enhanc mode disabled
-  delay_ms(20);      //needed for internal DC-DC converter startup
-}
-
-void lcdSetRefVolt(uint8_t val)
-{
-  if (!lcdInitFinished) {
-    lcdInitFinish();
-  }
-  WAIT_FOR_DMA_END();
-  AspiCmd(0x81);	//Set Vop
-  AspiCmd(val+CONTRAST_OFS);		//0--255
-}
-
-#if defined(PCBX9E)
-void turnBacklightOn(uint8_t level, uint8_t color)
-{
-  BACKLIGHT_TIMER->CCR1 = ((100-level)*(20-color))/20;
-  BACKLIGHT_TIMER->CCR2 = ((100-level)*color)/20;
-}
-
-void turnBacklightOff(void)
-{
-  BACKLIGHT_TIMER->CCR1 = 0;
-  BACKLIGHT_TIMER->CCR2 = 0;
-}
-#elif defined(PCBX9DP)
-void turnBacklightOn(uint8_t level, uint8_t color)
-{
-  BACKLIGHT_TIMER->CCR4 = ((100-level)*(20-color))/20;
-  BACKLIGHT_TIMER->CCR2 = ((100-level)*color)/20;
-}
-
-void turnBacklightOff(void)
-{
-  BACKLIGHT_TIMER->CCR4 = 0;
-  BACKLIGHT_TIMER->CCR2 = 0;
-}
-#endif
diff --git a/radio/src/targets/taranis/lcd_driver_spi.cpp b/radio/src/targets/taranis/lcd_driver_spi.cpp
new file mode 100644
index 000000000..8abdaa549
--- /dev/null
+++ b/radio/src/targets/taranis/lcd_driver_spi.cpp
@@ -0,0 +1,291 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Based on code named
+ *   th9x - http://code.google.com/p/th9x 
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program 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.
+ */
+
+#include "opentx.h"
+
+#define CONTRAST_OFS 160
+#define RESET_WAIT_DELAY_MS            300 // Wait time after LCD reset before first command
+#define WAIT_FOR_DMA_END()       { while(lcd_busy) {}; }
+
+#define LCD_NCS_HIGH()                 LCD_NCS_GPIO->BSRRL = LCD_NCS_GPIO_PIN
+#define LCD_NCS_LOW()                  LCD_NCS_GPIO->BSRRH = LCD_NCS_GPIO_PIN
+
+#define LCD_A0_HIGH()                  LCD_SPI_GPIO->BSRRL = LCD_A0_GPIO_PIN
+#define LCD_A0_LOW()                   LCD_SPI_GPIO->BSRRH = LCD_A0_GPIO_PIN
+
+#define LCD_RST_HIGH()                 LCD_RST_GPIO->BSRRL = LCD_RST_GPIO_PIN
+#define LCD_RST_LOW()                  LCD_RST_GPIO->BSRRH = LCD_RST_GPIO_PIN
+
+bool lcdInitFinished = false;
+void lcdInitFinish();
+
+void lcdWriteCommand(uint8_t Command_Byte)
+{
+  LCD_A0_LOW();
+  LCD_NCS_LOW();
+  while ( ( SPI3->SR & SPI_SR_TXE ) == 0 ) {
+    // Wait
+  }
+  (void)SPI3->DR;		// Clear receive
+  SPI3->DR = Command_Byte;
+  while ( ( SPI3->SR & SPI_SR_RXNE ) == 0 ) {
+    // Wait
+  }
+  LCD_NCS_HIGH();
+}
+
+void lcdInitSpi()
+{
+  // APB1 clock / 2 = 133nS per clock
+  LCD_SPI->CR1 = 0; // Clear any mode error
+  LCD_SPI->CR1 = SPI_CR1_SSM | SPI_CR1_SSI | SPI_CR1_CPOL | SPI_CR1_CPHA;
+  LCD_SPI->CR2 = 0;
+  LCD_SPI->CR1 |= SPI_CR1_MSTR;	// Make sure in case SSM/SSI needed to be set first
+  LCD_SPI->CR1 |= SPI_CR1_SPE;
+  
+  LDC_DMA_Stream->CR &= ~DMA_SxCR_EN; // Disable DMA
+  LCD_DMA->HIFCR = LCD_DMA_FLAGS; // Write ones to clear bits
+  LDC_DMA_Stream->CR =  DMA_SxCR_PL_0 | DMA_SxCR_MINC | DMA_SxCR_DIR_0;
+  LDC_DMA_Stream->PAR = (uint32_t) &LCD_SPI->DR;
+  LDC_DMA_Stream->M0AR = (uint32_t)displayBuf;
+  LDC_DMA_Stream->FCR = 0x05; // DMA_SxFCR_DMDIS | DMA_SxFCR_FTH_0;
+  LDC_DMA_Stream->NDTR = LCD_W*LCD_H/8*4;
+
+  NVIC_EnableIRQ(LCD_DMA_Stream_IRQn);
+}
+
+void lcdHardwareInit()
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  
+  LCD_NCS_HIGH();
+  
+  GPIO_InitStructure.GPIO_Pin = LCD_NCS_GPIO_PIN;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+  GPIO_Init(LCD_NCS_GPIO, &GPIO_InitStructure);
+  
+  GPIO_InitStructure.GPIO_Pin = LCD_RST_GPIO_PIN;
+  GPIO_Init(LCD_RST_GPIO, &GPIO_InitStructure);
+  
+  GPIO_InitStructure.GPIO_Pin = LCD_A0_GPIO_PIN;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(LCD_SPI_GPIO, &GPIO_InitStructure);
+  
+  GPIO_InitStructure.GPIO_Pin = LCD_CLK_GPIO_PIN | LCD_MOSI_GPIO_PIN;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_Init(LCD_SPI_GPIO, &GPIO_InitStructure);
+  
+  GPIO_PinAFConfig(LCD_SPI_GPIO, LCD_MOSI_GPIO_PinSource, LCD_GPIO_AF);
+  GPIO_PinAFConfig(LCD_SPI_GPIO, LCD_CLK_GPIO_PinSource, LCD_GPIO_AF);
+}
+
+void lcdStart()
+{
+  lcdWriteCommand(0x2F); // Internal pump control
+  delay_ms(20);
+  lcdWriteCommand(0x24); // Temperature compensation
+  lcdWriteCommand(0xE9); // Set bias=1/10
+  lcdWriteCommand(0x81); // Set Vop
+#if defined(BOOT)
+  lcdWriteCommand(CONTRAST_OFS+25);
+#else
+  lcdWriteCommand(CONTRAST_OFS+g_eeGeneral.contrast);
+#endif
+  lcdWriteCommand(0xA2); // Set line rate: 28KLPS
+  lcdWriteCommand(0x28); // Set panel loading
+  lcdWriteCommand(0x40); // Scroll line LSB
+  lcdWriteCommand(0x50); // Scroll line MSB
+  lcdWriteCommand(0x89); // RAM address control
+  lcdWriteCommand(0xC0); // LCD mapping control
+  lcdWriteCommand(0x04); // MX=0, MY=1
+  lcdWriteCommand(0xD0); // Display pattern = 16-SCALE GRAY
+  lcdWriteCommand(0xF1); // Set COM end
+  lcdWriteCommand(0x3F); // 64
+
+  lcdWriteCommand(0xF8); // Set Window Program Disable.
+
+  lcdWriteCommand(0xF5); // Start row address of RAM program window. PAGE1
+  lcdWriteCommand(0x00);
+  lcdWriteCommand(0xF7); // End row address of RAM program window. PAGE32
+  lcdWriteCommand(0x1F);
+  lcdWriteCommand(0xF4); // Start column address of RAM program window.
+  lcdWriteCommand(0x00);
+  lcdWriteCommand(0xF6); // End column address of RAM program window. SEG212
+  lcdWriteCommand(0xD3);
+}
+
+void lcdWriteAddress(uint8_t x, uint8_t y)
+{
+  lcdWriteCommand(x & 0x0F); // Set Column Address LSB CA[3:0]
+  lcdWriteCommand((x>>4) | 0x10); // Set Column Address MSB CA[7:4]
+    
+  lcdWriteCommand((y&0x0F) | 0x60); // Set Row Address LSB RA [3:0]
+  lcdWriteCommand(((y>>4) & 0x0F) | 0x70); // Set Row Address MSB RA [7:4]
+}
+
+volatile bool lcd_busy;
+
+#if !defined(LCD_DUAL_BUFFER)
+void lcdRefreshWait() 
+{
+  WAIT_FOR_DMA_END();
+}
+#endif
+
+void lcdRefresh(bool wait)
+{
+  if (!lcdInitFinished) {
+    lcdInitFinish();
+  }
+
+  // Wait if previous DMA transfer still active
+  WAIT_FOR_DMA_END();
+  lcd_busy = true;
+
+  lcdWriteAddress(0, 0);
+	
+  LCD_NCS_LOW();
+  LCD_A0_HIGH();
+
+  LDC_DMA_Stream->CR &= ~DMA_SxCR_EN; // Disable DMA
+  LCD_DMA->HIFCR = LCD_DMA_FLAGS; // Write ones to clear bits
+
+#if defined(LCD_DUAL_BUFFER)
+  // Switch LCD buffer
+  LDC_DMA_Stream->M0AR = (uint32_t)displayBuf;
+  displayBuf = (displayBuf == displayBuf1) ? displayBuf2 : displayBuf1;
+#endif
+
+  LDC_DMA_Stream->CR |= DMA_SxCR_EN | DMA_SxCR_TCIE; // Enable DMA & TC interrupts
+  LCD_SPI->CR2 |= SPI_CR2_TXDMAEN;
+}
+
+extern "C" void LCD_DMA_Stream_IRQHandler()
+{
+  DEBUG_INTERRUPT(INT_LCD);
+
+  LDC_DMA_Stream->CR &= ~DMA_SxCR_TCIE; // Stop interrupt
+  LCD_DMA->HIFCR |= LCD_DMA_FLAG_INT; // Clear interrupt flag
+  LCD_SPI->CR2 &= ~SPI_CR2_TXDMAEN;
+  LDC_DMA_Stream->CR &= ~DMA_SxCR_EN; // Disable DMA
+
+  while ( LCD_SPI->SR & SPI_SR_BSY ) {
+    /* Wait for SPI to finish sending data 
+    The DMA TX End interrupt comes two bytes before the end of SPI transmission,
+    therefore we have to wait here.
+    */
+  }
+  LCD_NCS_HIGH();
+  lcd_busy = false;
+}
+
+/*
+  Proper method for turning of LCD module. It must be used,
+  otherwise we might damage LCD crystals in the long run!
+*/
+void lcdOff()
+{
+  WAIT_FOR_DMA_END();
+
+  /* 
+  LCD Sleep mode is also good for draining capacitors and enables us
+  to re-init LCD without any delay
+  */
+  lcdWriteCommand(0xAE);    //LCD sleep
+  delay_ms(3);	        //wait for caps to drain
+}
+
+void lcdReset()
+{
+  LCD_RST_LOW();
+  delay_ms(1);       // only 3 us needed according to data-sheet, we use 1 ms
+  LCD_RST_HIGH();
+}
+
+/*
+  Starts LCD initialization routine. It should be called as
+  soon as possible after the reset because LCD takes a lot of
+  time to properly power-on.
+
+  Make sure that delay_ms() is functional before calling this function!
+*/
+void lcdInit()
+{
+  lcdHardwareInit();
+
+  if (!WAS_RESET_BY_WATCHDOG_OR_SOFTWARE()) {
+    lcdReset();
+  }
+}
+
+/*
+  Finishes LCD initialization. It is called auto-magically when first LCD command is 
+  issued by the other parts of the code.
+*/
+void lcdInitFinish()
+{
+  lcdInitFinished = true;
+
+  lcdInitSpi();
+  
+  /*
+    LCD needs longer time to initialize in low temperatures. The data-sheet 
+    mentions a time of at least 150 ms. The delay of 1300 ms was obtained 
+    experimentally. It was tested down to -10 deg Celsius.
+
+    The longer initialization time seems to only be needed for regular Taranis, 
+    the Taranis Plus (9XE) has been tested to work without any problems at -18 deg Celsius.
+    Therefore the delay for T+ is lower.
+    
+    If radio is reset by watchdog or boot-loader the wait is skipped, but the LCD
+    is initialized in any case. 
+
+    This initialization is needed in case the user moved power switch to OFF and 
+    then immediately to ON position, because lcdOff() was called. In any case the LCD 
+    initialization (without reset) is also recommended by the data sheet.
+  */
+
+  if (!WAS_RESET_BY_WATCHDOG_OR_SOFTWARE()) {
+#if !defined(BOOT)
+    while (g_tmr10ms < (RESET_WAIT_DELAY_MS/10)); // wait measured from the power-on
+#else
+    delay_ms(RESET_WAIT_DELAY_MS);
+#endif
+  }
+  
+  lcdStart();
+  lcdWriteCommand(0xAF); // dc2=1, IC into exit SLEEP MODE, dc3=1 gray=ON, dc4=1 Green Enhanc mode disabled
+  delay_ms(20);      //needed for internal DC-DC converter startup
+}
+
+void lcdSetRefVolt(uint8_t val)
+{
+  if (!lcdInitFinished) {
+    lcdInitFinish();
+  }
+
+  WAIT_FOR_DMA_END();
+  
+  lcdWriteCommand(0x81); // Set Vop
+  lcdWriteCommand(val+CONTRAST_OFS); // 0-255
+}
diff --git a/radio/src/targets/taranis/trainer_driver.cpp b/radio/src/targets/taranis/trainer_driver.cpp
index 970177ed1..c26ca1762 100644
--- a/radio/src/targets/taranis/trainer_driver.cpp
+++ b/radio/src/targets/taranis/trainer_driver.cpp
@@ -164,16 +164,25 @@ void init_cppm_on_heartbeat_capture(void)
 {
   EXTERNAL_MODULE_ON();
 
-  configure_pins(HEARTBEAT_GPIO_PIN, PIN_PERIPHERAL | PIN_PORTC | PIN_PER_2);
+  GPIO_PinAFConfig(HEARTBEAT_GPIO, HEARTBEAT_GPIO_PinSource, HEARTBEAT_GPIO_AF_CAPTURE);
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = HEARTBEAT_GPIO_PIN;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+  GPIO_Init(HEARTBEAT_GPIO, &GPIO_InitStructure);
 
   TRAINER_TIMER->ARR = 0xFFFF;
-  TRAINER_TIMER->PSC = (PERI1_FREQUENCY * TIMER_MULT_APB1) / 2000000 - 1;               // 0.5uS
+  TRAINER_TIMER->PSC = (PERI1_FREQUENCY * TIMER_MULT_APB1) / 2000000 - 1; // 0.5uS
   TRAINER_TIMER->CR2 = 0;
   TRAINER_TIMER->CCMR1 = TIM_CCMR1_IC2F_0 | TIM_CCMR1_IC2F_1 | TIM_CCMR1_CC2S_0;
   TRAINER_TIMER->CCER = TIM_CCER_CC2E;
-  TRAINER_TIMER->SR &= ~TIM_SR_CC2IF;                             // Clear flag
+  TRAINER_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag
   TRAINER_TIMER->DIER |= TIM_DIER_CC2IE;
   TRAINER_TIMER->CR1 = TIM_CR1_CEN;
+
   NVIC_SetPriority(TRAINER_TIMER_IRQn, 7);
   NVIC_EnableIRQ(TRAINER_TIMER_IRQn);
 }
@@ -196,7 +205,7 @@ void init_sbus_on_heartbeat_capture()
   USART_InitTypeDef USART_InitStructure;
   GPIO_InitTypeDef GPIO_InitStructure;
 
-  GPIO_PinAFConfig(GPIOC, HEARTBEAT_GPIO_PinSource, HEARTBEAT_GPIO_AF);
+  GPIO_PinAFConfig(GPIOC, HEARTBEAT_GPIO_PinSource, HEARTBEAT_GPIO_AF_SBUS);
 
   GPIO_InitStructure.GPIO_Pin = HEARTBEAT_GPIO_PIN;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
@@ -240,7 +249,6 @@ void init_sbus_on_heartbeat_capture()
 
 void stop_sbus_on_heartbeat_capture(void)
 {
-  configure_pins(HEARTBEAT_GPIO_PIN, PIN_INPUT | PIN_PORTC);
   NVIC_DisableIRQ(HEARTBEAT_USART_IRQn);
 
   if (!IS_EXTERNAL_MODULE_PRESENT()) {