diff --git a/companion/src/firmwares/opentx/simulator/opentxsimulator.cpp b/companion/src/firmwares/opentx/simulator/opentxsimulator.cpp
index 8a9fa5ccd..ceac3ec18 100644
--- a/companion/src/firmwares/opentx/simulator/opentxsimulator.cpp
+++ b/companion/src/firmwares/opentx/simulator/opentxsimulator.cpp
@@ -232,7 +232,7 @@ namespace NAMESPACE {
 #include "radio/src/targets/taranis/pulses_driver.cpp"
 #include "radio/src/targets/taranis/rtc_driver.cpp"
 #include "radio/src/targets/taranis/trainer_driver.cpp"
-#include "radio/src/targets/taranis/uart3_driver.cpp"
+#include "radio/src/targets/taranis/serial2_driver.cpp"
 #elif defined(PCBSKY9X)
 #include "radio/src/targets/sky9x/board_sky9x.cpp"
 #include "radio/src/targets/sky9x/telemetry_driver.cpp"
@@ -243,7 +243,7 @@ namespace NAMESPACE {
 #include "radio/src/targets/sky9x/sdcard_driver.cpp"
 #include "radio/src/targets/sky9x/coproc_driver.cpp"
 #include "radio/src/targets/sky9x/haptic_driver.cpp"
-#include "radio/src/targets/sky9x/second_serial_driver.cpp"
+#include "radio/src/targets/sky9x/serial2_driver.cpp"
 #include "radio/src/targets/sky9x/pulses_driver.cpp"
 #elif defined(PCBGRUVIN9X)
 #include "radio/src/targets/gruvin9x/board_gruvin9x.cpp"
diff --git a/radio/src/Makefile b/radio/src/Makefile
index 59f2dbdc9..19e0e2473 100644
--- a/radio/src/Makefile
+++ b/radio/src/Makefile
@@ -268,6 +268,10 @@ SHUTDOWN_CONFIRMATION = NO
 # Values = YES, NO
 DEBUG = NO
 
+# Activate Command Line Interpreter
+# Values = NO, YES
+CLI = NO
+
 # Activate writing of SPORT telemetry received data to sport.log file
 # Values = YES, NO
 SPORT_FILE_LOG = NO
@@ -800,7 +804,7 @@ ifeq ($(PCB), $(filter $(PCB), SKY9X 9XRPRO))
   EEPROMSRC = eeprom_common.cpp eeprom_raw.cpp eeprom_conversions.cpp
   PULSESSRC = pulses/pulses_arm.cpp pulses/ppm_arm.cpp pulses/pxx_arm.cpp pulses/dsm2_arm.cpp
   CPPSRC += tasks_arm.cpp audio_arm.cpp haptic.cpp gui/$(GUIDIRECTORY)/view_about.cpp gui/$(GUIDIRECTORY)/view_text.cpp telemetry/telemetry.cpp
-  CPPSRC += targets/sky9x/telemetry_driver.cpp targets/sky9x/second_serial_driver.cpp targets/sky9x/pwr_driver.cpp targets/sky9x/adc_driver.cpp targets/sky9x/eeprom_driver.cpp targets/sky9x/pulses_driver.cpp targets/sky9x/keys_driver.cpp targets/sky9x/audio_driver.cpp targets/sky9x/buzzer_driver.cpp targets/sky9x/haptic_driver.cpp targets/sky9x/sdcard_driver.cpp targets/sky9x/massstorage.cpp
+  CPPSRC += targets/sky9x/telemetry_driver.cpp targets/sky9x/serial2_driver.cpp targets/sky9x/pwr_driver.cpp targets/sky9x/adc_driver.cpp targets/sky9x/eeprom_driver.cpp targets/sky9x/pulses_driver.cpp targets/sky9x/keys_driver.cpp targets/sky9x/audio_driver.cpp targets/sky9x/buzzer_driver.cpp targets/sky9x/haptic_driver.cpp targets/sky9x/sdcard_driver.cpp targets/sky9x/massstorage.cpp
   CPPSRC += loadboot.cpp debug.cpp
   BITMAPS += bitmaps/9X/splash.lbm bitmaps/9X/asterisk.lbm bitmaps/9X/about.lbm
   ifeq ($(SDCARD), YES)
@@ -927,8 +931,8 @@ ifeq ($(PCB), TARANIS)
   EEPROMSRC = eeprom_common.cpp eeprom_rlc.cpp eeprom_conversions.cpp
   PULSESSRC = pulses/pulses_arm.cpp pulses/ppm_arm.cpp pulses/pxx_arm.cpp
   CPPSRC += tasks_arm.cpp audio_arm.cpp sbus.cpp telemetry/telemetry.cpp
-  CPPSRC += targets/taranis/pulses_driver.cpp targets/taranis/keys_driver.cpp targets/taranis/adc_driver.cpp targets/taranis/trainer_driver.cpp targets/taranis/audio_driver.cpp targets/taranis/uart3_driver.cpp targets/taranis/telemetry_driver.cpp
-  CPPSRC += bmp.cpp gui/$(GUIDIRECTORY)/view_channels.cpp gui/$(GUIDIRECTORY)/view_about.cpp gui/$(GUIDIRECTORY)/view_text.cpp debug.cpp
+  CPPSRC += targets/taranis/pulses_driver.cpp targets/taranis/keys_driver.cpp targets/taranis/adc_driver.cpp targets/taranis/trainer_driver.cpp targets/taranis/audio_driver.cpp targets/taranis/serial2_driver.cpp targets/taranis/telemetry_driver.cpp
+  CPPSRC += bmp.cpp gui/$(GUIDIRECTORY)/view_channels.cpp gui/$(GUIDIRECTORY)/view_about.cpp gui/$(GUIDIRECTORY)/view_text.cpp debug.cpp serial.cpp
   CPPSRC += loadboot.cpp
   ifeq ($(PCBREV), REV9E)
     CPPSRC += targets/taranis/top_lcd_driver.cpp
@@ -1246,10 +1250,10 @@ ifeq ($(EXT), JETI)
 endif
 
 ifeq ($(EXT), MAVLINK)
- CPPDEFS += -DMAVLINK
- INCDIRS += thirdparty
- CPPSRC += telemetry/mavlink.cpp gui/$(GUIDIRECTORY)/view_mavlink.cpp serial.cpp
- EEPROM_VARIANT += ${MAVLINK_VARIANT}
+  CPPDEFS += -DMAVLINK
+  INCDIRS += thirdparty
+  CPPSRC += telemetry/mavlink.cpp gui/$(GUIDIRECTORY)/view_mavlink.cpp targets/common_avr/serial_driver.cpp
+  EEPROM_VARIANT += ${MAVLINK_VARIANT}
 endif
 
 ifeq ($(EXT), TELEMETREZ)
@@ -1290,8 +1294,15 @@ ifeq ($(EXT), $(filter $(EXT), FRSKY FRSKY_SPORT TELEMETREZ))
   EEPROM_VARIANT += ${FRSKY_VARIANT}
 endif
 
-ifeq ($(DEBUG), YES)
+ifeq ($(SIMU), YES)
   CPPDEFS += -DDEBUG
+  CPPSRC += dump.cpp
+else ifeq ($(CLI), YES)
+  CPPDEFS += -DCLI -DDEBUG
+  CPPSRC += cli.cpp dump.cpp
+else ifeq ($(DEBUG), YES)
+  CPPDEFS += -DDEBUG
+  CPPSRC += dump.cpp
 endif
 
 ifeq ($(EEPROM_PROGRESS_BAR), YES)
diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp
new file mode 100644
index 000000000..b3ac3ad13
--- /dev/null
+++ b/radio/src/cli.cpp
@@ -0,0 +1,315 @@
+/*
+ * Authors (alphabetical order)
+ * - Andre Bernet <bernet.andre@gmail.com>
+ * - Andreas Weitl
+ * - Bertrand Songis <bsongis@gmail.com>
+ * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
+ * - Cameron Weeks <th9xer@gmail.com>
+ * - Erez Raviv
+ * - Gabriel Birkus
+ * - Jean-Pierre Parisy
+ * - Karl Szmutny
+ * - Michael Blandford
+ * - Michal Hlavinka
+ * - Pat Mackenzie
+ * - Philip Moss
+ * - Rob Thomson
+ * - Romolo Manfredini <romolo.manfredini@gmail.com>
+ * - Thomas Husterer
+ *
+ * opentx is based on code named
+ * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
+ * er9x by Erez Raviv: http://code.google.com/p/er9x/,
+ * and the original (and ongoing) project by
+ * Thomas Husterer, th9x: http://code.google.com/p/th9x/
+ *
+ * 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"
+#include <ctype.h>
+
+#define CLI_STACK_SIZE    500
+#define CLI_MAX_ARGS      8
+
+extern Fifo<512> uart3TxFifo;
+OS_TID cliTaskId;
+OS_STK cliStack[CLI_STACK_SIZE];
+Fifo<256> cliRxFifo;
+uint8_t cliTracesEnabled = false;
+
+typedef int (* CliFunction) (const char ** args);
+
+struct CliCommand
+{
+  const char * name;
+  CliFunction func;
+  const char * args;
+};
+
+struct MemArea
+{
+  const char * name;
+  void * start;
+  int size;
+};
+
+void cliPrompt()
+{
+  serialPutc('>');
+}
+
+int toInt(const char ** argv, int index, int * val)
+{
+  if (*argv[index] == '\0') {
+    return 0;
+  }
+  else {
+    int base = 10;
+    const char * s = argv[index];
+    if (strlen(s) > 2 && s[0] == '0' && s[1] == 'x') {
+      base = 16;
+      s = &argv[index][2];
+    }
+    char * endptr = NULL;
+    *val = strtol(s, &endptr, base);
+    if (*endptr == '\0')
+      return 1;
+    else {
+      serialPrint("%s: Invalid argument \"%s\"", argv[0], argv[index]);
+      return -1;
+    }
+  }
+}
+
+int cliBeep(const char ** argv)
+{
+  int freq = BEEP_DEFAULT_FREQ;
+  int duration = 100;
+  if (toInt(argv, 1, &freq) >= 0 && toInt(argv, 2, &duration) >= 0) {
+    audioQueue.playTone(freq, duration, 20, PLAY_NOW);
+  }
+  return 0;
+}
+
+int cliPlay(const char ** argv)
+{
+  audioQueue.playFile(argv[1], PLAY_NOW);
+  return 0;
+}
+
+int cliLs(const char ** argv)
+{
+  FILINFO fno;
+  DIR dir;
+  char *fn;   /* This function is assuming non-Unicode cfg. */
+#if _USE_LFN
+  TCHAR lfn[_MAX_LFN + 1];
+  fno.lfname = lfn;
+  fno.lfsize = sizeof(lfn);
+#endif
+
+  FRESULT res = f_opendir(&dir, argv[1]);        /* Open the directory */
+  if (res == FR_OK) {
+    for (;;) {
+      res = f_readdir(&dir, &fno);                   /* Read a directory item */
+      if (res != FR_OK || fno.fname[0] == 0) break;  /* Break on error or end of dir */
+
+#if _USE_LFN
+      fn = *fno.lfname ? fno.lfname : fno.fname;
+#else
+      fn = fno.fname;
+#endif
+      serialPrint(fn);
+    }
+  }
+  else {
+    serialPrint("%s: Invalid directory \"%s\"", argv[0], argv[1]);
+  }
+  return 0;
+}
+
+int cliTrace(const char ** argv)
+{
+  if (!strcmp(argv[1], "on")) {
+    cliTracesEnabled = true;
+  }
+  else if (!strcmp(argv[1], "off")) {
+    cliTracesEnabled = false;
+  }
+  else {
+    serialPrint("%s: Invalid argument \"%s\"", argv[0], argv[1]);
+  }
+  return 0;
+}
+
+int cliVolume(const char ** argv)
+{
+  int level = 0;
+  if (toInt(argv, 1, &level) > 0) {
+    setVolume(level);
+  }
+  else {
+    serialPrint("%s: Invalid argument \"%s\"", argv[0], argv[1]);
+  }
+  return 0;
+}
+
+const MemArea memAreas[] = {
+  { "RCC", RCC, sizeof(RCC_TypeDef) },
+  { "GPIOA", GPIOA, sizeof(GPIO_TypeDef) },
+  { "GPIOB", GPIOB, sizeof(GPIO_TypeDef) },
+  { "GPIOC", GPIOC, sizeof(GPIO_TypeDef) },
+  { "GPIOD", GPIOD, sizeof(GPIO_TypeDef) },
+  { "GPIOE", GPIOE, sizeof(GPIO_TypeDef) },
+  { "GPIOF", GPIOF, sizeof(GPIO_TypeDef) },
+  { "GPIOG", GPIOG, sizeof(GPIO_TypeDef) },
+  { "USART1", USART1, sizeof(USART_TypeDef) },
+  { "USART3", USART3, sizeof(USART_TypeDef) },
+  { NULL, NULL, 0 },
+};
+
+int cliDisplay(const char ** argv)
+{
+  int address = 0;
+
+  for (const MemArea * area = memAreas; area->name != NULL; area++) {
+    if (!strcmp(area->name, argv[1])) {
+      dump((uint8_t *)area->start, area->size);
+      return 0;
+    }
+  }
+
+  if (!strcmp(argv[1], "adc")) {
+    for (int i=0; i<NUMBER_ANALOG; i++) {
+      serialPrint("adc[%d] = %04X", i, Analog_values[i]);
+    }
+  }
+  else if (!strcmp(argv[1], "outputs")) {
+    for (int i=0; i<NUM_CHNOUT; i++) {
+      serialPrint("outputs[%d] = %04X", i, channelOutputs[i]);
+    }
+  }
+  else if (toInt(argv, 1, &address) > 0) {
+    int size = 256;
+    if (toInt(argv, 2, &size) >= 0) {
+      dump((uint8_t *)address, size);
+    }
+  }
+  return 0;
+}
+
+int cliHelp(const char ** argv);
+
+const CliCommand cliCommands[] = {
+  { "beep", cliBeep, "[<frequency>] [<duration>]" },
+  { "ls", cliLs, "<directory>" },
+  { "play", cliPlay, "<filename>" },
+  { "print", cliDisplay, "<address> [<size>] | <what>" },
+  { "trace", cliTrace, "on | off" },
+  { "volume", cliVolume, "<level>" },
+  { "help", cliHelp, "[<command>]" },
+  { NULL, NULL, NULL }  /* sentinel */
+};
+
+int cliHelp(const char ** argv)
+{ 
+  for (const CliCommand * command = cliCommands; command->name != NULL; command++) {
+    if (argv[1][0] == '\0' || !strcmp(command->name, argv[0])) {
+      serialPrint("%s %s", command->name, command->args);
+      if (argv[1][0] != '\0') {
+        return 0;
+      }
+    }
+  }
+  if (argv[1][0] != '\0') {
+    serialPrint("Invalid command \"%s\"", argv[0]);
+  }
+  return -1;
+}
+
+int cliExecCommand(const char ** argv)
+{
+  if (argv[0][0] == '\0')
+    return 0;
+
+  for (const CliCommand * command = cliCommands; command->name != NULL; command++) {
+    if (!strcmp(command->name, argv[0])) {
+      return command->func(argv);
+    }
+  }
+  serialPrint("Invalid command \"%s\"", argv[0]);
+  return -1;
+}
+
+int cliExecLine(char * line)
+{
+  int len = strlen(line);
+  const char * argv[CLI_MAX_ARGS];
+  memset(argv, 0, sizeof(argv));
+  int argc = 1;
+  argv[0] = line;
+  for (int i=0; i<len; i++) {
+    if (line[i] == ' ') {
+      line[i] = '\0';
+      if (argc < CLI_MAX_ARGS) {
+        argv[argc++] = &line[i+1];
+      }
+    }
+  }
+  return cliExecCommand(argv);
+}
+
+void cliTask(void * pdata)
+{
+  char line[256];
+  uint8_t pos = 0;
+
+  cliPrompt();
+
+  for (;;) {
+    uint8_t c;
+
+    while (!cliRxFifo.pop(c)) {
+      CoTickDelay(5); // 10ms
+    }
+
+    if (c == 12) {
+      // clear screen
+      serialPrint("\033[2J\033[1;1H");
+      cliPrompt();
+    }
+    else if (c == 127) {
+      // backspace
+      if (pos) {
+	line[--pos] = '\0';
+	serialPutc(c);
+      }
+    }
+    else if (c == '\r' || c == '\n') {
+      // enter
+      serialCrlf();
+      line[pos] = '\0';
+      cliExecLine(line);
+      pos = 0;
+      cliPrompt();
+    }
+    else if (isascii(c)) {
+      line[pos++] = c;
+      serialPutc(c);
+    }
+  }
+}
+
+void cliStart()
+{
+  cliTaskId = CoCreateTaskEx(cliTask, NULL, 10, &cliStack[CLI_STACK_SIZE-1], CLI_STACK_SIZE, 1, false);
+}
diff --git a/radio/src/cli.h b/radio/src/cli.h
new file mode 100644
index 000000000..d57f205bf
--- /dev/null
+++ b/radio/src/cli.h
@@ -0,0 +1,51 @@
+/*
+ * Authors (alphabetical order)
+ * - Andre Bernet <bernet.andre@gmail.com>
+ * - Andreas Weitl
+ * - Bertrand Songis <bsongis@gmail.com>
+ * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
+ * - Cameron Weeks <th9xer@gmail.com>
+ * - Erez Raviv
+ * - Gabriel Birkus
+ * - Jean-Pierre Parisy
+ * - Karl Szmutny
+ * - Michael Blandford
+ * - Michal Hlavinka
+ * - Pat Mackenzie
+ * - Philip Moss
+ * - Rob Thomson
+ * - Romolo Manfredini <romolo.manfredini@gmail.com>
+ * - Thomas Husterer
+ *
+ * opentx is based on code named
+ * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
+ * er9x by Erez Raviv: http://code.google.com/p/er9x/,
+ * and the original (and ongoing) project by
+ * Thomas Husterer, th9x: http://code.google.com/p/th9x/
+ *
+ * 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 _CLI_H_
+#define _CLI_H_
+
+#include "serial.h"
+
+extern uint8_t cliTracesEnabled;
+
+#ifdef __cplusplus
+#include "fifo.h"
+extern Fifo<256> cliRxFifo;
+#endif
+
+void cliStart();
+
+#endif // _CLI_H_
diff --git a/radio/src/debug.cpp b/radio/src/debug.cpp
index 468840cba..112e20330 100644
--- a/radio/src/debug.cpp
+++ b/radio/src/debug.cpp
@@ -34,222 +34,30 @@
  *
  */
 
-extern "C" {
-#include <stdio.h>
-#include <stdarg.h>
-}
-
 #include "opentx.h"
 
-#if (defined(DEBUG) && defined(CPUARM)) || defined(SIMU)
-
 #if defined(SIMU)
 traceCallbackFunc traceCallback = 0;
 #endif
 
 #if defined(SIMU)
-  #define PRINTF_BUFFER_SIZE    1024
-#else
-  #define PRINTF_BUFFER_SIZE    256
-#endif
-
-// Outputs a string to the UART
-void debugPuts(const char *format, ...)
+void debugPrintf(const char * format, ...)
 {
   va_list arglist;
-  char tmp[PRINTF_BUFFER_SIZE];
-
+  char * str;
   va_start(arglist, format);
-  vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
+  vasprintf(&str, format, arglist);
   va_end(arglist);
-
-#if defined(SIMU)
-  fputs(tmp, stdout);
+  fputs(str, stdout);
   fflush(stdout);
   if (traceCallback) {
-    traceCallback(tmp);
-  }
-#else
-  const char *t = tmp;
-  while (*t) {
-    debugPutc(*t++);
+    traceCallback(str);
   }
+  free(str);
+}
 #endif
-}
-
-void dump(unsigned char *data, unsigned int size)
-{
-  debugPuts("DUMP %d bytes ...\n\r", size);
-  unsigned int i = 0, j=0;
-  while (i*32+j < size) {
-    debugPuts("%.2X ", data[i*32+j]);
-    j++;
-    if (j==32) {
-      i++; j=0;
-      debugPuts("\n\r");
-    }
-  }
-  debugPuts("\n\r");
-}
-
-
-#if !defined(SIMU)
-
-uint32_t Mem_address ;
-uint32_t Next_mem_address ;
-uint32_t Memaddmode ;
-Fifo<512> debugRxFifo;
-
-void crlf()
-{
-  debugPutc( 13 ) ;
-  debugPutc( 10 ) ;
-}
-
-// Send a single 4 bit value to the RS232 port as a hex digit
-void hex_digit_send( unsigned char c )
-{
-  c &= 0x0F ;
-  if ( c > 9 )
-  {
-    c += 7 ;
-  }
-  c += '0' ;
-  debugPutc( c ) ;
-}
-
-// Send the 8 bit value to the RS232 port as 2 hex digits
-void p2hex( unsigned char c )
-{
-//  asm("swap %c") ;
-  hex_digit_send( c >> 4 ) ;
-//  asm("swap %c") ;
-  hex_digit_send( c ) ;
-}
-
-// Send the 16 bit value to the RS232 port as 4 hex digits
-void p4hex( uint16_t value )
-{
-  p2hex( value >> 8 ) ;
-  p2hex( value ) ;
-}
-
-// Send the 32 bit value to the RS232 port as 8 hex digits
-void p8hex( uint32_t value )
-{
-  p4hex( value >> 16 ) ;
-  p4hex( value ) ;
-}
-
-
-static void dispw_256( register uint32_t address, register uint32_t lines )
-{
-  register uint32_t i ;
-  register uint32_t j ;
-  address &= 0xFFFFFFFC ;
-  for ( i = 0 ; i < lines ; i += 1 )
-  {
-    p8hex( address ) ;
-    for ( j = 0 ; j < 4 ; j += 1 )
-    {
-      debugPutc(' ') ;
-      p8hex( *( (uint32_t *)address ) ) ;
-      address += 4 ;
-    }
-    crlf() ;
-  }
-}
-
-extern Fifo<512> uart3TxFifo;
-
-void debugFlush() 
-{
-  uart3TxFifo.flush();
-}
-
-void debugTask(void* pdata)
-{
-  uint8_t rxchar ;
-
-  TRACE("DEBUG Task started");
-
-  crlf() ;
-  dispw_256( (uint32_t)USART3, 4 ) ;
-
-  for (;;) {
-		
-    while ( (USART3->SR & USART_SR_RXNE) == 0 )
-      CoTickDelay(5); // 10ms
-		
-    rxchar = USART3->DR;
-
-    if ( Memaddmode )
-    {
-      if ( ( rxchar >= 'a' ) && ( rxchar <= 'f' ) )
-      {
-        rxchar -= 0x20;		// toupper!
-      }
-      if ( ( ( rxchar >= '0' ) && ( rxchar <= '9' ) ) || ( ( rxchar >= 'A' ) && ( rxchar <= 'F' ) ) )
-      {
-        debugPutc( rxchar );
-        rxchar -= '0';
-        if ( rxchar > 9 )
-        {
-          rxchar -= 7;
-        }
-        Mem_address <<= 4;
-        Mem_address |= rxchar;
-      }
-      else if ( rxchar == 13 )
-      {
-        crlf();
-        if ( Mem_address == 0 )
-        {
-          Mem_address = Next_mem_address;
-        }
-        dispw_256( Mem_address, 4 );
-        Next_mem_address = Mem_address + 64;
-        Memaddmode = 0;
-      }
-      else if ( rxchar == 8 )
-      {
-        debugPutc( rxchar );
-        debugPutc( rxchar );
-        debugPutc( rxchar );
-        Mem_address >>= 4;
-      }
-      else if ( rxchar == 27 )
-      {
-        crlf();
-        Memaddmode = 0;
-      }
-
-    }
-
-    if ( rxchar == '?' )
-    {
-      Memaddmode = 1;
-      Mem_address = 0;
-      debugPutc( '>' );
-    }
-
-    if ( rxchar == 'm' )
-    {
-      crlf();
-      p8hex( (uint32_t) &g_model.moduleData[0] );
-      debugPutc( ' ' );
-      p8hex( (uint32_t) &g_model.moduleData[1] );
-      crlf();
-    }
-
-  }
-}
-#endif  // #if !defined(SIMU)
-#endif  // #if (defined(DEBUG) && defined(CPUARM)) || defined(SIMU)
-
 
 #if defined(DEBUG_TRACE_BUFFER)
