diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 2b7b44bc6c..1d5dbacb16 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -91,6 +91,9 @@ FastSerialPort2(Serial3);
 FastSerialPort3(Serial3);        // Telemetry port for APM1
 #endif
 
+// port to use for command line interface
+static FastSerial *cliSerial = &Serial;
+
 // this sets up the parameter table, and sets the default values. This
 // must be the first AP_Param variable declared to ensure its
 // constructor runs before the constructors of the other AP_Param
@@ -828,15 +831,6 @@ static void medium_loop()
             ahrs.set_compass(NULL);
         }
 #endif
-/*{
- *  Serial.print(ahrs.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
- *  Serial.print(ahrs.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
- *  Serial.print(ahrs.yaw_sensor, DEC);	Serial.printf_P(PSTR("\t"));
- *  Vector3f tempaccel = ins.get_accel();
- *  Serial.print(tempaccel.x, DEC);	Serial.printf_P(PSTR("\t"));
- *  Serial.print(tempaccel.y, DEC);	Serial.printf_P(PSTR("\t"));
- *  Serial.println(tempaccel.z, DEC);
- *  }*/
 
         break;
 
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 8c8ca127d4..7e40c61099 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -797,7 +797,7 @@ GCS_MAVLINK::update(void)
                 crlf_count = 0;
             }
             if (crlf_count == 3) {
-                run_cli();
+                run_cli(_port);
             }
         }
 #endif
@@ -1868,8 +1868,6 @@ mission_failed:
         // TODO: check scaling for temp/absPress
         //float temp = 70;
         //float absPress = 1;
-        //Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
-        //Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
 
         // rad/sec
         Vector3f gyros;
diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index 523859aff8..1f378e7bfc 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -21,7 +21,7 @@ static int8_t   select_logs(uint8_t argc,               const Menu::arg *argv);
 // printf_P is a version of print_f that reads from flash memory
 //static int8_t	help_log(uint8_t argc,          const Menu::arg *argv)
 /*{
- *       Serial.printf_P(PSTR("\n"
+ *       cliSerial->printf_P(PSTR("\n"
  *                                                "Commands:\n"
  *                                                "  dump <n>"
  *                                                "  erase (all logs)\n"
@@ -55,16 +55,16 @@ print_log_menu(void)
 
     uint16_t num_logs = DataFlash.get_num_logs();
 
-    Serial.printf_P(PSTR("logs enabled: "));
+    cliSerial->printf_P(PSTR("logs enabled: "));
 
     if (0 == g.log_bitmask) {
-        Serial.printf_P(PSTR("none"));
+        cliSerial->printf_P(PSTR("none"));
     }else{
         // Macro to make the following code a bit easier on the eye.
         // Pass it the capitalised name of the log option, as defined
         // in defines.h but without the LOG_ prefix.  It will check for
         // the bit being set and print the name of the log option to suit.
- #define PLOG(_s)        if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(# _s))
+ #define PLOG(_s)        if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(# _s))
         PLOG(ATTITUDE_FAST);
         PLOG(ATTITUDE_MED);
         PLOG(GPS);
@@ -78,24 +78,24 @@ print_log_menu(void)
  #undef PLOG
     }
 
-    Serial.println();
+    cliSerial->println();
 
     if (num_logs == 0) {
-        Serial.printf_P(PSTR("\nNo logs\n\n"));
+        cliSerial->printf_P(PSTR("\nNo logs\n\n"));
     }else{
-        Serial.printf_P(PSTR("\n%d logs\n"), (int)num_logs);
+        cliSerial->printf_P(PSTR("\n%d logs\n"), (int)num_logs);
 
         for(int16_t i=num_logs; i>=1; i--) {
             int16_t last_log_start = log_start, last_log_end = log_end;
             temp = last_log_num-i+1;
             DataFlash.get_log_boundaries(temp, log_start, log_end);
-            Serial.printf_P(PSTR("Log %d,    start %d,   end %d\n"), (int)temp, (int)log_start, (int)log_end);
+            cliSerial->printf_P(PSTR("Log %d,    start %d,   end %d\n"), (int)temp, (int)log_start, (int)log_end);
             if (last_log_start == log_start && last_log_end == log_end) {
                 // we are printing bogus logs
                 break;
             }
         }
-        Serial.println();
+        cliSerial->println();
     }
     return(true);
 }
@@ -115,28 +115,28 @@ dump_log(uint8_t argc, const Menu::arg *argv)
     if (dump_log == -2) {
         for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) {
             DataFlash.StartRead(count);
-            Serial.printf_P(PSTR("DF page, log file #, log page: %d,\t"), (int)count);
-            Serial.printf_P(PSTR("%d,\t"), (int)DataFlash.GetFileNumber());
-            Serial.printf_P(PSTR("%d\n"), (int)DataFlash.GetFilePage());
+            cliSerial->printf_P(PSTR("DF page, log file #, log page: %d,\t"), (int)count);
+            cliSerial->printf_P(PSTR("%d,\t"), (int)DataFlash.GetFileNumber());
+            cliSerial->printf_P(PSTR("%d\n"), (int)DataFlash.GetFilePage());
         }
         return(-1);
     } else if (dump_log <= 0) {
-        Serial.printf_P(PSTR("dumping all\n"));
+        cliSerial->printf_P(PSTR("dumping all\n"));
         Log_Read(1, DataFlash.df_NumPages);
         return(-1);
     } else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) {
-        Serial.printf_P(PSTR("bad log number\n"));
+        cliSerial->printf_P(PSTR("bad log number\n"));
         return(-1);
     }
 
     DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
-    Serial.printf_P(PSTR("Dumping Log %d,    start pg %d,   end pg %d\n"),
+    cliSerial->printf_P(PSTR("Dumping Log %d,    start pg %d,   end pg %d\n"),
                     (int)dump_log,
                     (int)dump_log_start,
                     (int)dump_log_end);
 
     Log_Read(dump_log_start, dump_log_end);
-    Serial.printf_P(PSTR("Done\n"));
+    cliSerial->printf_P(PSTR("Done\n"));
     return 0;
 }
 
@@ -162,7 +162,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
     uint16_t bits;
 
     if (argc != 2) {
-        Serial.printf_P(PSTR("missing log type\n"));
+        cliSerial->printf_P(PSTR("missing log type\n"));
         return(-1);
     }
 
@@ -387,7 +387,7 @@ static void Log_Write_Current()
 // Read a Current packet
 static void Log_Read_Current()
 {
-    Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
+    cliSerial->printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
                     (int)DataFlash.ReadInt(),
                     ((float)DataFlash.ReadInt() / 100.f),
                     ((float)DataFlash.ReadInt() / 100.f),
@@ -399,15 +399,15 @@ static void Log_Read_Control_Tuning()
 {
     float logvar;
 
-    Serial.printf_P(PSTR("CTUN:"));
+    cliSerial->printf_P(PSTR("CTUN:"));
     for (int16_t y = 1; y < 10; y++) {
         logvar = DataFlash.ReadInt();
         if(y < 8) logvar        = logvar/100.f;
         if(y == 9) logvar       = logvar/10000.f;
-        Serial.print(logvar);
+        cliSerial->print(logvar);
         print_comma();
     }
-    Serial.println();
+    cliSerial->println();
 }
 
 // Read a nav tuning packet
@@ -417,7 +417,7 @@ static void Log_Read_Nav_Tuning()
     for (int8_t i=0; i<7; i++) {
         d[i] = DataFlash.ReadInt();
     }
-    Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"),              // \n
+    cliSerial->printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"),              // \n
                     d[0]/100.0,
                     (int)d[1],
                     ((uint16_t)d[2])/100.0,
@@ -433,9 +433,9 @@ static void Log_Read_Performance()
     int32_t pm_time;
     int16_t logvar;
 
-    Serial.printf_P(PSTR("PM:"));
+    cliSerial->printf_P(PSTR("PM:"));
     pm_time = DataFlash.ReadLong();
-    Serial.print(pm_time);
+    cliSerial->print(pm_time);
     print_comma();
     for (int16_t y = 1; y <= 12; y++) {
         if(y < 3 || y > 7) {
@@ -443,10 +443,10 @@ static void Log_Read_Performance()
         }else{
             logvar = DataFlash.ReadByte();
         }
-        Serial.print(logvar);
+        cliSerial->print(logvar);
         print_comma();
     }
-    Serial.println();
+    cliSerial->println();
 }
 
 // Read a command processing packet
@@ -455,18 +455,18 @@ static void Log_Read_Cmd()
     byte logvarb;
     int32_t logvarl;
 
-    Serial.printf_P(PSTR("CMD:"));
+    cliSerial->printf_P(PSTR("CMD:"));
     for(int16_t i = 1; i < 4; i++) {
         logvarb = DataFlash.ReadByte();
-        Serial.print(logvarb, DEC);
+        cliSerial->print(logvarb, DEC);
         print_comma();
     }
     for(int16_t i = 1; i < 4; i++) {
         logvarl = DataFlash.ReadLong();
-        Serial.print(logvarl, DEC);
+        cliSerial->print(logvarl, DEC);
         print_comma();
     }
-    Serial.println();
+    cliSerial->println();
 }
 
 static void Log_Read_Startup()
@@ -474,13 +474,13 @@ static void Log_Read_Startup()
     byte logbyte = DataFlash.ReadByte();
 
     if (logbyte == TYPE_AIRSTART_MSG)
-        Serial.printf_P(PSTR("AIR START - "));
+        cliSerial->printf_P(PSTR("AIR START - "));
     else if (logbyte == TYPE_GROUNDSTART_MSG)
-        Serial.printf_P(PSTR("GROUND START - "));
+        cliSerial->printf_P(PSTR("GROUND START - "));
     else
-        Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
+        cliSerial->printf_P(PSTR("UNKNOWN STARTUP - "));
 
-    Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
+    cliSerial->printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
 }
 
 // Read an attitude packet
@@ -490,7 +490,7 @@ static void Log_Read_Attitude()
     d[0] = DataFlash.ReadInt();
     d[1] = DataFlash.ReadInt();
     d[2] = DataFlash.ReadInt();
-    Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
+    cliSerial->printf_P(PSTR("ATT: %d, %d, %u\n"),
                     (int)d[0], (int)d[1],
                     (unsigned)d[2]);
 }
@@ -498,7 +498,7 @@ static void Log_Read_Attitude()
 // Read a mode packet
 static void Log_Read_Mode()
 {
-    Serial.printf_P(PSTR("MOD:"));
+    cliSerial->printf_P(PSTR("MOD:"));
     print_flight_mode(DataFlash.ReadByte());
 }
 
@@ -518,7 +518,7 @@ static void Log_Read_GPS()
     l[4] = DataFlash.ReadLong();
     l[5] = DataFlash.ReadLong();
     l[6] = DataFlash.ReadLong();
-    Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+    cliSerial->printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
                     (long)l[0], (int)b[0], (int)b[1],
                     l[1]/t7, l[2]/t7,
                     (int)i,
@@ -529,13 +529,13 @@ static void Log_Read_GPS()
 static void Log_Read_Raw()
 {
     float logvar;
-    Serial.printf_P(PSTR("RAW:"));
+    cliSerial->printf_P(PSTR("RAW:"));
     for (int16_t y = 0; y < 6; y++) {
         logvar = (float)DataFlash.ReadLong() / t7;
-        Serial.print(logvar);
+        cliSerial->print(logvar);
         print_comma();
     }
-    Serial.println();
+    cliSerial->println();
 }
 
 // Read the DataFlash log memory : Packet Parser
@@ -544,9 +544,9 @@ static void Log_Read(int16_t start_page, int16_t end_page)
     int16_t packet_count = 0;
 
  #ifdef AIRFRAME_NAME
-    Serial.printf_P(PSTR((AIRFRAME_NAME)
+    cliSerial->printf_P(PSTR((AIRFRAME_NAME)
  #endif
-    Serial.printf_P(PSTR("\n" THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n" THISFIRMWARE
                          "\nFree RAM: %u\n"),
                     memcheck_available_memory());
 
@@ -558,7 +558,7 @@ static void Log_Read(int16_t start_page, int16_t end_page)
         packet_count = Log_Read_Process(start_page, end_page);
     }
 
-    Serial.printf_P(PSTR("Number of packets read: %d\n"), (int) packet_count);
+    cliSerial->printf_P(PSTR("Number of packets read: %d\n"), (int) packet_count);
 }
 
 // Read the DataFlash log memory : Packet Parser
@@ -626,7 +626,7 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
                                         Log_Read_GPS();
                                         log_step++;
                                     }else{
-                                        Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
+                                        cliSerial->printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
                                         log_step = 0;            // Restart, we have a problem...
                                     }
                                 }
@@ -635,7 +635,7 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
                                 if(data == END_BYTE) {
                                     packet_count++;
                                 }else{
-                                    Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),(int)data);
+                                    cliSerial->printf_P(PSTR("Error Reading END_BYTE: %d\n"),(int)data);
                                 }
                                 log_step = 0;                   // Restart sequence: new packet...
                                 break;
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 0221c0b569..d90f5d1ad2 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -699,17 +699,17 @@ static void load_parameters(void)
         g.format_version != Parameters::k_format_version) {
 
         // erase all parameters
-        Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
+        cliSerial->printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
         AP_Param::erase_all();
 
         // save the current format version
         g.format_version.set_and_save(Parameters::k_format_version);
-        Serial.println_P(PSTR("done."));
+        cliSerial->println_P(PSTR("done."));
     } else {
         uint32_t before = micros();
         // Load all auto-loaded EEPROM variables
         AP_Param::load_all();
 
-        Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
+        cliSerial->printf_P(PSTR("load_all took %luus\n"), micros() - before);
     }
 }
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index 9c1eaaeeb1..02f943c393 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -244,10 +244,7 @@ static void do_takeoff()
     set_next_WP(&next_nav_command);
     // pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
     takeoff_pitch_cd                = (int)next_nav_command.p1 * 100;
-    //Serial.printf_P(PSTR("TO pitch:"));	Serial.println(takeoff_pitch);
-    //Serial.printf_P(PSTR("home.alt:"));	Serial.println(home.alt);
     takeoff_altitude        = next_nav_command.alt;
-    //Serial.printf_P(PSTR("takeoff_altitude:"));	Serial.println(takeoff_altitude);
     next_WP.lat             = home.lat + 1000;          // so we don't have bad calcs
     next_WP.lng             = home.lng + 1000;          // so we don't have bad calcs
     takeoff_complete        = false;                            // set flag to use gps ground course during TO.  IMU will be doing yaw drift correction
diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index 371538b207..769290158a 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -21,7 +21,6 @@ static void navigate()
 
     if (wp_distance < 0) {
         gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0"));
-        //Serial.println(wp_distance,DEC);
         return;
     }
 
diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde
index 4002db75bb..b534365163 100644
--- a/ArduPlane/radio.pde
+++ b/ArduPlane/radio.pde
@@ -108,7 +108,7 @@ static void read_radio()
     }
 
     /*
-     *  Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
+     *  cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
      *                       (int)g.rc_1.control_in,
      *                       (int)g.rc_2.control_in,
      *                       (int)g.rc_3.control_in,
diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde
index 63cec6147d..53a6e8dfbb 100644
--- a/ArduPlane/setup.pde
+++ b/ArduPlane/setup.pde
@@ -40,7 +40,7 @@ static int8_t
 setup_mode(uint8_t argc, const Menu::arg *argv)
 {
     // Give the user some guidance
-    Serial.printf_P(PSTR("Setup Mode\n"
+    cliSerial->printf_P(PSTR("Setup Mode\n"
                          "\n"
                          "IMPORTANT: if you have not previously set this system up, use the\n"
                          "'reset' command to initialize the EEPROM to sensible default values\n"
@@ -69,7 +69,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
     report_ins();
     report_compass();
 
-    Serial.printf_P(PSTR("Raw Values\n"));
+    cliSerial->printf_P(PSTR("Raw Values\n"));
     print_divider();
 
     AP_Param::show_all();
@@ -84,16 +84,16 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 {
     int c;
 
-    Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
+    cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
 
     do {
-        c = Serial.read();
+        c = cliSerial->read();
     } while (-1 == c);
 
     if (('y' != c) && ('Y' != c))
         return(-1);
     AP_Param::erase_all();
-    Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
+    cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
 
     //default_flight_modes();   // This will not work here.  Replacement code located in init_ardupilot()
 
@@ -109,7 +109,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_radio(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.printf_P(PSTR("\n\nRadio Setup:\n"));
+    cliSerial->printf_P(PSTR("\n\nRadio Setup:\n"));
     uint8_t i;
 
     for(i = 0; i < 100; i++) {
@@ -120,7 +120,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 
     if(g.channel_roll.radio_in < 500) {
         while(1) {
-            Serial.printf_P(PSTR("\nNo radio; Check connectors."));
+            cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
             delay(1000);
             // stop here
         }
@@ -152,7 +152,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
     g.rc_7.radio_trim = 1500;
     g.rc_8.radio_trim = 1500;
 
-    Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
+    cliSerial->printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
     while(1) {
 
         delay(20);
@@ -169,8 +169,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
         g.rc_7.update_min_max();
         g.rc_8.update_min_max();
 
-        if(Serial.available() > 0) {
-            Serial.flush();
+        if(cliSerial->available() > 0) {
+            cliSerial->flush();
             g.channel_roll.save_eeprom();
             g.channel_pitch.save_eeprom();
             g.channel_throttle.save_eeprom();
@@ -194,7 +194,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 {
     byte switchPosition, mode = 0;
 
-    Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
+    cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
     print_hit_enter();
     trim_radio();
 
@@ -257,7 +257,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
         }
 
         // escape hatch
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             // save changes
             for (mode=0; mode<6; mode++)
                 flight_modes[mode].save();
@@ -282,10 +282,10 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
 {
     int c;
 
-    Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
+    cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
 
     do {
-        c = Serial.read();
+        c = cliSerial->read();
     } while (-1 == c);
 
     if (('y' != c) && ('Y' != c))
@@ -302,13 +302,38 @@ setup_level(uint8_t argc, const Menu::arg *argv)
 }
 
 #if !defined( __AVR_ATmega1280__ )
+/*
+  handle full accelerometer calibration via user dialog
+ */
+
+static void setup_printf_P(const prog_char_t *fmt, ...)
+{
+    va_list arg_list;
+    va_start(arg_list, fmt);
+    cliSerial->vprintf_P(fmt, arg_list);
+    va_end(arg_list);
+}
+
+static void setup_wait_key(void)
+{
+    // wait for user input
+    while (!cliSerial->available()) {
+        delay(20);
+    }
+    // clear input buffer
+    while( cliSerial->available() ) {
+        cliSerial->read();
+    }
+}
+
 static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