-
 static struct TraceElement traceBuffer[TRACE_BUFFER_LEN];
 static uint8_t traceBufferPos;
 extern Fifo<512> uart3TxFifo;
@@ -305,6 +113,4 @@ void dumpTraceBuffer()
   }
   TRACE("End of Trace Buffer dump");
 }
-
 #endif
-
diff --git a/radio/src/debug.h b/radio/src/debug.h
index 998b5550e..3ec150d12 100644
--- a/radio/src/debug.h
+++ b/radio/src/debug.h
@@ -39,58 +39,32 @@
 
 #include <inttypes.h>
 #include "rtc.h"
-
-#if (defined(DEBUG) && defined(CPUARM)) || defined(SIMU)
-
-#ifdef __cplusplus
-extern "C" {
-#endif
+#include "dump.h"
+#include "cli.h"
 
 #if defined(SIMU)
 typedef void (*traceCallbackFunc)(const char * text);
 extern traceCallbackFunc traceCallback;
+void debugPrintf(const char * format, ...);
+#elif defined(DEBUG) && defined(CLI) && defined(USB_SERIAL)
+#define debugPrintf(...) do { if (cliTracesEnabled) serialPrintf(__VA_ARGS__); } while(0)
+#elif defined(DEBUG) && defined(CLI)
+#define debugPrintf(...) do { if (serialTracesEnabled() && cliTracesEnabled) serialPrintf(__VA_ARGS__); } while(0)
+#elif defined(DEBUG) && defined(CPUARM)
+#define debugPrintf(...) do { if (serialTracesEnabled()) serialPrintf(__VA_ARGS__); } while(0)
+#else
+#define debugPrintf(...)
 #endif
 
-void debugPuts(const char *string, ...);
-void dump(unsigned char *data, unsigned int size);
-void debugFlush();
-
-#ifdef __cplusplus
-}
-#endif
-
-#define TRACE(...)            do { debugPuts(__VA_ARGS__); debugPuts("\r\n"); } while(0)
+#define TRACE(...)            do { debugPrintf(__VA_ARGS__); debugPrintf("\r\n"); } while(0)
 #define DUMP(data, size)      dump(data, size)
-#define TRACE_DEBUG(...)      debugPuts("-D- " __VA_ARGS__)
-#define TRACE_DEBUG_WP(...)   debugPuts(__VA_ARGS__)
-#define TRACE_INFO(...)       debugPuts("-I- " __VA_ARGS__)
-#define TRACE_INFO_WP(...)    debugPuts(__VA_ARGS__)
-#define TRACE_WARNING(...)    debugPuts("-W- " __VA_ARGS__)
-#define TRACE_WARNING_WP(...) debugPuts(__VA_ARGS__)
-#define TRACE_ERROR(...)      debugPuts("-E- " __VA_ARGS__)
-
-#if !defined(SIMU)
-#define FLUSH()               debugFlush();
-void debugTask(void* pdata);
-#else
-#define FLUSH()
-#endif
-
-#else
-
-#define TRACE(...) { }
-#define DUMP(...) { }
-#define TRACE_DEBUG(...) { }
-#define TRACE_DEBUG_WP(...) { }
-#define TRACE_INFO(...) { }
-#define TRACE_INFO_WP(...) { }
-#define TRACE_WARNING(...) { }
-#define TRACE_WARNING_WP(...) { }
-#define TRACE_ERROR(...) { }
-#define FLUSH()
-
-#endif
-
+#define TRACE_DEBUG(...)      debugPrintf("-D- " __VA_ARGS__)
+#define TRACE_DEBUG_WP(...)   debugPrintf(__VA_ARGS__)
+#define TRACE_INFO(...)       debugPrintf("-I- " __VA_ARGS__)
+#define TRACE_INFO_WP(...)    debugPrintf(__VA_ARGS__)
+#define TRACE_WARNING(...)    debugPrintf("-W- " __VA_ARGS__)
+#define TRACE_WARNING_WP(...) debugPrintf(__VA_ARGS__)
+#define TRACE_ERROR(...)      debugPrintf("-E- " __VA_ARGS__)
 
 #if defined(DEBUG_TRACE_BUFFER)
 