+    cliSerial->println_P(PSTR("Initialising gyros"));
     ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
-    if (ins.calibrate_accel(delay, flash_leds, NULL)) {
+    if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
         if (g.manual_level == 0) {
-            Serial.println_P(PSTR("Setting MANUAL_LEVEL to 1"));
+            cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1"));
             g.manual_level.set_and_save(1);
         }
     }
@@ -323,7 +348,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
     if (!strcmp_P(argv[1].str, PSTR("on"))) {
         compass.set_orientation(MAG_ORIENTATION);       // set compass's orientation on aircraft
         if (!compass.init()) {
-            Serial.println_P(PSTR("Compass initialisation failed!"));
+            cliSerial->println_P(PSTR("Compass initialisation failed!"));
             g.compass_enabled = false;
         } else {
             g.compass_enabled = true;
@@ -335,7 +360,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
         compass.set_offsets(0,0,0);
 
     } else {
-        Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n"));
+        cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
         report_compass();
         return 0;
     }
@@ -352,7 +377,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
         g.battery_monitoring.set_and_save(argv[1].i);
 
     } else {
-        Serial.printf_P(PSTR("\nOptions: 3-4"));
+        cliSerial->printf_P(PSTR("\nOptions: 3-4"));
     }
 
     report_batt_monitor();