diff --git a/radio/src/dump.cpp b/radio/src/dump.cpp
new file mode 100644
index 000000000..25bf29ab3
--- /dev/null
+++ b/radio/src/dump.cpp
@@ -0,0 +1,38 @@
+#include "opentx.h"
+
+#if defined(SIMU)
+#define dumpPrintf(...) debugPrintf(__VA_ARGS__)
+#else
+#define dumpPrintf(...) serialPrintf(__VA_ARGS__)
+#endif
+
+uint8_t dumpPosition;
+
+void dumpStart(unsigned int size)
+{
+  dumpPrintf("DUMP %d bytes ...\n\r", size);
+  dumpPosition = 0;
+}
+
+void dumpBody(const uint8_t *data, unsigned int size)
+{
+  for (unsigned int i=0; i<size; i++) {
+    dumpPrintf("%.2X ", data[i]);
+    dumpPosition++;
+    if ((dumpPosition & (32-1)) == 0) {
+      dumpPrintf("\r\n");
+    }
+  }
+}
+
+void dumpEnd()
+{
+  dumpPrintf("\r\n");
+}
+
+void dump(const uint8_t *data, unsigned int size)
+{
+  dumpStart(size);
+  dumpBody(data, size);
+  dumpEnd();
+}
diff --git a/radio/src/dump.h b/radio/src/dump.h
new file mode 100644
index 000000000..a0ed81468
--- /dev/null
+++ b/radio/src/dump.h
@@ -0,0 +1,13 @@
+#ifndef _DUMP_H_
+#define _DUMP_H_
+
+#if defined(DEBUG) || defined(CLI)
+void dumpStart(unsigned int size);
+void dumpBody(const uint8_t * data, unsigned int size);
+void dumpEnd();
+void dump(const uint8_t * data, unsigned int size);
+#else
+#define dump(data, size)
+#endif
+
+#endif // _DUMP_H_
diff --git a/radio/src/gui/9X/view_mavlink.h b/radio/src/gui/9X/view_mavlink.h
index 993bca2b3..9032124f2 100755
--- a/radio/src/gui/9X/view_mavlink.h
+++ b/radio/src/gui/9X/view_mavlink.h
@@ -46,7 +46,7 @@
 #include "opentx.h"
 #include "telemetry/mavlink.h"
 #include "gui/9X/menus.h"
-#include "serial.h"
+#include "targets/common_avr/serial_driver.h"
 
 #define APSIZE (BSS | DBLSIZE)
 
diff --git a/radio/src/gui/Taranis/menu_general_hardware.cpp b/radio/src/gui/Taranis/menu_general_hardware.cpp
index df8e99e82..f1b9deee2 100755
--- a/radio/src/gui/Taranis/menu_general_hardware.cpp
+++ b/radio/src/gui/Taranis/menu_general_hardware.cpp
@@ -219,9 +219,9 @@ void menuGeneralHardware(uint8_t event)
         break;
 #endif
       case ITEM_SETUP_HW_UART3_MODE:
-        g_eeGeneral.uart3Mode = selectMenuItem(HW_SETTINGS_COLUMN, y, STR_UART3MODE, STR_UART3MODES, g_eeGeneral.uart3Mode, 0, UART_MODE_MAX, attr, event);
+        g_eeGeneral.serial2Mode = selectMenuItem(HW_SETTINGS_COLUMN, y, STR_UART3MODE, STR_UART3MODES, g_eeGeneral.serial2Mode, 0, UART_MODE_MAX, attr, event);
         if (attr && checkIncDec_Ret) {
-          uart3Init(g_eeGeneral.uart3Mode, MODEL_TELEMETRY_PROTOCOL());
+          serial2Init(g_eeGeneral.serial2Mode, MODEL_TELEMETRY_PROTOCOL());
         }
         break;
     }
diff --git a/radio/src/gui/Taranis/menu_model_telemetry.cpp b/radio/src/gui/Taranis/menu_model_telemetry.cpp
index 62b21c4e5..0ec2cab8d 100644
--- a/radio/src/gui/Taranis/menu_model_telemetry.cpp
+++ b/radio/src/gui/Taranis/menu_model_telemetry.cpp
@@ -574,7 +574,7 @@ void menuModelTelemetry(uint8_t event)
 
     switch (k) {
       case ITEM_TELEMETRY_PROTOCOL_TYPE:
-        g_model.telemetryProtocol = selectMenuItem(TELEM_COL2, y, STR_TELEMETRY_TYPE, "\017FrSky S.PORT\0  FrSky D\0       FrSky D (cable)", g_model.telemetryProtocol, PROTOCOL_TELEMETRY_FIRST, g_eeGeneral.uart3Mode==UART_MODE_TELEMETRY ? PROTOCOL_FRSKY_D_SECONDARY : PROTOCOL_FRSKY_D, attr, event);
+        g_model.telemetryProtocol = selectMenuItem(TELEM_COL2, y, STR_TELEMETRY_TYPE, "\017FrSky S.PORT\0  FrSky D\0       FrSky D (cable)", g_model.telemetryProtocol, PROTOCOL_TELEMETRY_FIRST, g_eeGeneral.serial2Mode==UART_MODE_TELEMETRY ? PROTOCOL_FRSKY_D_SECONDARY : PROTOCOL_FRSKY_D, attr, event);
         break;
 
       case ITEM_TELEMETRY_SENSORS_LABEL:
diff --git a/radio/src/myeeprom.h b/radio/src/myeeprom.h
index e09da2eb4..53956b8fd 100644
--- a/radio/src/myeeprom.h
+++ b/radio/src/myeeprom.h
@@ -384,7 +384,7 @@ enum BeeperMode {
   #define LEN_SWITCH_NAME              3
   #define LEN_ANA_NAME                 3
   #define LEN_BLUETOOTH_NAME           10
-  #define HAS_WIRELESS_TRAINER_HARDWARE() (g_eeGeneral.uart3Mode==UART_MODE_SBUS_TRAINER/* || g_eeGeneral.uart3Mode==UART_MODE_CPPM_TRAINER*/)
+  #define HAS_WIRELESS_TRAINER_HARDWARE() (g_eeGeneral.serial2Mode==UART_MODE_SBUS_TRAINER/* || g_eeGeneral.serial2Mode==UART_MODE_CPPM_TRAINER*/)
 
   #if defined(REV9E)
     #define BLUETOOTH_FIELDS \
@@ -396,7 +396,7 @@ enum BeeperMode {
 
   #define EXTRA_GENERAL_FIELDS \
     EXTRA_GENERAL_FIELDS_ARM \
-    uint8_t  uart3Mode:6; \
+    uint8_t  serial2Mode:6; \
     uint8_t  slidersConfig:2; \
     uint8_t  potsConfig; /*two bits for every pot*/\
     uint8_t  backlightColor; \
diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp
index 3ba3a085c..f1bd91be2 100644
--- a/radio/src/opentx.cpp
+++ b/radio/src/opentx.cpp
@@ -2456,7 +2456,7 @@ void opentxInit(OPENTX_INIT_ARGS)
   backlightOn();
 
 #if defined(PCBTARANIS)
-  uart3Init(g_eeGeneral.uart3Mode, MODEL_TELEMETRY_PROTOCOL());
+  serial2Init(g_eeGeneral.serial2Mode, MODEL_TELEMETRY_PROTOCOL());
 #endif
 
 #if defined(PCBSKY9X) && !defined(SIMU)
diff --git a/radio/src/opentx.h b/radio/src/opentx.h
index 02724d937..4b6c424ff 100644
--- a/radio/src/opentx.h
+++ b/radio/src/opentx.h
@@ -1759,10 +1759,4 @@ extern Clipboard clipboard;
 extern uint16_t s_anaFilt[NUMBER_ANALOG];
 #endif
 
-#if defined(USB_SERIAL)
-extern Fifo<64> cliRxFifo;
-void sendUsbSerialChar(uint8_t c);
-#endif
-
-
 #endif
diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp
index c90ebefe0..c4de11d17 100644
--- a/radio/src/serial.cpp
+++ b/radio/src/serial.cpp
@@ -1,297 +1,75 @@
 /*
- * Authors - Gerard Valade <gerard.valade@gmail.com>
+ * Authors (alphabetical order)
+ * - Andre Bernet <bernet.andre@gmail.com>
+ * - Andreas Weitl
+ * - Bertrand Songis <bsongis@gmail.com>
+ * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
+ * - Cameron Weeks <th9xer@gmail.com>
+ * - Erez Raviv
+ * - Gabriel Birkus
+ * - Jean-Pierre Parisy
+ * - Karl Szmutny
+ * - Michael Blandford
+ * - Michal Hlavinka
+ * - Pat Mackenzie
+ * - Philip Moss
+ * - Rob Thomson
+ * - Romolo Manfredini <romolo.manfredini@gmail.com>
+ * - Thomas Husterer
  *
- * Adapted from frsky
+ * opentx is based on code named
+ * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
+ * er9x by Erez Raviv: http://code.google.com/p/er9x/,
+ * and the original (and ongoing) project by
+ * Thomas Husterer, th9x: http://code.google.com/p/th9x/
  *
- * 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 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.
+ * 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"
 #include "serial.h"
 
-/*
- Receive serial (RS-232) characters, detecting and storing each Fr-Sky
- 0x7e-framed packet as it arrives.  When a complete packet has been
- received, process its data into storage variables.  NOTE: This is an
- interrupt routine and should not get too lengthy. I originally had
- the buffer being checked in the perMain function (because per10ms
- isn't quite often enough for data streaming at 9600baud) but alas
- that scheme lost packets also. So each packet is parsed as it arrives,
- directly at the ISR function (through a call to frskyProcessPacket).
+extern "C" {
+  #include <stdio.h>
+  #include <stdarg.h>
+}
 
- If this proves a problem in the future, then I'll just have to implement
- a second buffer to receive data while one buffer is being processed (slowly).
- */
+#define PRINTF_BUFFER_SIZE    256
 
-#ifndef SIMU
-ISR (USART0_RX_vect)
+void serialPutc(char c)
 {
-	uint8_t iostat; //USART control and Status Register 0 A
-	// uint8_t rh; //USART control and Status Register 0 B
-	UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
-	sei();
-	iostat = UCSR0A; //USART control and Status Register 0 A
-	uint8_t byte = UDR0;
-
-	/*
-	 bit 	7		6		5		4	3		2		1		0
-	 RxC0	TxC0	UDRE0	FE0	DOR0	UPE0	U2X0	MPCM0
-
-	 RxC0: 	Receive complete
-	 TXC0: 	Transmit Complete
-	 UDRE0: 	USART Data Register Empty
-	 FE0:	Frame Error
-	 DOR0:	Data OverRun
-	 UPE0:	USART Parity Error
-	 U2X0:	Double Tx Speed
-	 MPCM0:	MultiProcessor Comms Mode
-	 */
-	if (iostat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0))) {
-		byte = 0;
-	}
-	//rh = UCSR0B; //USART control and Status Register 0 B
-#ifdef MAVLINK
-	(RXHandler)(byte);
+#if defined(USB_SERIAL)
+  sendUsbSerialChar(c);
+#else
+  serial2Putc(c);
 #endif
-
-	cli();
-	UCSR0B |= (1 << RXCIE0); // enable Interrupt
-
-}
-#endif
-
-inline void SERIAL_EnableRXD(void) {
-	UCSR0B |= (1 << RXEN0); // enable RX
-	UCSR0B |= (1 << RXCIE0); // enable Interrupt
 }
 
-#if 0
-void SERIAL_DisableRXD(void) {
-	UCSR0B &= ~(1 << RXEN0); // disable RX
-	UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
-}
-#endif
-
-/*
- USART0 (transmit) Data Register Emtpy ISR
- Usef to transmit FrSky data packets, which are buffered in frskyTXBuffer.
- */
-uint8_t serialTxBuffer[MAX_TX_BUFFER]; // 32 characters
-uint8_t serialTxBufferCount = 0;
-uint8_t * ptrTxISR = 0;
-serial_tx_state_t serialTxState = TX_STATE_EMPTY;
-
-#ifndef SIMU
-ISR(USART0_UDRE_vect)
+void serialPrintf(const char * format, ...)
 {
-	if (serialTxBufferCount > 0) {
-		UDR0 = *ptrTxISR++;
-		serialTxBufferCount--;
-	} else {
-		UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
-		serialTxState = TX_STATE_EMPTY;
-	}
-}
-#endif
+  va_list arglist;
+  char tmp[PRINTF_BUFFER_SIZE];
 
-void SERIAL_start_uart_send() {
-	ptrTxISR = serialTxBuffer;
-	serialTxBufferCount = 0;
+  va_start(arglist, format);
+  vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
+  va_end(arglist);
+
+  const char *t = tmp;
+  while (*t) {
+    serialPutc(*t++);
+  }
 }
 
-void SERIAL_end_uart_send() {
-	ptrTxISR = serialTxBuffer;
-	//UCSR0B |= (1 << UDRIE0); // enable  UDRE0 interrupt
-	serialTxState = TX_STATE_READY;
+void serialCrlf()
+{
+  serialPutc('\r');
+  serialPutc('\n');
 }
-
-void SERIAL_send_uart_bytes(const uint8_t * buf, uint16_t len) {
-	while (len--) {
-		*ptrTxISR++ = *buf++;
-		serialTxBufferCount++;
-	}
-
-}
-
-#if 0
-void SERIAL_transmitBuffer(uint8_t len) {
-	serialTxBufferCount = len;
-	ptrTxISR = serialTxBuffer;
-	//UCSR0B |= (1 << UDRIE0); // enable  UDRE0 interrupt
-	serialTxState = TX_STATE_READY;
-}
-#endif
-
-void SERIAL_startTX(void) {
-	if (serialTxState == TX_STATE_READY) {
-		serialTxState = TX_STATE_BUSY;
-		UCSR0B |= (1 << UDRIE0); // enable  UDRE0 interrupt
-	}
-}
-
-static void uart_4800(void) {
-#ifndef SIMU
-#undef BAUD  // avoid compiler warning
-#define BAUD 4800
-#include <util/setbaud.h>
-  UBRR0H = UBRRH_VALUE;
-  UBRR0L = UBRRL_VALUE;
-#if USE_2X
-  UCSR0A |= (1 << U2X0);
-#else
-	UCSR0A &= ~(1 << U2X0);
-#endif
-#endif
-}
-
-static void uart_9600(void) {
-#ifndef SIMU
-#undef BAUD  // avoid compiler warning
-#define BAUD 9600
-#include <util/setbaud.h>
-  UBRR0H = UBRRH_VALUE;
-  UBRR0L = UBRRL_VALUE;
-#if USE_2X
-  UCSR0A |= (1 << U2X0);
-#else
-	UCSR0A &= ~(1 << U2X0);
-#endif
-#endif
-}
-
-static void uart_14400(void) {
-#ifndef SIMU
-#undef BAUD  // avoid compiler warning
-#define BAUD 14400
-#include <util/setbaud.h>
-  UBRR0H = UBRRH_VALUE;
-  UBRR0L = UBRRL_VALUE;
-#if USE_2X
-  UCSR0A |= (1 << U2X0);
-#else
-	UCSR0A &= ~(1 << U2X0);
-#endif
-#endif
-}
-
-static void uart_19200(void) {
-#ifndef SIMU
-#undef BAUD  // avoid compiler warning
-#define BAUD 19200
-#include <util/setbaud.h>
-  UBRR0H = UBRRH_VALUE;
-  UBRR0L = UBRRL_VALUE;
-#if USE_2X
-  UCSR0A |= (1 << U2X0);
-#else
-	UCSR0A &= ~(1 << U2X0);
-#endif
-#endif
-}
-
-static void uart_38400(void) {
-#ifndef SIMU
-#undef BAUD  // avoid compiler warning
-#define BAUD 38400
-#include <util/setbaud.h>
-  UBRR0H = UBRRH_VALUE;
-  UBRR0L = UBRRL_VALUE;
-#if USE_2X
-  UCSR0A |= (1 << U2X0);
-#else
-  UCSR0A &= ~(1 << U2X0);
-#endif
-#endif
-}
-
-static void uart_57600(void) {
-#ifndef SIMU
-#undef BAUD  // avoid compiler warning
-#define BAUD 57600
-#include <util/setbaud.h>
-  UBRR0H = UBRRH_VALUE;
-  UBRR0L = UBRRL_VALUE;
-#if USE_2X
-  UCSR0A |= (1 << U2X0);
-#else
-	UCSR0A &= ~(1 << U2X0);
-#endif
-#endif
-}
-
-
-static void uart_76800(void) {
-#ifndef SIMU
-#undef BAUD  // avoid compiler warning
-#define BAUD 76800
-#include <util/setbaud.h>
-  UBRR0H = UBRRH_VALUE;
-  UBRR0L = UBRRL_VALUE;
-#if USE_2X
-  UCSR0A |= (1 << U2X0);
-#else
-	UCSR0A &= ~(1 << U2X0);
-#endif
-#endif
-}
-
-inline void SERIAL_EnableTXD(void) {
-	//UCSR0B |= (1 << TXEN0); // enable TX
-	UCSR0B |= (1 << TXEN0) | (1 << UDRIE0); // enable TX and TX interrupt
-}
-
-#if 0
-void SERIAL_DisableTXD(void) {
-	UCSR0B &= ~(1 << TXEN0); // disable TX
-	UCSR0B &= ~(1 << UDRIE0); // disable Interrupt
-}
-#endif
-
-void SERIAL_Init(void) {
-	DDRE &= ~(1 << DDE0); // set RXD0 pin as input
-	PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
-
-	switch (g_eeGeneral.mavbaud) {
-	case BAUD_4800:
-		uart_4800();
-		break;
-	case BAUD_9600:
-		uart_9600();
-		break;
-	case BAUD_14400:
-		uart_14400();
-		break;
-	case BAUD_19200:
-		uart_19200();
-		break;
-	case BAUD_38400:
-		uart_38400();
-		break;
-	case BAUD_57600:
-		uart_57600();
-		break;
-	case BAUD_76800:
-		uart_76800();
-		break;
-	}
-
-
-	// UCSR0A &= ~(1 << U2X0); // disable double speed operation
-	// set 8N1
-	UCSR0B = 0 | (0 << RXCIE0) | (0 << TXCIE0) | (0 << UDRIE0) | (0 << RXEN0) | (0 << TXEN0) | (0 << UCSZ02);
-	UCSR0C = 0 | (1 << UCSZ01) | (1 << UCSZ00);
-	while (UCSR0A & (1 << RXC0))
-	  UDR0; // flush receive buffer
-
-	SERIAL_EnableTXD();
-	SERIAL_EnableRXD();
-}
-
diff --git a/radio/src/serial.h b/radio/src/serial.h
index a3089d691..9d887512b 100644
--- a/radio/src/serial.h
+++ b/radio/src/serial.h
@@ -1,59 +1,55 @@
-/*
- * Authors - Gerard Valade <gerard.valade@gmail.com>
- *
- * Adapted from frsky
- *
- * 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 _SERIAL_H_
-#define _SERIAL_H_
-
-typedef enum serial_tx_state_ {
-	TX_STATE_EMPTY = 0, //
-	TX_STATE_READY, //
-	TX_STATE_BUSY
-} serial_tx_state_t;
-extern serial_tx_state_t serialTxState;
-
-//! \brief Baudrate selection.
-enum SERIAL_BAUDS {
-	BAUD_4800 = 0,
-	BAUD_9600,
-	BAUD_14400,
-	BAUD_19200,
-	BAUD_38400,
-	BAUD_57600,
-	BAUD_76800
-};
-
-//! \brief Definition of baudrate settings item choices.
-typedef void (*SerialFuncP)(uint8_t event);
-
-extern SerialFuncP RXHandler;
-
-#define MAX_TX_BUFFER 32
-//#if 0
-extern uint8_t serialTxBuffer[MAX_TX_BUFFER]; // 32 characters
-extern uint8_t serialTxBufferCount;
-extern uint8_t * ptrTxISR;
-//#endif
-
-void SERIAL_Init(void);
-//void SERIAL_transmitBuffer(uint8_t len);
-extern void SERIAL_start_uart_send();
-extern void SERIAL_end_uart_send();
-extern void SERIAL_send_uart_bytes(uint8_t * buf, uint16_t len);
-
-void SERIAL_startTX(void);
-#define IS_TX_BUSY (serialTxState!=TX_STATE_EMPTY)
-
-#endif
+/*
+ * Authors (alphabetical order)
+ * - Andre Bernet <bernet.andre@gmail.com>
+ * - Andreas Weitl
+ * - Bertrand Songis <bsongis@gmail.com>
+ * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
+ * - Cameron Weeks <th9xer@gmail.com>
+ * - Erez Raviv
+ * - Gabriel Birkus
+ * - Jean-Pierre Parisy
+ * - Karl Szmutny
+ * - Michael Blandford
+ * - Michal Hlavinka
+ * - Pat Mackenzie
+ * - Philip Moss
+ * - Rob Thomson
+ * - Romolo Manfredini <romolo.manfredini@gmail.com>
+ * - Thomas Husterer
+ *
+ * opentx is based on code named
+ * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
+ * er9x by Erez Raviv: http://code.google.com/p/er9x/,
+ * and the original (and ongoing) project by
+ * Thomas Husterer, th9x: http://code.google.com/p/th9x/
+ *
+ * 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 _SERIAL_H_
+#define _SERIAL_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void serialPutc(char c);
+void serialPrintf(const char *format, ...);
+void serialCrlf();
+
+#ifdef __cplusplus
+}
+#endif
+
+#define serialPrint(...) do { serialPrintf(__VA_ARGS__); serialCrlf(); } while(0)
+
+#endif // _SERIAL_H_
+
diff --git a/radio/src/targets/common_avr/serial_driver.cpp b/radio/src/targets/common_avr/serial_driver.cpp
new file mode 100644
index 000000000..e46a48f3d
--- /dev/null
+++ b/radio/src/targets/common_avr/serial_driver.cpp
@@ -0,0 +1,297 @@
+/*
+ * Authors - Gerard Valade <gerard.valade@gmail.com>
+ *
+ * Adapted from frsky
+ *
+ * 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 "serial_driver.h"
+#include "opentx.h"
+
+/*
+ Receive serial (RS-232) characters, detecting and storing each Fr-Sky
+ 0x7e-framed packet as it arrives.  When a complete packet has been
+ received, process its data into storage variables.  NOTE: This is an
+ interrupt routine and should not get too lengthy. I originally had
+ the buffer being checked in the perMain function (because per10ms
+ isn't quite often enough for data streaming at 9600baud) but alas
+ that scheme lost packets also. So each packet is parsed as it arrives,
+ directly at the ISR function (through a call to frskyProcessPacket).
+
+ If this proves a problem in the future, then I'll just have to implement
+ a second buffer to receive data while one buffer is being processed (slowly).
+ */
+
+#ifndef SIMU
+ISR (USART0_RX_vect)
+{
+	uint8_t iostat; //USART control and Status Register 0 A
+	// uint8_t rh; //USART control and Status Register 0 B
+	UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
+	sei();
+	iostat = UCSR0A; //USART control and Status Register 0 A
+	uint8_t byte = UDR0;
+
+	/*
+	 bit 	7		6		5		4	3		2		1		0
+	 RxC0	TxC0	UDRE0	FE0	DOR0	UPE0	U2X0	MPCM0
+
+	 RxC0: 	Receive complete
+	 TXC0: 	Transmit Complete
+	 UDRE0: 	USART Data Register Empty
+	 FE0:	Frame Error
+	 DOR0:	Data OverRun
+	 UPE0:	USART Parity Error
+	 U2X0:	Double Tx Speed
+	 MPCM0:	MultiProcessor Comms Mode
+	 */
+	if (iostat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0))) {
+		byte = 0;
+	}
+	//rh = UCSR0B; //USART control and Status Register 0 B
+#ifdef MAVLINK
+	(RXHandler)(byte);
+#endif
+
+	cli();
+	UCSR0B |= (1 << RXCIE0); // enable Interrupt
+
+}
+#endif
+
+inline void SERIAL_EnableRXD(void) {
+	UCSR0B |= (1 << RXEN0); // enable RX
+	UCSR0B |= (1 << RXCIE0); // enable Interrupt
+}
+
+#if 0
+void SERIAL_DisableRXD(void) {
+	UCSR0B &= ~(1 << RXEN0); // disable RX
+	UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
+}
+#endif
+
+/*
+ USART0 (transmit) Data Register Emtpy ISR
+ Usef to transmit FrSky data packets, which are buffered in frskyTXBuffer.
+ */
+uint8_t serialTxBuffer[MAX_TX_BUFFER]; // 32 characters
+uint8_t serialTxBufferCount = 0;
+uint8_t * ptrTxISR = 0;
+serial_tx_state_t serialTxState = TX_STATE_EMPTY;
+
+#ifndef SIMU
+ISR(USART0_UDRE_vect)
+{
+	if (serialTxBufferCount > 0) {
+		UDR0 = *ptrTxISR++;
+		serialTxBufferCount--;
+	} else {
+		UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
+		serialTxState = TX_STATE_EMPTY;
+	}
+}
+#endif
+
+void SERIAL_start_uart_send() {
+	ptrTxISR = serialTxBuffer;
+	serialTxBufferCount = 0;
+}
+
+void SERIAL_end_uart_send() {
+	ptrTxISR = serialTxBuffer;
+	//UCSR0B |= (1 << UDRIE0); // enable  UDRE0 interrupt
+	serialTxState = TX_STATE_READY;
+}
+
+void SERIAL_send_uart_bytes(const uint8_t * buf, uint16_t len) {
+	while (len--) {
+		*ptrTxISR++ = *buf++;
+		serialTxBufferCount++;
+	}
+
+}
+
+#if 0
+void SERIAL_transmitBuffer(uint8_t len) {
+	serialTxBufferCount = len;
+	ptrTxISR = serialTxBuffer;
+	//UCSR0B |= (1 << UDRIE0); // enable  UDRE0 interrupt
+	serialTxState = TX_STATE_READY;
+}
+#endif
+
+void SERIAL_startTX(void) {
+	if (serialTxState == TX_STATE_READY) {
+		serialTxState = TX_STATE_BUSY;
+		UCSR0B |= (1 << UDRIE0); // enable  UDRE0 interrupt
+	}
+}
+
+static void uart_4800(void) {
+#ifndef SIMU
+#undef BAUD  // avoid compiler warning
+#define BAUD 4800
+#include <util/setbaud.h>
+  UBRR0H = UBRRH_VALUE;
+  UBRR0L = UBRRL_VALUE;
+#if USE_2X
+  UCSR0A |= (1 << U2X0);
+#else
+	UCSR0A &= ~(1 << U2X0);
+#endif
+#endif
+}
+
+static void uart_9600(void) {
+#ifndef SIMU
+#undef BAUD  // avoid compiler warning
+#define BAUD 9600
+#include <util/setbaud.h>
+  UBRR0H = UBRRH_VALUE;
+  UBRR0L = UBRRL_VALUE;
+#if USE_2X
+  UCSR0A |= (1 << U2X0);
+#else
+	UCSR0A &= ~(1 << U2X0);
+#endif
+#endif
+}
+
+static void uart_14400(void) {
+#ifndef SIMU
+#undef BAUD  // avoid compiler warning
+#define BAUD 14400
+#include <util/setbaud.h>
+  UBRR0H = UBRRH_VALUE;
+  UBRR0L = UBRRL_VALUE;
+#if USE_2X
+  UCSR0A |= (1 << U2X0);
+#else
+	UCSR0A &= ~(1 << U2X0);
+#endif
+#endif
+}
+
+static void uart_19200(void) {
+#ifndef SIMU
+#undef BAUD  // avoid compiler warning
+#define BAUD 19200
+#include <util/setbaud.h>
+  UBRR0H = UBRRH_VALUE;
+  UBRR0L = UBRRL_VALUE;
+#if USE_2X
+  UCSR0A |= (1 << U2X0);
+#else
+	UCSR0A &= ~(1 << U2X0);
+#endif
+#endif
+}
+
+static void uart_38400(void) {
+#ifndef SIMU
+#undef BAUD  // avoid compiler warning
+#define BAUD 38400
+#include <util/setbaud.h>
+  UBRR0H = UBRRH_VALUE;
+  UBRR0L = UBRRL_VALUE;
+#if USE_2X
+  UCSR0A |= (1 << U2X0);
+#else
+  UCSR0A &= ~(1 << U2X0);
+#endif
+#endif
+}
+
+static void uart_57600(void) {
+#ifndef SIMU
+#undef BAUD  // avoid compiler warning
+#define BAUD 57600
+#include <util/setbaud.h>
+  UBRR0H = UBRRH_VALUE;
+  UBRR0L = UBRRL_VALUE;
+#if USE_2X
+  UCSR0A |= (1 << U2X0);
+#else
+	UCSR0A &= ~(1 << U2X0);
+#endif
+#endif
+}
+
+
+static void uart_76800(void) {
+#ifndef SIMU
+#undef BAUD  // avoid compiler warning
+#define BAUD 76800
+#include <util/setbaud.h>
+  UBRR0H = UBRRH_VALUE;
+  UBRR0L = UBRRL_VALUE;
+#if USE_2X
+  UCSR0A |= (1 << U2X0);
+#else
+	UCSR0A &= ~(1 << U2X0);
+#endif
+#endif
+}
+
+inline void SERIAL_EnableTXD(void) {
+	//UCSR0B |= (1 << TXEN0); // enable TX
+	UCSR0B |= (1 << TXEN0) | (1 << UDRIE0); // enable TX and TX interrupt
+}
+
+#if 0
+void SERIAL_DisableTXD(void) {
+	UCSR0B &= ~(1 << TXEN0); // disable TX
+	UCSR0B &= ~(1 << UDRIE0); // disable Interrupt
+}
+#endif
+
+void SERIAL_Init(void) {
+	DDRE &= ~(1 << DDE0); // set RXD0 pin as input
+	PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
+
+	switch (g_eeGeneral.mavbaud) {
+	case BAUD_4800:
+		uart_4800();
+		break;
+	case BAUD_9600:
+		uart_9600();
+		break;
+	case BAUD_14400:
+		uart_14400();
+		break;
+	case BAUD_19200:
+		uart_19200();
+		break;
+	case BAUD_38400:
+		uart_38400();
+		break;
+	case BAUD_57600:
+		uart_57600();
+		break;
+	case BAUD_76800:
+		uart_76800();
+		break;
+	}
+
+
+	// UCSR0A &= ~(1 << U2X0); // disable double speed operation
+	// set 8N1
+	UCSR0B = 0 | (0 << RXCIE0) | (0 << TXCIE0) | (0 << UDRIE0) | (0 << RXEN0) | (0 << TXEN0) | (0 << UCSZ02);
+	UCSR0C = 0 | (1 << UCSZ01) | (1 << UCSZ00);
+	while (UCSR0A & (1 << RXC0))
+	  UDR0; // flush receive buffer
+
+	SERIAL_EnableTXD();
+	SERIAL_EnableRXD();
+}
+
diff --git a/radio/src/targets/common_avr/serial_driver.h b/radio/src/targets/common_avr/serial_driver.h
new file mode 100644
index 000000000..a3089d691
--- /dev/null
+++ b/radio/src/targets/common_avr/serial_driver.h
@@ -0,0 +1,59 @@
+/*
+ * Authors - Gerard Valade <gerard.valade@gmail.com>
+ *
+ * Adapted from frsky
+ *
+ * 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 _SERIAL_H_
+#define _SERIAL_H_
+
+typedef enum serial_tx_state_ {
+	TX_STATE_EMPTY = 0, //
+	TX_STATE_READY, //
+	TX_STATE_BUSY
+} serial_tx_state_t;
+extern serial_tx_state_t serialTxState;
+
+//! \brief Baudrate selection.
+enum SERIAL_BAUDS {
+	BAUD_4800 = 0,
+	BAUD_9600,
+	BAUD_14400,
+	BAUD_19200,
+	BAUD_38400,
+	BAUD_57600,
+	BAUD_76800
+};
+
+//! \brief Definition of baudrate settings item choices.
+typedef void (*SerialFuncP)(uint8_t event);
+
+extern SerialFuncP RXHandler;
+
+#define MAX_TX_BUFFER 32
+//#if 0
+extern uint8_t serialTxBuffer[MAX_TX_BUFFER]; // 32 characters
+extern uint8_t serialTxBufferCount;
+extern uint8_t * ptrTxISR;
+//#endif
+
+void SERIAL_Init(void);
+//void SERIAL_transmitBuffer(uint8_t len);
+extern void SERIAL_start_uart_send();
+extern void SERIAL_end_uart_send();
+extern void SERIAL_send_uart_bytes(uint8_t * buf, uint16_t len);
+
+void SERIAL_startTX(void);
+#define IS_TX_BUSY (serialTxState!=TX_STATE_EMPTY)
+
+#endif
diff --git a/radio/src/targets/sky9x/board_sky9x.h b/radio/src/targets/sky9x/board_sky9x.h
index 2c9592c9b..92ef02035 100644
--- a/radio/src/targets/sky9x/board_sky9x.h
+++ b/radio/src/targets/sky9x/board_sky9x.h
@@ -290,7 +290,8 @@ uint32_t telemetryTransmitPending();
 void telemetryTransmitBuffer(uint8_t * buffer, uint32_t size);
 
 // Second UART driver
-void telemetrySecondPortInit(unsigned int protocol);
+#define serialTracesEnabled()          false
+void serial2TelemetryInit(unsigned int protocol);
 bool telemetrySecondPortReceive(uint8_t & data);
 
 #endif
diff --git a/radio/src/targets/sky9x/second_serial_driver.cpp b/radio/src/targets/sky9x/serial2_driver.cpp
similarity index 94%
rename from radio/src/targets/sky9x/second_serial_driver.cpp
rename to radio/src/targets/sky9x/serial2_driver.cpp
index 69b0d7bdf..799936baa 100644
--- a/radio/src/targets/sky9x/second_serial_driver.cpp
+++ b/radio/src/targets/sky9x/serial2_driver.cpp
@@ -53,7 +53,7 @@ Fifo<512> serial2RxFifo;
  * This function is synchronous (i.e. uses polling).
  * c  Character to send.
  */