@@ -366,17 +391,17 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
 static void report_batt_monitor()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("Batt Mointor\n"));
+    cliSerial->printf_P(PSTR("Batt Mointor\n"));
     print_divider();
-    if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
-    if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
-    if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
+    if(g.battery_monitoring == 0) cliSerial->printf_P(PSTR("Batt monitoring disabled"));
+    if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("Monitoring batt volts"));
+    if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("Monitoring volts and current"));
     print_blanks(2);
 }
 static void report_radio()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("Radio\n"));
+    cliSerial->printf_P(PSTR("Radio\n"));
     print_divider();
     // radio
     print_radio_values();
@@ -386,30 +411,30 @@ static void report_radio()
 static void report_gains()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("Gains\n"));
+    cliSerial->printf_P(PSTR("Gains\n"));
     print_divider();
 
 #if APM_CONTROL == DISABLED
-	Serial.printf_P(PSTR("servo roll:\n"));
+	cliSerial->printf_P(PSTR("servo roll:\n"));
 	print_PID(&g.pidServoRoll);
 
-    Serial.printf_P(PSTR("servo pitch:\n"));
+    cliSerial->printf_P(PSTR("servo pitch:\n"));
     print_PID(&g.pidServoPitch);
 
-	Serial.printf_P(PSTR("servo rudder:\n"));
+	cliSerial->printf_P(PSTR("servo rudder:\n"));
 	print_PID(&g.pidServoRudder);
 #endif
 
-    Serial.printf_P(PSTR("nav roll:\n"));
+    cliSerial->printf_P(PSTR("nav roll:\n"));
     print_PID(&g.pidNavRoll);
 
-    Serial.printf_P(PSTR("nav pitch airspeed:\n"));
+    cliSerial->printf_P(PSTR("nav pitch airspeed:\n"));
     print_PID(&g.pidNavPitchAirspeed);
 
-    Serial.printf_P(PSTR("energry throttle:\n"));
+    cliSerial->printf_P(PSTR("energry throttle:\n"));
     print_PID(&g.pidTeThrottle);
 
-    Serial.printf_P(PSTR("nav pitch alt:\n"));
+    cliSerial->printf_P(PSTR("nav pitch alt:\n"));
     print_PID(&g.pidNavPitchAltitude);
 
     print_blanks(2);
@@ -418,10 +443,10 @@ static void report_gains()
 static void report_xtrack()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("Crosstrack\n"));
+    cliSerial->printf_P(PSTR("Crosstrack\n"));
     print_divider();
     // radio