-void debugPutc(const char c)
+void serial2Putc(const char c)
 {
   Uart *pUart = SECOND_SERIAL_UART;
 
@@ -117,7 +117,7 @@ extern "C" void UART0_IRQHandler()
 #endif
 
 #if defined(FRSKY)
-void telemetrySecondPortInit(unsigned int /*protocol*/)
+void serial2TelemetryInit(unsigned int /*protocol*/)
 {
   SECOND_UART_Configure(FRSKY_D_BAUDRATE, Master_frequency);
   // startPdcUsartReceive();
diff --git a/radio/src/targets/taranis/board_taranis.cpp b/radio/src/targets/taranis/board_taranis.cpp
index c24c633b6..d47b572c4 100644
--- a/radio/src/targets/taranis/board_taranis.cpp
+++ b/radio/src/targets/taranis/board_taranis.cpp
@@ -260,7 +260,7 @@ void checkTrainerSettings()
         stop_sbus_on_heartbeat_capture() ;
         break;
       case TRAINER_MODE_MASTER_BATTERY_COMPARTMENT:
-        uart3Stop();
+        serial2Stop();
     }
 
     currentTrainerMode = requiredTrainerMode;
@@ -275,8 +275,8 @@ void checkTrainerSettings()
          init_sbus_on_heartbeat_capture() ;
          break;
       case TRAINER_MODE_MASTER_BATTERY_COMPARTMENT:
-        if (g_eeGeneral.uart3Mode == UART_MODE_SBUS_TRAINER) {
-          uart3SbusInit();
+        if (g_eeGeneral.serial2Mode == UART_MODE_SBUS_TRAINER) {
+          serial2SbusInit();
           break;
         }
         // no break
diff --git a/radio/src/targets/taranis/board_taranis.h b/radio/src/targets/taranis/board_taranis.h
index 1123eb0fa..30f3b5fe9 100644
--- a/radio/src/targets/taranis/board_taranis.h
+++ b/radio/src/targets/taranis/board_taranis.h
@@ -320,13 +320,15 @@ void hapticOff(void);
   void hapticOn(void);
 #endif
 
-// SERIAL_USART driver
+// Second serial port driver
 #define DEBUG_BAUDRATE                 115200
-void uart3Init(unsigned int mode, unsigned int protocol);
-void uart3Putc(const char c);
-#define telemetrySecondPortInit(protocol) uart3Init(UART_MODE_TELEMETRY, protocol)
-void uart3SbusInit(void);
-void uart3Stop(void);
+extern uint8_t serial2Mode;
+#define serialTracesEnabled()          (serial2Mode == 0)
+void serial2Init(unsigned int mode, unsigned int protocol);
+void serial2Putc(char c);
+#define serial2TelemetryInit(protocol) serial2Init(UART_MODE_TELEMETRY, protocol)
+void serial2SbusInit(void);
+void serial2Stop(void);
 
 // BT driver
 #define BLUETOOTH_DEFAULT_BAUDRATE     115200
diff --git a/radio/src/targets/taranis/uart3_driver.cpp b/radio/src/targets/taranis/serial2_driver.cpp
similarity index 85%
rename from radio/src/targets/taranis/uart3_driver.cpp
rename to radio/src/targets/taranis/serial2_driver.cpp
index 987273740..74f2943d2 100644
--- a/radio/src/targets/taranis/uart3_driver.cpp
+++ b/radio/src/targets/taranis/serial2_driver.cpp
@@ -36,7 +36,7 @@
 
 #include "../../opentx.h"
 
-uint8_t uart3Mode = UART_MODE_NONE;
+uint8_t serial2Mode = 0;
 Fifo<512> uart3TxFifo;
 extern Fifo<512> telemetryFifo;
 extern Fifo<32> sbusFifo;
@@ -73,17 +73,17 @@ void uart3Setup(unsigned int baudrate)
   NVIC_EnableIRQ(SERIAL_USART_IRQn);
 }
 
-void uart3Init(unsigned int mode, unsigned int protocol)
+void serial2Init(unsigned int mode, unsigned int protocol)
 {
   USART_DeInit(SERIAL_USART);
 
-  uart3Mode = false;
+  serial2Mode = mode;
 
   switch (mode) {
     case UART_MODE_TELEMETRY_MIRROR:
       uart3Setup(FRSKY_SPORT_BAUDRATE);
       break;
-#if defined(DEBUG)
+#if !defined(USB_SERIAL) && (defined(DEBUG) || defined(CLI))
     case UART_MODE_DEBUG:
       uart3Setup(DEBUG_BAUDRATE);
       break;
@@ -94,36 +94,21 @@ void uart3Init(unsigned int mode, unsigned int protocol)
       }
       break;
   }
-
-  uart3Mode = mode;
 }
 
-void uart3Putc(const char c)
+void serial2Putc(char c)
 {
   uart3TxFifo.push(c);
   USART_ITConfig(SERIAL_USART, USART_IT_TXE, ENABLE);
 }
 
-#if defined(DEBUG)
-void debugPutc(const char c)
-{
-  if (uart3Mode == UART_MODE_DEBUG) {
-    uart3Putc(c);
-#if defined(USB_SERIAL)
-    // also send debug output into USB serial port (if active)
-    sendUsbSerialChar(c);
-#endif
-  }
-}
-#endif
-
-void uart3SbusInit()
+void serial2SbusInit()
 {
   uart3Setup(100000);
   SERIAL_USART->CR1 |= USART_CR1_M | USART_CR1_PCE ;
 }
 
-void uart3Stop()
+void serial2Stop()
 {
   USART_DeInit(SERIAL_USART);
 }
@@ -149,13 +134,18 @@ extern "C" void SERIAL_USART_IRQHandler(void)
     uint8_t data = SERIAL_USART->DR;
 
     if (!(status & USART_FLAG_ERRORS)) {
-      switch (uart3Mode) {
+      switch (serial2Mode) {
         case UART_MODE_TELEMETRY:
           telemetryFifo.push(data);
           break;
         case UART_MODE_SBUS_TRAINER:
           sbusFifo.push(data);
           break;
+#if !defined(USB_SERIAL) && defined(CLI)
+        case UART_MODE_DEBUG:
+          cliRxFifo.push(data);
+          break;
+#endif
       }
     }
 
diff --git a/radio/src/tasks_arm.cpp b/radio/src/tasks_arm.cpp
index 8d3762411..c6d8ddd74 100644
--- a/radio/src/tasks_arm.cpp
+++ b/radio/src/tasks_arm.cpp
@@ -40,7 +40,6 @@
 #define MIXER_STACK_SIZE    500
 #define AUDIO_STACK_SIZE    500
 #define BT_STACK_SIZE       500
-#define DEBUG_STACK_SIZE    500
 
 #if defined(_MSC_VER)
   #define _ALIGNED(x) __declspec(align(x))
@@ -63,11 +62,6 @@ OS_TID btTaskId;
 OS_STK btStack[BT_STACK_SIZE];
 #endif
 
-#if defined(DEBUG)
-OS_TID debugTaskId;
-OS_STK debugStack[DEBUG_STACK_SIZE];
-#endif
-
 OS_MutexID audioMutex;
 OS_MutexID mixerMutex;
 
@@ -210,8 +204,8 @@ void tasksStart()
 {
   CoInitOS();
 
-#if defined(CPUARM) && defined(DEBUG) && !defined(SIMU)
-  debugTaskId = CoCreateTaskEx(debugTask, NULL, 10, &debugStack[DEBUG_STACK_SIZE-1], DEBUG_STACK_SIZE, 1, false);
+#if defined(CLI)
+  cliStart();
 #endif
 
 #if defined(BLUETOOTH)
diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp
index bb832b122..2402eb8f1 100644
--- a/radio/src/telemetry/frsky.cpp
+++ b/radio/src/telemetry/frsky.cpp
@@ -165,8 +165,8 @@ NOINLINE void processSerialData(uint8_t data)
 #endif
 
 #if defined(PCBTARANIS)
-    if (g_eeGeneral.uart3Mode == UART_MODE_TELEMETRY_MIRROR) {
-      uart3Putc(data);
+    if (g_eeGeneral.serial2Mode == UART_MODE_TELEMETRY_MIRROR) {
+      serial2Putc(data);
     }
 #endif
 
@@ -604,7 +604,7 @@ void telemetryInit(void)
   }
   else if (telemetryProtocol==PROTOCOL_FRSKY_D_SECONDARY) {
     telemetryPortInit(0);
-    telemetrySecondPortInit(PROTOCOL_FRSKY_D_SECONDARY);
+    serial2TelemetryInit(PROTOCOL_FRSKY_D_SECONDARY);
   }
   else {
     telemetryPortInit(FRSKY_SPORT_BAUDRATE);
diff --git a/radio/src/telemetry/mavlink.h b/radio/src/telemetry/mavlink.h
index 9c76b54c6..63ae2f661 100644
--- a/radio/src/telemetry/mavlink.h
+++ b/radio/src/telemetry/mavlink.h
@@ -43,10 +43,8 @@
 #define MAVLINK_COMM_NUM_BUFFERS 1
 
 #include "GCS_MAVLink/include_v1.0/mavlink_types.h"
-#include "serial.h"
+#include "targets/common_avr/serial_driver.h"
 #include "opentx.h"
-#include "serial.h"
-//#include "include/mavlink_helpers.h"
 
 extern mavlink_system_t mavlink_system;