-    Serial.printf_P(PSTR("XTRACK: %4.2f\n"
+    cliSerial->printf_P(PSTR("XTRACK: %4.2f\n"
                          "XTRACK angle: %d\n"),
                     (float)g.crosstrack_gain,
                     (int)g.crosstrack_entry_angle);
@@ -431,10 +456,10 @@ static void report_xtrack()
 static void report_throttle()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("Throttle\n"));
+    cliSerial->printf_P(PSTR("Throttle\n"));
     print_divider();
 
-    Serial.printf_P(PSTR("min: %d\n"
+    cliSerial->printf_P(PSTR("min: %d\n"
                          "max: %d\n"
                          "cruise: %d\n"
                          "failsafe_enabled: %d\n"
@@ -450,7 +475,7 @@ static void report_throttle()
 static void report_ins()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("INS\n"));
+    cliSerial->printf_P(PSTR("INS\n"));
     print_divider();
 
     print_gyro_offsets();
@@ -461,20 +486,20 @@ static void report_ins()
 static void report_compass()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("Compass: "));
+    cliSerial->printf_P(PSTR("Compass: "));
 
     switch (compass.product_id) {
     case AP_COMPASS_TYPE_HMC5883L:
-        Serial.println_P(PSTR("HMC5883L"));
+        cliSerial->println_P(PSTR("HMC5883L"));
         break;
     case AP_COMPASS_TYPE_HMC5843:
-        Serial.println_P(PSTR("HMC5843"));
+        cliSerial->println_P(PSTR("HMC5843"));
         break;
     case AP_COMPASS_TYPE_HIL:
-        Serial.println_P(PSTR("HIL"));
+        cliSerial->println_P(PSTR("HIL"));
         break;
     default:
-        Serial.println_P(PSTR("??"));
+        cliSerial->println_P(PSTR("??"));
         break;
     }
 
@@ -483,13 +508,13 @@ static void report_compass()
     print_enabled(g.compass_enabled);
 
     // mag declination
-    Serial.printf_P(PSTR("Mag Declination: %4.4f\n"),
+    cliSerial->printf_P(PSTR("Mag Declination: %4.4f\n"),
                     degrees(compass.get_declination()));
 
     Vector3f offsets = compass.get_offsets();
 
     // mag offsets
-    Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
+    cliSerial->printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
                     offsets.x,
                     offsets.y,
                     offsets.z);
@@ -499,7 +524,7 @@ static void report_compass()
 static void report_flight_modes()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("Flight modes\n"));
+    cliSerial->printf_P(PSTR("Flight modes\n"));
     print_divider();
 
     for(int16_t i = 0; i < 6; i++ ) {
@@ -515,7 +540,7 @@ static void report_flight_modes()
 static void
 print_PID(PID * pid)
 {
-    Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
+    cliSerial->printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
                     pid->kP(),
                     pid->kI(),
                     pid->kD(),
@@ -525,28 +550,28 @@ print_PID(PID * pid)
 static void
 print_radio_values()
 {
-    Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
-    Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
-    Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
-    Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
-    Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
-    Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
-    Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
-    Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
+    cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
+    cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
+    cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
+    cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
+    cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
+    cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
+    cliSerial->printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
+    cliSerial->printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
 
 }
 
 static void
 print_switch(byte p, byte m)
 {
-    Serial.printf_P(PSTR("Pos %d: "),p);
+    cliSerial->printf_P(PSTR("Pos %d: "),p);
     print_flight_mode(m);
 }
 
 static void
 print_done()
 {
-    Serial.printf_P(PSTR("\nSaved Settings\n\n"));
+    cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
 }
 
 static void
@@ -554,7 +579,7 @@ print_blanks(int16_t num)
 {
     while(num > 0) {
         num--;
-        Serial.println();
+        cliSerial->println();
     }
 }
 
@@ -562,9 +587,9 @@ static void
 print_divider(void)
 {
     for (int16_t i = 0; i < 40; i++) {
-        Serial.printf_P(PSTR("-"));
+        cliSerial->printf_P(PSTR("-"));
     }
-    Serial.println();
+    cliSerial->println();
 }
 
 static int8_t
@@ -597,20 +622,20 @@ radio_input_switch(void)
 static void zero_eeprom(void)
 {
     byte b = 0;
-    Serial.printf_P(PSTR("\nErasing EEPROM\n"));
+    cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
     for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) {
         eeprom_write_byte((uint8_t *) i, b);
     }
-    Serial.printf_P(PSTR("done\n"));
+    cliSerial->printf_P(PSTR("done\n"));
 }
 
 static void print_enabled(bool b)
 {
     if(b)
-        Serial.printf_P(PSTR("en"));
+        cliSerial->printf_P(PSTR("en"));
     else
-        Serial.printf_P(PSTR("dis"));
-    Serial.printf_P(PSTR("abled\n"));
+        cliSerial->printf_P(PSTR("dis"));
+    cliSerial->printf_P(PSTR("abled\n"));
 }
 
 static void
@@ -618,7 +643,7 @@ print_accel_offsets_and_scaling(void)
 {
     Vector3f accel_offsets = ins.get_accel_offsets();
     Vector3f accel_scale = ins.get_accel_scale();
-    Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
+    cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
                     (float)accel_offsets.x,                           // Pitch
                     (float)accel_offsets.y,                           // Roll
                     (float)accel_offsets.z,                           // YAW
@@ -631,7 +656,7 @@ static void
 print_gyro_offsets(void)
 {
     Vector3f gyro_offsets = ins.get_gyro_offsets();
-    Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
+    cliSerial->printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
                     (float)gyro_offsets.x,
                     (float)gyro_offsets.y,
                     (float)gyro_offsets.z);
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index aa7dc58c8c..b3930f0bbe 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -18,7 +18,7 @@ static int8_t   test_mode(uint8_t argc, const Menu::arg *argv);         // in te
 // printf_P is a version of print_f that reads from flash memory
 static int8_t   main_menu_help(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.printf_P(PSTR("Commands:\n"
+    cliSerial->printf_P(PSTR("Commands:\n"
                          "  logs        log readback/setup mode\n"
                          "  setup       setup mode\n"
                          "  test        test mode\n"
@@ -42,11 +42,15 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
 MENU(main_menu, THISFIRMWARE, main_menu_commands);
 
 // the user wants the CLI. It never exits
-static void run_cli(void)
+static void run_cli(FastSerial *port)
 {
     // disable the failsafe code in the CLI
     timer_scheduler.set_failsafe(NULL);
 
+    cliSerial = port;
+    Menu::set_port(port);
+    port->set_blocking_writes(true);
+
     while (1) {
         main_menu.run();
     }
@@ -86,7 +90,7 @@ static void init_ardupilot()
     // standard gps running
     Serial1.begin(38400, 256, 16);
 
-    Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
                          "\n\nFree RAM: %u\n"),
                     memcheck_available_memory());
 
@@ -169,7 +173,7 @@ static void init_ardupilot()
     if (g.compass_enabled==true) {
         compass.set_orientation(MAG_ORIENTATION);                                                       // set compass's orientation on aircraft
         if (!compass.init() || !compass.read()) {
-            Serial.println_P(PSTR("Compass initialisation failed!"));
+            cliSerial->println_P(PSTR("Compass initialisation failed!"));
             g.compass_enabled = false;
         } else {
             ahrs.set_compass(&compass);
@@ -220,7 +224,11 @@ static void init_ardupilot()
      */
     timer_scheduler.set_failsafe(failsafe_check);
 
-    Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n"));
+    const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
+    cliSerial->println_P(msg);
+#if USB_MUX_PIN == 0
+    Serial3.println_P(msg);
+#endif
 
     if (ENABLE_AIR_START == 1) {
         // Perform an air start and get back to flying
@@ -504,7 +512,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
     case 111:  return 111100;
     case 115:  return 115200;
     }
-    Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
+    cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
     return default_baud;
 }
 
@@ -554,7 +562,7 @@ uint16_t board_voltage(void)
  */
 static void reboot_apm(void)
 {
-    Serial.println_P(PSTR("REBOOTING"));
+    cliSerial->println_P(PSTR("REBOOTING"));
     delay(100);
     // see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1250663814/
     // for the method
@@ -570,36 +578,36 @@ print_flight_mode(uint8_t mode)
 {
     switch (mode) {
     case MANUAL:
-        Serial.println_P(PSTR("Manual"));
+        cliSerial->println_P(PSTR("Manual"));
         break;
     case CIRCLE:
-        Serial.println_P(PSTR("Circle"));
+        cliSerial->println_P(PSTR("Circle"));
         break;
     case STABILIZE:
-        Serial.println_P(PSTR("Stabilize"));
+        cliSerial->println_P(PSTR("Stabilize"));
         break;
     case FLY_BY_WIRE_A:
-        Serial.println_P(PSTR("FBW_A"));
+        cliSerial->println_P(PSTR("FBW_A"));
         break;
     case FLY_BY_WIRE_B:
-        Serial.println_P(PSTR("FBW_B"));
+        cliSerial->println_P(PSTR("FBW_B"));
         break;
     case AUTO:
-        Serial.println_P(PSTR("AUTO"));
+        cliSerial->println_P(PSTR("AUTO"));
         break;
     case RTL:
-        Serial.println_P(PSTR("RTL"));
+        cliSerial->println_P(PSTR("RTL"));
         break;
     case LOITER:
-        Serial.println_P(PSTR("Loiter"));
+        cliSerial->println_P(PSTR("Loiter"));
         break;
     default:
-        Serial.println_P(PSTR("---"));
+        cliSerial->println_P(PSTR("---"));
         break;
     }
 }
 
 static void print_comma(void)
 {
-    Serial.print_P(PSTR(","));
+    cliSerial->print_P(PSTR(","));
 }
diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde
index bc304d4500..77cf567084 100644
--- a/ArduPlane/test.pde
+++ b/ArduPlane/test.pde
@@ -70,14 +70,14 @@ MENU(test_menu, "test", test_menu_commands);
 static int8_t
 test_mode(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.printf_P(PSTR("Test Mode\n\n"));
+    cliSerial->printf_P(PSTR("Test Mode\n\n"));
     test_menu.run();
     return 0;
 }
 
 static void print_hit_enter()
 {
-    Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
+    cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
 }
 
 static int8_t
@@ -87,10 +87,10 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
 
     // hexdump the EEPROM
     for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
-        Serial.printf_P(PSTR("%04x:"), i);
+        cliSerial->printf_P(PSTR("%04x:"), i);
         for (j = 0; j < 16; j++)
-            Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
-        Serial.println();
+            cliSerial->printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
+        cliSerial->println();
     }
     return(0);
 }
@@ -108,7 +108,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
         // ----------------------------------------------------------
         read_radio();
 
-        Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
+        cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
                         (int)g.channel_roll.radio_in,
                         (int)g.channel_pitch.radio_in,
                         (int)g.channel_throttle.radio_in,
@@ -118,7 +118,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
                         (int)g.rc_7.radio_in,
                         (int)g.rc_8.radio_in);
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -136,15 +136,15 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
 
         // New radio frame? (we could use also if((millis()- timer) > 20)
         if (APM_RC.GetState() == 1) {
-            Serial.print_P(PSTR("CH:"));
+            cliSerial->print_P(PSTR("CH:"));
             for(int16_t i = 0; i < 8; i++) {
-                Serial.print(APM_RC.InputCh(i));        // Print channel values
+                cliSerial->print(APM_RC.InputCh(i));        // Print channel values
                 print_comma();
                 APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
             }
-            Serial.println();
+            cliSerial->println();
         }
-        if (Serial.available() > 0) {
+        if (cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -174,7 +174,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
         // ------------------------------
         set_servos();
 
-        Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
+        cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
                         (int)g.channel_roll.control_in,
                         (int)g.channel_pitch.control_in,
                         (int)g.channel_throttle.control_in,
@@ -184,7 +184,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
                         (int)g.rc_7.control_in,
                         (int)g.rc_8.control_in);
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -206,7 +206,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
 
     oldSwitchPosition = readSwitch();
 
-    Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
+    cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
     while(g.channel_throttle.control_in > 0) {
         delay(20);
         read_radio();
@@ -217,18 +217,18 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
         read_radio();
 
         if(g.channel_throttle.control_in > 0) {
-            Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)g.channel_throttle.control_in);
+            cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)g.channel_throttle.control_in);
             fail_test++;
         }
 
         if(oldSwitchPosition != readSwitch()) {
-            Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
+            cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
             print_flight_mode(readSwitch());
             fail_test++;
         }
 
         if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()) {
-            Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.radio_in);
+            cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.radio_in);
             print_flight_mode(readSwitch());
             fail_test++;
         }
@@ -236,8 +236,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
         if(fail_test > 0) {
             return (0);
         }
-        if(Serial.available() > 0) {
-            Serial.printf_P(PSTR("LOS caused no change in APM.\n"));
+        if(cliSerial->available() > 0) {
+            cliSerial->printf_P(PSTR("LOS caused no change in APM.\n"));
             return (0);
         }
     }
@@ -255,12 +255,12 @@ test_battery(uint8_t argc, const Menu::arg *argv)
             read_radio();
             read_battery();
             if (g.battery_monitoring == 3) {
-                Serial.printf_P(PSTR("V: %4.4f\n"),
+                cliSerial->printf_P(PSTR("V: %4.4f\n"),
                                 battery_voltage1,
                                 current_amps1,
                                 current_total1);
             } else {
-                Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
+                cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
                                 battery_voltage1,
                                 current_amps1,
                                 current_total1);
@@ -270,12 +270,12 @@ test_battery(uint8_t argc, const Menu::arg *argv)
             // ------------------------------
             set_servos();
 
-            if(Serial.available() > 0) {
+            if(cliSerial->available() > 0) {
                 return (0);
             }
         }
     } else {
-        Serial.printf_P(PSTR("Not enabled\n"));
+        cliSerial->printf_P(PSTR("Not enabled\n"));
         return (0);
     }
 
@@ -288,17 +288,17 @@ test_relay(uint8_t argc, const Menu::arg *argv)
     delay(1000);
 
     while(1) {
-        Serial.printf_P(PSTR("Relay on\n"));
+        cliSerial->printf_P(PSTR("Relay on\n"));
         relay.on();
         delay(3000);
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
 
-        Serial.printf_P(PSTR("Relay off\n"));
+        cliSerial->printf_P(PSTR("Relay off\n"));
         relay.off();
         delay(3000);
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -311,14 +311,14 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 
     // save the alitude above home option
     if (g.RTL_altitude_cm < 0) {
-        Serial.printf_P(PSTR("Hold current altitude\n"));
+        cliSerial->printf_P(PSTR("Hold current altitude\n"));
     }else{
-        Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100);
+        cliSerial->printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100);
     }
 
-    Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
-    Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
-    Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
+    cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
+    cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
+    cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
 
     for(byte i = 0; i <= g.command_total; i++) {
         struct Location temp = get_cmd_with_index(i);
@@ -331,7 +331,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 static void
 test_wp_print(struct Location *cmd, byte wp_index)
 {
-    Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
+    cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
                     (int)wp_index,
                     (int)cmd->id,
                     (int)cmd->options,
@@ -346,14 +346,14 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     delay(1000);
-    Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
+    cliSerial->printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
 
     while(1) {
 
         if (Serial3.available())
             Serial3.write(Serial3.read());
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -366,18 +366,18 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
     print_hit_enter();
     delay(1000);
 
-    Serial.printf_P(PSTR("Control CH "));
+    cliSerial->printf_P(PSTR("Control CH "));
 
-    Serial.println(FLIGHT_MODE_CHANNEL, DEC);
+    cliSerial->println(FLIGHT_MODE_CHANNEL, DEC);
 
     while(1) {
         delay(20);
         byte switchPosition = readSwitch();
         if (oldSwitchPosition != switchPosition) {
-            Serial.printf_P(PSTR("Position %d\n"),  (int)switchPosition);
+            cliSerial->printf_P(PSTR("Position %d\n"),  (int)switchPosition);
             oldSwitchPosition = switchPosition;
         }
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -389,20 +389,20 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_logging(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.println_P(PSTR("Testing dataflash logging"));
+    cliSerial->println_P(PSTR("Testing dataflash logging"));
     if (!DataFlash.CardInserted()) {
-        Serial.println_P(PSTR("ERR: No dataflash inserted"));
+        cliSerial->println_P(PSTR("ERR: No dataflash inserted"));
         return 0;
     }
     DataFlash.ReadManufacturerID();
-    Serial.printf_P(PSTR("Manufacturer: 0x%02x   Device: 0x%04x\n"),
+    cliSerial->printf_P(PSTR("Manufacturer: 0x%02x   Device: 0x%04x\n"),
                     (unsigned)DataFlash.df_manufacturer,
                     (unsigned)DataFlash.df_device);
-    Serial.printf_P(PSTR("NumPages: %u  PageSize: %u\n"),
+    cliSerial->printf_P(PSTR("NumPages: %u  PageSize: %u\n"),
                     (unsigned)DataFlash.df_NumPages+1,
                     (unsigned)DataFlash.df_PageSize);
     DataFlash.StartRead(DataFlash.df_NumPages+1);
-    Serial.printf_P(PSTR("Format version: %lx  Expected format version: %lx\n"),
+    cliSerial->printf_P(PSTR("Format version: %lx  Expected format version: %lx\n"),
                     (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT);
     return 0;
 }
@@ -419,14 +419,14 @@ test_adc(uint8_t argc, const Menu::arg *argv)
     print_hit_enter();
     adc.Init(&timer_scheduler);
     delay(1000);
-    Serial.printf_P(PSTR("ADC\n"));
+    cliSerial->printf_P(PSTR("ADC\n"));
     delay(1000);
 
     while(1) {
-        for (int16_t i=0; i<9; i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i));
-        Serial.println();
+        for (int16_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i));
+        cliSerial->println();
         delay(100);
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -449,15 +449,15 @@ test_gps(uint8_t argc, const Menu::arg *argv)
         g_gps->update();
 
         if (g_gps->new_data) {
-            Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
+            cliSerial->printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
                             (long)g_gps->latitude,
                             (long)g_gps->longitude,
                             (long)g_gps->altitude/100,
                             (int)g_gps->num_sats);
         }else{
-            Serial.printf_P(PSTR("."));
+            cliSerial->printf_P(PSTR("."));
         }
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -466,7 +466,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_ins(uint8_t argc, const Menu::arg *argv)
 {
-    //Serial.printf_P(PSTR("Calibrating."));
+    //cliSerial->printf_P(PSTR("Calibrating."));
     ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
     ahrs.reset();
 
@@ -496,14 +496,14 @@ test_ins(uint8_t argc, const Menu::arg *argv)
             // ---------------------
             Vector3f gyros  = ins.get_gyro();
             Vector3f accels = ins.get_accel();
-            Serial.printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
+            cliSerial->printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
                             (int)ahrs.roll_sensor / 100,
                             (int)ahrs.pitch_sensor / 100,
                             (uint16_t)ahrs.yaw_sensor / 100,
                             gyros.x, gyros.y, gyros.z,
                             accels.x, accels.y, accels.z);
         }
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -514,14 +514,14 @@ static int8_t
 test_mag(uint8_t argc, const Menu::arg *argv)
 {
     if (!g.compass_enabled) {
-        Serial.printf_P(PSTR("Compass: "));
+        cliSerial->printf_P(PSTR("Compass: "));
         print_enabled(false);
         return (0);
     }
 
     compass.set_orientation(MAG_ORIENTATION);
     if (!compass.init()) {
-        Serial.println_P(PSTR("Compass initialisation failed!"));
+        cliSerial->println_P(PSTR("Compass initialisation failed!"));
         return 0;
     }
     ahrs.set_compass(&compass);
@@ -534,7 +534,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
     int16_t counter = 0;
     float heading = 0;
 
-    //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
+    //cliSerial->printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
 
     print_hit_enter();
 
@@ -564,7 +564,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
             if (counter>20) {
                 if (compass.healthy) {
                     Vector3f maggy = compass.get_offsets();
-                    Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
+                    cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
                                     (wrap_360_cd(ToDeg(heading) * 100)) /100,
                                     (int)compass.mag_x,
                                     (int)compass.mag_y,
@@ -573,19 +573,19 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                                     maggy.y,
                                     maggy.z);
                 } else {
-                    Serial.println_P(PSTR("compass not healthy"));
+                    cliSerial->println_P(PSTR("compass not healthy"));
                 }
                 counter=0;
             }
         }
-        if (Serial.available() > 0) {
+        if (cliSerial->available() > 0) {
             break;
         }
     }
 
     // save offsets. This allows you to get sane offset values using
     // the CLI before you go flying.
-    Serial.println_P(PSTR("saving offsets"));
+    cliSerial->println_P(PSTR("saving offsets"));
     compass.save_offsets();
     return (0);
 }
@@ -601,26 +601,26 @@ static int8_t
 test_airspeed(uint8_t argc, const Menu::arg *argv)
 {
     float airspeed_ch = pitot_analog_source.read();
-    // Serial.println(pitot_analog_source.read());
-    Serial.printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch);
+    // cliSerial->println(pitot_analog_source.read());
+    cliSerial->printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch);
 
     if (!airspeed.enabled()) {
-        Serial.printf_P(PSTR("airspeed: "));
+        cliSerial->printf_P(PSTR("airspeed: "));
         print_enabled(false);
         return (0);
 
     }else{
         print_hit_enter();
         zero_airspeed();
-        Serial.printf_P(PSTR("airspeed: "));
+        cliSerial->printf_P(PSTR("airspeed: "));
         print_enabled(true);
 
         while(1) {
             delay(20);
             read_airspeed();
-            Serial.printf_P(PSTR("%.1f m/s\n"), airspeed.get_airspeed());
+            cliSerial->printf_P(PSTR("%.1f m/s\n"), airspeed.get_airspeed());
 
-            if(Serial.available() > 0) {
+            if(cliSerial->available() > 0) {
                 return (0);
             }
         }
@@ -631,7 +631,7 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_pressure(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
+    cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n"));
     print_hit_enter();
 
     home.alt        = 0;
@@ -643,14 +643,14 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
         current_loc.alt = read_barometer() + home.alt;
 
         if (!barometer.healthy) {
-            Serial.println_P(PSTR("not healthy"));
+            cliSerial->println_P(PSTR("not healthy"));
         } else {
-            Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
+            cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
                             current_loc.alt / 100.0,
                             barometer.get_pressure(), 0.1*barometer.get_temperature());
         }
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -673,7 +673,7 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
             Serial3.write(Serial1.read());
             digitalWrite(C_LED_PIN, LED_OFF);
         }
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }