diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde
index 7ca585df4c..b5f75bf440 100644
--- a/ArduCopter/AP_State.pde
+++ b/ArduCopter/AP_State.pde
@@ -143,6 +143,6 @@ void set_gps_healthy(bool b)
 
 void dump_state()
 {
-    Serial.printf("st: %u\n",ap.value);
-	//Serial.printf("%u\n", *(uint16_t*)&ap);
+    cliSerial->printf("st: %u\n",ap.value);
+	//cliSerial->printf("%u\n", *(uint16_t*)&ap);
 }
diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index ce2c2301f5..543cbab4a9 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -143,6 +143,9 @@ FastSerialPort0(Serial);        // FTDI/console
 FastSerialPort1(Serial1);       // GPS port
 FastSerialPort3(Serial3);       // Telemetry port
 
+// 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
@@ -1375,7 +1378,7 @@ static void super_slow_loop()
 #endif
 
     /*
-     *  //Serial.printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n",
+     *  //cliSerial->printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n",
      *                               current_loc.alt,
      *                               next_WP.alt,
      *                               altitude_error,
@@ -1502,7 +1505,7 @@ void update_yaw_mode(void)
 
     case YAW_AUTO:
         nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60);                 // 40 deg a second
-        //Serial.printf("nav_yaw %d ", nav_yaw);
+        //cliSerial->printf("nav_yaw %d ", nav_yaw);
         nav_yaw  = wrap_360(nav_yaw);
         get_stabilize_yaw(nav_yaw);
         break;
@@ -1941,7 +1944,7 @@ static void update_altitude_est()
         climb_rate += climb_rate_error;
         current_loc.alt += (climb_rate / 50);
     }
-    //Serial.printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt);
+    //cliSerial->printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt);
 }
 
 static void tuning(){
@@ -2154,7 +2157,7 @@ static void update_nav_wp()
         //int16_t angleTest = degrees(circle_angle);
         //int16_t nroll = nav_roll;
         //int16_t npitch = nav_pitch;
-        //Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d  \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll);
+        //cliSerial->printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d  \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll);
 
     }else if(wp_control == WP_MODE) {
         // calc error to target
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index cb5c0b44d0..e8aa2282ca 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -799,7 +799,7 @@ GCS_MAVLINK::update(void)
                 crlf_count = 0;
             }
             if (crlf_count == 3) {
-                run_cli();
+                run_cli(_port);
             }
         }
 #endif
@@ -894,7 +894,7 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_RAW_IMU1);
         send_message(MSG_RAW_IMU2);
         send_message(MSG_RAW_IMU3);
-        //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
+        //cliSerial->printf("mav1 %d\n", (int)streamRateRawSensors.get());
     }
 
     if (stream_trigger(STREAM_EXTENDED_STATUS)) {
@@ -918,29 +918,29 @@ GCS_MAVLINK::data_stream_send(void)
 
     if (stream_trigger(STREAM_POSITION)) {
         // sent with GPS read
-        //Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
+        //cliSerial->printf("mav3 %d\n", (int)streamRatePosition.get());
     }
 
     if (stream_trigger(STREAM_RAW_CONTROLLER)) {
         send_message(MSG_SERVO_OUT);
-        //Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
+        //cliSerial->printf("mav4 %d\n", (int)streamRateRawController.get());
     }
 
     if (stream_trigger(STREAM_RC_CHANNELS)) {
         send_message(MSG_RADIO_OUT);
         send_message(MSG_RADIO_IN);
-        //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
+        //cliSerial->printf("mav5 %d\n", (int)streamRateRCChannels.get());
     }
 
     if (stream_trigger(STREAM_EXTRA1)) {
         send_message(MSG_ATTITUDE);
         send_message(MSG_SIMSTATE);
-        //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
+        //cliSerial->printf("mav6 %d\n", (int)streamRateExtra1.get());
     }
 
     if (stream_trigger(STREAM_EXTRA2)) {
         send_message(MSG_VFR_HUD);
-        //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
+        //cliSerial->printf("mav7 %d\n", (int)streamRateExtra2.get());
     }
 
     if (stream_trigger(STREAM_EXTRA3)) {
@@ -1604,7 +1604,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (!waypoint_receiving) break;
 
 
-            //Serial.printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get());
+            //cliSerial->printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get());
 
             // check if this is the requested waypoint
             if (packet.seq != waypoint_request_i)
@@ -1828,8 +1828,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         // TODO: check scaling for temp/absPress
         float temp = 70;
         float absPress = 1;
-        //      Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
-        //      Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
+        //      cliSerial->printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
+        //      cliSerial->printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
 
         // rad/sec
         Vector3f gyros;
diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde
index b9c53b7514..fc9f93facd 100644
--- a/ArduCopter/Log.pde
+++ b/ArduCopter/Log.pde
@@ -22,7 +22,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"
@@ -69,45 +69,45 @@ 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{
-        if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Serial.printf_P(PSTR(" ATTITUDE_FAST"));
-        if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Serial.printf_P(PSTR(" ATTITUDE_MED"));
-        if (g.log_bitmask & MASK_LOG_GPS) Serial.printf_P(PSTR(" GPS"));
-        if (g.log_bitmask & MASK_LOG_PM) Serial.printf_P(PSTR(" PM"));
-        if (g.log_bitmask & MASK_LOG_CTUN) Serial.printf_P(PSTR(" CTUN"));
-        if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN"));
-        if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW"));
-        if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD"));
-        if (g.log_bitmask & MASK_LOG_CUR) Serial.printf_P(PSTR(" CURRENT"));
-        if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS"));
-        if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW"));
-        if (g.log_bitmask & MASK_LOG_PID) Serial.printf_P(PSTR(" PID"));
-        if (g.log_bitmask & MASK_LOG_ITERM) Serial.printf_P(PSTR(" ITERM"));
-        if (g.log_bitmask & MASK_LOG_INAV) Serial.printf_P(PSTR(" INAV"));
+        if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf_P(PSTR(" ATTITUDE_FAST"));
+        if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf_P(PSTR(" ATTITUDE_MED"));
+        if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf_P(PSTR(" GPS"));
+        if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf_P(PSTR(" PM"));
+        if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf_P(PSTR(" CTUN"));
+        if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf_P(PSTR(" NTUN"));
+        if (g.log_bitmask & MASK_LOG_RAW) cliSerial->printf_P(PSTR(" RAW"));
+        if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf_P(PSTR(" CMD"));
+        if (g.log_bitmask & MASK_LOG_CUR) cliSerial->printf_P(PSTR(" CURRENT"));
+        if (g.log_bitmask & MASK_LOG_MOTORS) cliSerial->printf_P(PSTR(" MOTORS"));
+        if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf_P(PSTR(" OPTFLOW"));
+        if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf_P(PSTR(" PID"));
+        if (g.log_bitmask & MASK_LOG_ITERM) cliSerial->printf_P(PSTR(" ITERM"));
+        if (g.log_bitmask & MASK_LOG_INAV) cliSerial->printf_P(PSTR(" INAV"));
     }
 
-    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%u logs\n"), (unsigned)num_logs);
+        cliSerial->printf_P(PSTR("\n%u logs\n"), (unsigned)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);
 }
@@ -127,28 +127,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 number %d,    start %d,   end %d\n"),
+    /*cliSerial->printf_P(PSTR("Dumping Log number %d,    start %d,   end %d\n"),
      *                         dump_log,
      *                         dump_log_start,
      *                         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);
 }
 
@@ -174,7 +174,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);
     }
 
@@ -278,13 +278,13 @@ static void Log_Read_GPS()
     int32_t temp8   = DataFlash.ReadLong();           // 8 ground course
 
     //  1   2    3      4     5      6      7    8
-    Serial.printf_P(PSTR("GPS, %ld, %d, "),
+    cliSerial->printf_P(PSTR("GPS, %ld, %d, "),
                     (long)temp1,                          // 1 time
                     (int)temp2);                          // 2 sats
     print_latlon(&Serial, temp3);
-    Serial.print_P(PSTR(", "));
+    cliSerial->print_P(PSTR(", "));
     print_latlon(&Serial, temp4);
-    Serial.printf_P(PSTR(", %4.4f, %4.4f, %d, %ld\n"),
+    cliSerial->printf_P(PSTR(", %4.4f, %4.4f, %d, %ld\n"),
                     temp5,                                // 5 gps alt
                     temp6,                                // 6 sensor alt
                     (int)temp7,                           // 7 ground speed
@@ -325,19 +325,19 @@ static void Log_Write_Raw()
 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 = get_float(DataFlash.ReadLong());
-        Serial.print(logvar);
-        Serial.print_P(PSTR(", "));
+        cliSerial->print(logvar);
+        cliSerial->print_P(PSTR(", "));
     }
-    Serial.println_P(PSTR(" "));
+    cliSerial->println_P(PSTR(" "));
 
 	/*
 	float temp1 = get_float(DataFlash.ReadLong());
 	float temp2 = get_float(DataFlash.ReadLong());
 
-	Serial.printf_P(PSTR("RAW, %4.4f, %4.4f\n"),
+	cliSerial->printf_P(PSTR("RAW, %4.4f, %4.4f\n"),
 			temp1,
 			temp2);
 	*/
@@ -370,7 +370,7 @@ static void Log_Read_Current()
     int16_t temp5 = DataFlash.ReadInt();                        // 5
 
     //  1    2    3      4      5
-    Serial.printf_P(PSTR("CURR, %d, %ld, %4.4f, %4.4f, %d\n"),
+    cliSerial->printf_P(PSTR("CURR, %d, %ld, %4.4f, %4.4f, %d\n"),
                     (int)temp1,
                     (long)temp2,
                     temp3,
@@ -448,7 +448,7 @@ static void Log_Read_Motors()
     int16_t temp5 = DataFlash.ReadInt();                        // 5
     int16_t temp6 = DataFlash.ReadInt();                        // 6
     // 1  2   3   4   5   6
-    Serial.printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d\n"),
+    cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d\n"),
                     (int)temp1,         //1
                     (int)temp2,         //2
                     (int)temp3,         //3
@@ -466,7 +466,7 @@ static void Log_Read_Motors()
     int16_t temp7 = DataFlash.ReadInt();                        // 7
     int16_t temp8 = DataFlash.ReadInt();                        // 8
     // 1   2   3   4   5   6   7   8
-    Serial.printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d, %d, %d\n"),
+    cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d, %d, %d, %d, %d\n"),
                     (int)temp1,         //1
                     (int)temp2,         //2
                     (int)temp3,         //3
@@ -483,7 +483,7 @@ static void Log_Read_Motors()
     int16_t temp4 = DataFlash.ReadInt();                        // 4
     int16_t temp5 = DataFlash.ReadInt();                        // 5
     // 1   2   3   4   5
-    Serial.printf_P(PSTR("MOT, %d, %d, %d, %d, %d\n"),
+    cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d, %d\n"),
                     (int)temp1,         //1
                     (int)temp2,         //2
                     (int)temp3,         //3
@@ -497,7 +497,7 @@ static void Log_Read_Motors()
     int16_t temp4 = DataFlash.ReadInt();                        // 4
 
     // 1   2   3   4
-    Serial.printf_P(PSTR("MOT, %d, %d, %d, %d\n"),
+    cliSerial->printf_P(PSTR("MOT, %d, %d, %d, %d\n"),
                     (int)temp1,         //1
                     (int)temp2,         //2
                     (int)temp3,         //3
@@ -538,7 +538,7 @@ static void Log_Read_Optflow()
     int32_t temp8   = DataFlash.ReadLong();                     // 8
     int32_t temp9   = DataFlash.ReadLong();                     // 9
 
-    Serial.printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %ld, %ld\n"),
+    cliSerial->printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %ld, %ld\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
@@ -577,15 +577,15 @@ static void Log_Read_Nav_Tuning()
 {
     int16_t temp;
 
-    Serial.printf_P(PSTR("NTUN, "));
+    cliSerial->printf_P(PSTR("NTUN, "));
 
     for(int8_t i = 1; i < 8; i++ ) {
         temp = DataFlash.ReadInt();
-        Serial.printf_P(PSTR("%d, "), (int)temp);
+        cliSerial->printf_P(PSTR("%d, "), (int)temp);
     }
     // read 8
     temp = DataFlash.ReadInt();
-    Serial.printf_P(PSTR("%d\n"), (int)temp);
+    cliSerial->printf_P(PSTR("%d\n"), (int)temp);
 }
 
 
@@ -615,15 +615,15 @@ static void Log_Read_Control_Tuning()
 {
     int16_t temp;
 
-    Serial.printf_P(PSTR("CTUN, "));
+    cliSerial->printf_P(PSTR("CTUN, "));
 
     for(uint8_t i = 1; i < 9; i++ ) {
         temp = DataFlash.ReadInt();
-        Serial.printf_P(PSTR("%d, "), (int)temp);
+        cliSerial->printf_P(PSTR("%d, "), (int)temp);
     }
     // read 9
     temp = DataFlash.ReadInt();
-    Serial.printf_P(PSTR("%d\n"), (int)temp);
+    cliSerial->printf_P(PSTR("%d\n"), (int)temp);
 }
 
 static void Log_Write_Iterm()
@@ -653,15 +653,15 @@ static void Log_Read_Iterm()
 {
     int16_t temp;
 
-    Serial.printf_P(PSTR("ITERM, "));
+    cliSerial->printf_P(PSTR("ITERM, "));
 
     for(uint8_t i = 1; i < 12; i++ ) {
         temp = DataFlash.ReadInt();
-        Serial.printf_P(PSTR("%d, "), (int)temp);
+        cliSerial->printf_P(PSTR("%d, "), (int)temp);
     }
     // read 12
     temp = DataFlash.ReadInt();
-    Serial.println((int)temp);
+    cliSerial->println((int)temp);
 }
 
 
@@ -691,7 +691,7 @@ static void Log_Read_Performance()
     uint32_t temp6  = DataFlash.ReadLong();
 
     //                         1   2   3   4   5    6
-    Serial.printf_P(PSTR("PM, %d, %d, %d, %u, %u, %lu\n"),
+    cliSerial->printf_P(PSTR("PM, %d, %d, %d, %u, %u, %lu\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
@@ -734,7 +734,7 @@ static void Log_Read_Cmd()
     int32_t temp8   = DataFlash.ReadLong();
 
     //  1   2    3   4   5   6   7    8
-    Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
+    cliSerial->printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
@@ -774,7 +774,7 @@ static void Log_Read_Attitude()
     uint16_t temp7  = DataFlash.ReadInt();
 
     // 1   2   3    4   5   6  7    8   9
-    Serial.printf_P(PSTR("ATT, %d, %d, %d, %d, %d, %u, %u\n"),
+    cliSerial->printf_P(PSTR("ATT, %d, %d, %d, %d, %d, %u, %u\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
@@ -837,7 +837,7 @@ static void Log_Read_INAV()
     float temp17    = get_float(DataFlash.ReadLong());  // 17 accel based lat velocity
     float temp18    = get_float(DataFlash.ReadLong());  // 18 accel based lon velocity
                               // 1   2   3   4      5      6      7      8      9     10     11     12   13   14   15       16     17     18
-    Serial.printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"),
+    cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
@@ -872,9 +872,9 @@ static void Log_Write_Mode(byte mode)
 // Read a mode packet
 static void Log_Read_Mode()
 {
-    Serial.printf_P(PSTR("MOD:"));
+    cliSerial->printf_P(PSTR("MOD:"));
     print_flight_mode(DataFlash.ReadByte());
-    Serial.printf_P(PSTR(", %d\n"),(int)DataFlash.ReadInt());
+    cliSerial->printf_P(PSTR(", %d\n"),(int)DataFlash.ReadInt());
 }
 
 // Write Startup packet. Total length : 4 bytes
@@ -889,7 +889,7 @@ static void Log_Write_Startup()
 // Read a startup packet
 static void Log_Read_Startup()
 {
-    Serial.printf_P(PSTR("START UP\n"));
+    cliSerial->printf_P(PSTR("START UP\n"));
 }
 
 #define DATA_INT32 0
@@ -960,23 +960,23 @@ static void Log_Read_Data()
     int8_t _type = DataFlash.ReadByte();
 
     if(_type == DATA_EVENT) {
-        Serial.printf_P(PSTR("EV: %u\n"), _index);
+        cliSerial->printf_P(PSTR("EV: %u\n"), _index);
 
     }else if(_type == DATA_FLOAT) {
         float _value = get_float(DataFlash.ReadLong());
-        Serial.printf_P(PSTR("DATA: %u, %1.6f\n"), _index, _value);
+        cliSerial->printf_P(PSTR("DATA: %u, %1.6f\n"), _index, _value);
 
     }else if(_type == DATA_INT16) {
         int16_t _value = DataFlash.ReadInt();
-        Serial.printf_P(PSTR("DATA: %u, %d\n"), _index, _value);
+        cliSerial->printf_P(PSTR("DATA: %u, %d\n"), _index, _value);
 
     }else if(_type == DATA_UINT16) {
         uint16_t _value = DataFlash.ReadInt();
-        Serial.printf_P(PSTR("DATA: %u, %u\n"), _index, _value);
+        cliSerial->printf_P(PSTR("DATA: %u, %u\n"), _index, _value);
 
     }else if(_type == DATA_INT32) {
         int32_t _value = DataFlash.ReadLong();
-        Serial.printf_P(PSTR("DATA: %u, %ld\n"), _index, _value);
+        cliSerial->printf_P(PSTR("DATA: %u, %ld\n"), _index, _value);
     }
 }
 
@@ -1010,7 +1010,7 @@ static void Log_Read_PID()
     float temp7     = DataFlash.ReadLong() / 1000.f;                    // gain
 
     //  1    2    3    4    5    6      7
-    Serial.printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"),
+    cliSerial->printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"),
                     (int)temp1,         // pid id
                     (long)temp2,                // error
                     (long)temp3,                // p
@@ -1049,7 +1049,7 @@ static void Log_Read_DMP()
     uint16_t temp6  = DataFlash.ReadInt();
 
                              // 1   2   3   4   5   6
-    Serial.printf_P(PSTR("DMP, %d, %d, %d, %d, %u, %u\n"),
+    cliSerial->printf_P(PSTR("DMP, %d, %d, %d, %d, %u, %u\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
@@ -1064,19 +1064,19 @@ 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"),
                     (unsigned) memcheck_available_memory());
 
  #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
-    Serial.printf_P(PSTR("APM 2\n"));
+    cliSerial->printf_P(PSTR("APM 2\n"));
  #elif  CONFIG_APM_HARDWARE == APM2_BETA_HARDWARE
-    Serial.printf_P(PSTR("APM 2Beta\n"));
+    cliSerial->printf_P(PSTR("APM 2Beta\n"));
  #else
-    Serial.printf_P(PSTR("APM 1\n"));
+    cliSerial->printf_P(PSTR("APM 1\n"));
  #endif
 
 #if CLI_ENABLED == ENABLED
@@ -1090,7 +1090,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
@@ -1119,7 +1119,7 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
 					log_step++;
 				else{
 					log_step = 0;
-					Serial.println_P(PSTR("."));
+					cliSerial->println_P(PSTR("."));
 				}
 				break;
 
@@ -1199,7 +1199,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"),data);
+				cliSerial->printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
 			}
 			log_step = 0;                   // Restart sequence: new packet...
 			break;
diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index 302a091e0e..7b7c740daa 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -465,18 +465,18 @@ 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);
         default_dead_zones();
-        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/ArduCopter/commands.pde b/ArduCopter/commands.pde
index b3ac35d65d..fe7e1da374 100644
--- a/ArduCopter/commands.pde
+++ b/ArduCopter/commands.pde
@@ -75,7 +75,7 @@ static void set_cmd_with_index(struct Location temp, int i)
 {
 
     i = constrain(i, 0, g.command_total.get());
-    //Serial.printf("set_command: %d with id: %d\n", i, temp.id);
+    //cliSerial->printf("set_command: %d with id: %d\n", i, temp.id);
 
     // store home as 0 altitude!!!
     // Home is always a MAV_CMD_NAV_WAYPOINT (16)
@@ -146,7 +146,7 @@ static void set_next_WP(struct Location *wp)
             prev_WP = current_loc;
     }
 
-    //Serial.printf("set_next_WP #%d, ", command_nav_index);
+    //cliSerial->printf("set_next_WP #%d, ", command_nav_index);
     //print_wp(&prev_WP, command_nav_index -1);
 
     // Load the next_WP slot
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index b4a18692f7..3edf384074 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -298,7 +298,7 @@ static void do_loiter_unlimited()
 {
     wp_control = LOITER_MODE;
 
-    //Serial.println("dloi ");
+    //cliSerial->println("dloi ");
     if(command_nav_queue.lat == 0) {
         set_next_WP(&current_loc);
         wp_control = LOITER_MODE;
@@ -466,7 +466,7 @@ static bool verify_nav_wp()
         if ((millis() - loiter_time) > loiter_time_max) {
             wp_verify_byte |= NAV_DELAY;
             //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
-            //Serial.println("vlt done");
+            //cliSerial->println("vlt done");
         }
     }
 
@@ -508,7 +508,7 @@ static bool verify_loiter_time()
 
 static bool verify_loiter_turns()
 {
-    //Serial.printf("loiter_sum: %d \n", loiter_sum);
+    //cliSerial->printf("loiter_sum: %d \n", loiter_sum);
     // have we rotated around the center enough times?
     // -----------------------------------------------
     if(abs(loiter_sum) > loiter_total) {
@@ -542,10 +542,10 @@ static bool verify_RTL()
 
 static void do_wait_delay()
 {
-    //Serial.print("dwd ");
+    //cliSerial->print("dwd ");
     condition_start = millis();
     condition_value = command_cond_queue.lat * 1000;     // convert to milliseconds
-    //Serial.println(condition_value,DEC);
+    //cliSerial->println(condition_value,DEC);
 }
 
 static void do_change_alt()
@@ -564,7 +564,7 @@ static void do_within_distance()
 
 static void do_yaw()
 {
-    //Serial.println("dyaw ");
+    //cliSerial->println("dyaw ");
     yaw_tracking = MAV_ROI_NONE;
 
     // target angle in degrees
@@ -624,19 +624,19 @@ static void do_yaw()
 
 static bool verify_wait_delay()
 {
-    //Serial.print("vwd");
+    //cliSerial->print("vwd");
     if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
-        //Serial.println("y");
+        //cliSerial->println("y");
         condition_value = 0;
         return true;
     }
-    //Serial.println("n");
+    //cliSerial->println("n");
     return false;
 }
 
 static bool verify_change_alt()
 {
-    //Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
+    //cliSerial->printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
     if ((int32_t)condition_start < next_WP.alt) {
         // we are going higer
         if(current_loc.alt > next_WP.alt) {
@@ -653,7 +653,7 @@ static bool verify_change_alt()
 
 static bool verify_within_distance()
 {
-    //Serial.printf("cond dist :%d\n", (int)condition_value);
+    //cliSerial->printf("cond dist :%d\n", (int)condition_value);
     if (wp_distance < condition_value) {
         condition_value = 0;
         return true;
@@ -663,7 +663,7 @@ static bool verify_within_distance()
 
 static bool verify_yaw()
 {
-    //Serial.printf("vyaw %d\n", (int)(nav_yaw/100));
+    //cliSerial->printf("vyaw %d\n", (int)(nav_yaw/100));
 
     if((millis() - command_yaw_start_time) > command_yaw_time) {
         // time out
@@ -674,7 +674,7 @@ static bool verify_yaw()
         // TO-DO: there's still a problem with Condition_yaw, it will do it two times(probably more) sometimes, if it hasn't reached the next waypoint yet.
         // it should only do it one time so there should be code here to prevent another Condition_Yaw.
 
-        //Serial.println("Y");
+        //cliSerial->println("Y");
         return true;
 
     }else{
@@ -689,7 +689,7 @@ static bool verify_yaw()
         }
         nav_yaw         = wrap_360(nav_yaw);
         auto_yaw        = nav_yaw;
-        //Serial.printf("ny %ld\n",nav_yaw);
+        //cliSerial->printf("ny %ld\n",nav_yaw);
         return false;
     }
 }
@@ -757,27 +757,27 @@ static void do_jump()
     // when in use, it contains the current remaining jumps
     static int8_t jump = -10;                                                                   // used to track loops in jump command
 
-    //Serial.printf("do Jump: %d\n", jump);
+    //cliSerial->printf("do Jump: %d\n", jump);
 
     if(jump == -10) {
-        //Serial.printf("Fresh Jump\n");
+        //cliSerial->printf("Fresh Jump\n");
         // we use a locally stored index for jump
         jump = command_cond_queue.lat;
     }
-    //Serial.printf("Jumps left: %d\n",jump);
+    //cliSerial->printf("Jumps left: %d\n",jump);
 
     if(jump > 0) {
-        //Serial.printf("Do Jump to %d\n",command_cond_queue.p1);
+        //cliSerial->printf("Do Jump to %d\n",command_cond_queue.p1);
         jump--;
         change_command(command_cond_queue.p1);
 
     } else if (jump == 0) {
-        //Serial.printf("Did last jump\n");
+        //cliSerial->printf("Did last jump\n");
         // we're done, move along
         jump = -11;
 
     } else if (jump == -1) {
-        //Serial.printf("jumpForever\n");
+        //cliSerial->printf("jumpForever\n");
         // repeat forever
         change_command(command_cond_queue.p1);
     }
diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde
index e57a764859..edc463e7d8 100644
--- a/ArduCopter/commands_process.pde
+++ b/ArduCopter/commands_process.pde
@@ -4,14 +4,14 @@
 //----------------------------------------
 static void change_command(uint8_t cmd_index)
 {
-    //Serial.printf("change_command: %d\n",cmd_index );
+    //cliSerial->printf("change_command: %d\n",cmd_index );
     // limit range
     cmd_index = min(g.command_total - 1, cmd_index);
 
     // load command
     struct Location temp = get_cmd_with_index(cmd_index);
 
-    //Serial.printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);
+    //cliSerial->printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);
 
     // verify it's a nav command
     if(temp.id > MAV_CMD_NAV_LAST) {
@@ -32,12 +32,12 @@ static void change_command(uint8_t cmd_index)
 // --------------------
 static void update_commands()
 {
-    //Serial.printf("update_commands: %d\n",increment );
+    //cliSerial->printf("update_commands: %d\n",increment );
     // A: if we do not have any commands there is nothing to do
     // B: We have completed the mission, don't redo the mission
     // XXX debug
     //uint8_t tmp = g.command_index.get();
-    //Serial.printf("command_index %u \n", tmp);
+    //cliSerial->printf("command_index %u \n", tmp);
 
     if(g.command_total <= 1 || g.command_index >= 255)
         return;
@@ -172,7 +172,7 @@ static void execute_nav_command(void)
 static void verify_commands(void)
 {
     if(verify_must()) {
-        //Serial.printf("verified must cmd %d\n" , command_nav_index);
+        //cliSerial->printf("verified must cmd %d\n" , command_nav_index);
         command_nav_queue.id    = NO_COMMAND;
 
         // store our most recent executed nav command
@@ -183,11 +183,11 @@ static void verify_commands(void)
         command_cond_queue.id   = NO_COMMAND;
 
     }else{
-        //Serial.printf("verified must false %d\n" , command_nav_index);
+        //cliSerial->printf("verified must false %d\n" , command_nav_index);
     }
 
     if(verify_may()) {
-        //Serial.printf("verified may cmd %d\n" , command_cond_index);
+        //cliSerial->printf("verified may cmd %d\n" , command_cond_index);
         command_cond_queue.id = NO_COMMAND;
     }
 }
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 0aa6d0fe7a..512755051f 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -96,7 +96,7 @@ static void init_arm_motors()
     // disable failsafe because initialising everything takes a while
     failsafe_disable();
 
-    //Serial.printf("\nARM\n");
+    //cliSerial->printf("\nARM\n");
 #if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD)
     gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
 #endif
@@ -104,7 +104,7 @@ static void init_arm_motors()
     // we don't want writes to the serial port to cause us to pause
     // mid-flight, so set the serial ports non-blocking once we arm
     // the motors
-    Serial.set_blocking_writes(false);
+    cliSerial->set_blocking_writes(false);
     if (gcs3.initialised) {
         Serial3.set_blocking_writes(false);
     }
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index d573fd1cfb..743b9bcf90 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -301,7 +301,7 @@ static void run_navigation_contollers()
         if(home_distance > SUPER_SIMPLE_RADIUS) {        // 10m from home
             // we reset the angular offset to be a vector from home to the quad
             initial_simple_bearing = home_to_copter_bearing;
-            //Serial.printf("ISB: %d\n", initial_simple_bearing);
+            //cliSerial->printf("ISB: %d\n", initial_simple_bearing);
         }
     }
 
@@ -477,7 +477,7 @@ static void calc_nav_rate(int16_t max_speed)
 // We use the DCM's matrix to precalculate these trig values at 50hz
 static void calc_nav_pitch_roll()
 {
-    //Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
+    //cliSerial->printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
     // rotate the vector
     auto_roll       = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
     auto_pitch      = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde
index c1095b9da4..dc9a6ff9dd 100644
--- a/ArduCopter/radio.pde
+++ b/ArduCopter/radio.pde
@@ -89,14 +89,14 @@ static void init_rc_out()
             // send miinimum throttle out to ESC
             motors.output_min();
             // display message on console
-            Serial.printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
+            cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
             // block until we restart
             while(1) {
                 delay(200);
                 dancing_light();
             }
         }else{
-            Serial.printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
+            cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
             // clear esc flag
             g.esc_calibrate.set_and_save(0);
             // block until we restart
diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
index 3c8a05eed2..986f300e0b 100644
--- a/ArduCopter/sensors.pde
+++ b/ArduCopter/sensors.pde
@@ -39,7 +39,7 @@ static void init_compass()
     compass.set_orientation(MAG_ORIENTATION);                                                   // set compass's orientation on aircraft
     if (!compass.init() || !compass.read()) {
         // make sure we don't pass a broken compass to DCM
-        Serial.println_P(PSTR("COMPASS INIT ERROR"));
+        cliSerial->println_P(PSTR("COMPASS INIT ERROR"));
         return;
     }
     ahrs.set_compass(&compass);
@@ -53,7 +53,7 @@ static void init_optflow()
 #if OPTFLOW == ENABLED
     if( optflow.init(false, &timer_scheduler, &spi_semaphore, &spi3_semaphore) == false ) {
         g.optflow_enabled = false;
-        Serial.print_P(PSTR("\nFailed to Init OptFlow "));
+        cliSerial->print_P(PSTR("\nFailed to Init OptFlow "));
     }
     // suspend timer while we set-up SPI communication
     timer_scheduler.suspend_timer();
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index 213520e4ca..4b065a85e6 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -61,7 +61,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\n\n"));
+    cliSerial->printf_P(PSTR("Setup Mode\n\n\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"
@@ -70,9 +70,9 @@ setup_mode(uint8_t argc, const Menu::arg *argv)
 
     if(g.rc_1.radio_min >= 1300) {
         delay(1000);
-        Serial.printf_P(PSTR("\n!Warning, radio not configured!"));
+        cliSerial->printf_P(PSTR("\n!Warning, radio not configured!"));
         delay(1000);
-        Serial.printf_P(PSTR("\n Type 'radio' now.\n\n"));
+        cliSerial->printf_P(PSTR("\n Type 'radio' now.\n\n"));
     }
 
     // Run the setup menu.  When the menu exits, we will return to the main menu.
@@ -118,17 +118,17 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 {
     int16_t c;
 
-    Serial.printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n"));
+    cliSerial->printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n"));
 
     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("\nReboot APM"));
+    cliSerial->printf_P(PSTR("\nReboot APM"));
 
     delay(1000);
     //default_gains();
@@ -144,7 +144,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_radio(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.println_P(PSTR("\n\nRadio Setup:"));
+    cliSerial->println_P(PSTR("\n\nRadio Setup:"));
     uint8_t i;
 
     for(i = 0; i < 100; i++) {
@@ -154,7 +154,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 
     if(g.rc_1.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
         }
@@ -188,7 +188,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
     g.rc_8.radio_trim = 1500;
 
 
-    Serial.printf_P(PSTR("\nMove all controls to extremes. Enter to save: "));
+    cliSerial->printf_P(PSTR("\nMove all controls to extremes. Enter to save: "));
     while(1) {
 
         delay(20);
@@ -205,9 +205,9 @@ 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) {
+        if(cliSerial->available() > 0) {
             delay(20);
-            Serial.flush();
+            cliSerial->flush();
 
             g.rc_1.save_eeprom();
             g.rc_2.save_eeprom();
@@ -229,7 +229,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_motors(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.printf_P(PSTR(
+    cliSerial->printf_P(PSTR(
                         "Now connect the main lipo and follow the instruction on the wiki for your frame setup.\n"
                         "For security remember to disconnect the main lipo after the test, then hit any key to exit.\n"
                         "Any key to exit.\n"));
@@ -237,7 +237,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
         delay(20);
         read_radio();
         motors.output_test();
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             g.esc_calibrate.set_and_save(0);
             return(0);
         }
@@ -253,12 +253,37 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
     return(0);
 }
 
+/*
+  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);
-    ins.calibrate_accel(delay, flash_leds, gcs_send_text_fmt);
+    ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key);
     report_ins();
     return(0);
 }
@@ -275,7 +300,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
     } else if (!strcmp_P(argv[1].str, PSTR("v"))) {
         g.frame_orientation.set_and_save(V_FRAME);
     }else{
-        Serial.printf_P(PSTR("\nOp:[x,+,v]\n"));
+        cliSerial->printf_P(PSTR("\nOp:[x,+,v]\n"));
         report_frame();
         return 0;
     }
@@ -291,7 +316,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
     byte _oldSwitchPosition = 0;
     int8_t mode = 0;
 
-    Serial.printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
+    cliSerial->printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
     print_hit_enter();
 
     while(1) {
@@ -344,7 +369,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
         }
 
         // escape hatch
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             for (mode = 0; mode < 6; mode++)
                 flight_modes[mode].save();
 
@@ -377,7 +402,7 @@ setup_tune(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_range(uint8_t argc, const Menu::arg *argv)
 {
-    Serial.printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n"));
+    cliSerial->printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n"));
 
     g.radio_tuning_low.set_and_save(argv[1].i);
     g.radio_tuning_high.set_and_save(argv[2].i);
@@ -406,7 +431,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
         g.compass_enabled.set_and_save(false);
 
     }else{
-        Serial.printf_P(PSTR("\nOp:[on,off]\n"));
+        cliSerial->printf_P(PSTR("\nOp:[on,off]\n"));
         report_compass();
         return 0;
     }
@@ -426,7 +451,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("\nOp: off, 3-4"));
+        cliSerial->printf_P(PSTR("\nOp: off, 3-4"));
     }
 
     report_batt_monitor();
@@ -447,7 +472,7 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
         g.sonar_type.set_and_save(argv[1].i);
 
     }else{
-        Serial.printf_P(PSTR("\nOp:[on, off, 0-3]\n"));
+        cliSerial->printf_P(PSTR("\nOp:[on, off, 0-3]\n"));
         report_sonar();
         return 0;
     }
@@ -480,18 +505,18 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
     report_heli();
 
     // display help
-    Serial.printf_P(PSTR("Instructions:"));
+    cliSerial->printf_P(PSTR("Instructions:"));
     print_divider();
-    Serial.printf_P(PSTR("\td\t\tdisplay settings\n"));
-    Serial.printf_P(PSTR("\t1~4\t\tselect servo\n"));
-    Serial.printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
-    Serial.printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
-    Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
-    Serial.printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
-    Serial.printf_P(PSTR("\tr\t\treverse servo\n"));
-    Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n"));
-    Serial.printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
-    Serial.printf_P(PSTR("\tx\t\texit & save\n"));
+    cliSerial->printf_P(PSTR("\td\t\tdisplay settings\n"));
+    cliSerial->printf_P(PSTR("\t1~4\t\tselect servo\n"));
+    cliSerial->printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
+    cliSerial->printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
+    cliSerial->printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
+    cliSerial->printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
+    cliSerial->printf_P(PSTR("\tr\t\treverse servo\n"));
+    cliSerial->printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n"));
+    cliSerial->printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
+    cliSerial->printf_P(PSTR("\tx\t\texit & save\n"));
 
     // start capturing
     while( value != 'x' ) {
@@ -516,8 +541,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
             max_tail = max(g.rc_4.radio_out, max_tail);
         }
 
-        if( Serial.available() ) {
-            value = Serial.read();
+        if( cliSerial->available() ) {
+            value = cliSerial->read();
 
             // process the user's input
             switch( value ) {
@@ -541,7 +566,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
             case 'C':
                 if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
                     motors.collective_mid = g.rc_3.radio_out;
-                    Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_mid);
+                    cliSerial->printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_mid);
                 }
                 break;
             case 'd':
@@ -553,7 +578,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
             case 'M':
                 if( state == 0 ) {
                     state = 1;                          // switch to capture min/max mode
-                    Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"));
+                    cliSerial->printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"));
 
                     // reset servo ranges
                     motors.roll_max = motors.pitch_max = 4500;
@@ -572,7 +597,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
                     state = 0;                          // switch back to normal mode
                     // double check values aren't totally terrible
                     if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
-                        Serial.printf_P(PSTR("Invalid min/max captured roll:%d,  pitch:%d,  collective min: %d max: %d,  tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail);
+                        cliSerial->printf_P(PSTR("Invalid min/max captured roll:%d,  pitch:%d,  collective min: %d max: %d,  tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail);
                     else{
                         motors.roll_max = max_roll;
                         motors.pitch_max = max_pitch;
@@ -600,7 +625,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
                     if( active_servo == CH_3 )
                         motors.servo3_pos = temp;
                     motors.init_swash();
-                    Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
+                    cliSerial->printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
                 }
                 break;
             case 'r':
@@ -615,28 +640,28 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
                 if( temp > -500 && temp < 500 ) {
                     heli_get_servo(active_servo)->radio_trim = 1500 + temp;
                     motors.init_swash();
-                    Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
+                    cliSerial->printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
                 }
                 break;
             case 'u':
             case 'U':
                 temp = 0;
                 // delay up to 2 seconds for servo type from user
-                while( !Serial.available() && temp < 20 ) {
+                while( !cliSerial->available() && temp < 20 ) {
                     temp++;
                     delay(100);
                 }
-                if( Serial.available() ) {
-                    value = Serial.read();
+                if( cliSerial->available() ) {
+                    value = cliSerial->read();
                     if( value == 'a' || value == 'A' ) {
                         g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS);
                         //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS;  // need to force this update to take effect immediately
-                        Serial.printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed);
+                        cliSerial->printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed);
                     }
                     if( value == 'd' || value == 'D' ) {
                         g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS);
                         //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS;  // need to force this update to take effect immediately
-                        Serial.printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed);
+                        cliSerial->printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed);
                     }
                 }
                 break;
@@ -696,7 +721,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
         motors.ext_gyro_gain.save();
 
     }else{
-        Serial.printf_P(PSTR("\nOp:[on, off] gain\n"));
+        cliSerial->printf_P(PSTR("\nOp:[on, off] gain\n"));
     }
 
     report_gyro();
@@ -724,7 +749,7 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
         g.optflow_enabled = false;
 
     }else{
-        Serial.printf_P(PSTR("\nOp:[on, off]\n"));
+        cliSerial->printf_P(PSTR("\nOp:[on, off]\n"));
         report_optflow();
         return 0;
     }
@@ -743,11 +768,11 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
 
 static void report_batt_monitor()
 {
-    Serial.printf_P(PSTR("\nBatt Mon:\n"));
+    cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
     print_divider();
     if(g.battery_monitoring == 0) print_enabled(false);
-    if(g.battery_monitoring == 3) Serial.printf_P(PSTR("volts"));
-    if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
+    if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("volts"));
+    if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("volts and cur"));
     print_blanks(2);
 }
 
@@ -766,39 +791,39 @@ static void report_wp(byte index = 255)
 
 static void report_sonar()
 {
-    Serial.printf_P(PSTR("Sonar\n"));
+    cliSerial->printf_P(PSTR("Sonar\n"));
     print_divider();
     print_enabled(g.sonar_enabled.get());
-    Serial.printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type);
+    cliSerial->printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type);
     print_blanks(2);
 }
 
 static void report_frame()
 {
-    Serial.printf_P(PSTR("Frame\n"));
+    cliSerial->printf_P(PSTR("Frame\n"));
     print_divider();
 
  #if FRAME_CONFIG == QUAD_FRAME
-    Serial.printf_P(PSTR("Quad frame\n"));
+    cliSerial->printf_P(PSTR("Quad frame\n"));
  #elif FRAME_CONFIG == TRI_FRAME
-    Serial.printf_P(PSTR("TRI frame\n"));
+    cliSerial->printf_P(PSTR("TRI frame\n"));
  #elif FRAME_CONFIG == HEXA_FRAME
-    Serial.printf_P(PSTR("Hexa frame\n"));
+    cliSerial->printf_P(PSTR("Hexa frame\n"));
  #elif FRAME_CONFIG == Y6_FRAME
-    Serial.printf_P(PSTR("Y6 frame\n"));
+    cliSerial->printf_P(PSTR("Y6 frame\n"));
  #elif FRAME_CONFIG == OCTA_FRAME
-    Serial.printf_P(PSTR("Octa frame\n"));
+    cliSerial->printf_P(PSTR("Octa frame\n"));
  #elif FRAME_CONFIG == HELI_FRAME
-    Serial.printf_P(PSTR("Heli frame\n"));
+    cliSerial->printf_P(PSTR("Heli frame\n"));
  #endif
 
  #if FRAME_CONFIG != HELI_FRAME
     if(g.frame_orientation == X_FRAME)
-        Serial.printf_P(PSTR("X mode\n"));
+        cliSerial->printf_P(PSTR("X mode\n"));
     else if(g.frame_orientation == PLUS_FRAME)
-        Serial.printf_P(PSTR("+ mode\n"));
+        cliSerial->printf_P(PSTR("+ mode\n"));
     else if(g.frame_orientation == V_FRAME)
-        Serial.printf_P(PSTR("V mode\n"));
+        cliSerial->printf_P(PSTR("V mode\n"));
  #endif
 
     print_blanks(2);
@@ -806,7 +831,7 @@ static void report_frame()
 
 static void report_radio()
 {
-    Serial.printf_P(PSTR("Radio\n"));
+    cliSerial->printf_P(PSTR("Radio\n"));
     print_divider();
     // radio
     print_radio_values();
@@ -815,7 +840,7 @@ static void report_radio()
 
 static void report_ins()
 {
-    Serial.printf_P(PSTR("INS\n"));
+    cliSerial->printf_P(PSTR("INS\n"));
     print_divider();
 
     print_gyro_offsets();
@@ -825,19 +850,19 @@ static void report_ins()
 
 static void report_compass()
 {
-    Serial.printf_P(PSTR("Compass\n"));
+    cliSerial->printf_P(PSTR("Compass\n"));
     print_divider();
 
     print_enabled(g.compass_enabled);
 
     // mag declination
-    Serial.printf_P(PSTR("Mag Dec: %4.4f\n"),
+    cliSerial->printf_P(PSTR("Mag Dec: %4.4f\n"),
                     degrees(compass.get_declination()));
 
     Vector3f offsets = compass.get_offsets();
 
     // mag offsets
-    Serial.printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f"),
+    cliSerial->printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f"),
                     offsets.x,
                     offsets.y,
                     offsets.z);
@@ -846,7 +871,7 @@ static void report_compass()
 
 static void report_flight_modes()
 {
-    Serial.printf_P(PSTR("Flight modes\n"));
+    cliSerial->printf_P(PSTR("Flight modes\n"));
     print_divider();
 
     for(int16_t i = 0; i < 6; i++ ) {
@@ -858,13 +883,13 @@ static void report_flight_modes()
 void report_optflow()
 {
  #if OPTFLOW == ENABLED
-    Serial.printf_P(PSTR("OptFlow\n"));
+    cliSerial->printf_P(PSTR("OptFlow\n"));
     print_divider();
 
     print_enabled(g.optflow_enabled);
 
     // field of view
-    //Serial.printf_P(PSTR("FOV: %4.0f\n"),
+    //cliSerial->printf_P(PSTR("FOV: %4.0f\n"),
     //						degrees(g.optflow_fov));
 
     print_blanks(2);
@@ -874,22 +899,22 @@ void report_optflow()
  #if FRAME_CONFIG == HELI_FRAME
 static void report_heli()
 {
-    Serial.printf_P(PSTR("Heli\n"));
+    cliSerial->printf_P(PSTR("Heli\n"));
     print_divider();
 
     // main servo settings
-    Serial.printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
-    Serial.printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)motors.servo1_pos, (int)motors._servo_1->radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse());
-    Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse());
-    Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse());
-    Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse());
+    cliSerial->printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
+    cliSerial->printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)motors.servo1_pos, (int)motors._servo_1->radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse());
+    cliSerial->printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse());
+    cliSerial->printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse());
+    cliSerial->printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse());
 
-    Serial.printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max);
-    Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max);
-    Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max);
+    cliSerial->printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max);
+    cliSerial->printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max);
+    cliSerial->printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max);
 
     // calculate and print servo rate
-    Serial.printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed);
+    cliSerial->printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed);
 
     print_blanks(2);
 }
@@ -897,12 +922,12 @@ static void report_heli()
 static void report_gyro()
 {
 
-    Serial.printf_P(PSTR("Gyro:\n"));
+    cliSerial->printf_P(PSTR("Gyro:\n"));
     print_divider();
 
     print_enabled( motors.ext_gyro_enabled );
     if( motors.ext_gyro_enabled )
-        Serial.printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain);
+        cliSerial->printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain);
 
     print_blanks(2);
 }
@@ -916,7 +941,7 @@ static void report_gyro()
 /*static void
  *  print_PID(PI * pid)
  *  {
- *       Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"),
+ *       cliSerial->printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"),
  *                                               pid->kP(),
  *                                               pid->kI(),
  *                                               (long)pid->imax());
@@ -926,32 +951,32 @@ static void report_gyro()
 static void
 print_radio_values()
 {
-    Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
-    Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
-    Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
-    Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
-    Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
-    Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
-    Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
-    //Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
+    cliSerial->printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
+    cliSerial->printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
+    cliSerial->printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
+    cliSerial->printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
+    cliSerial->printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
+    cliSerial->printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
+    cliSerial->printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
+    //cliSerial->printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
 }
 
 static void
 print_switch(byte p, byte m, bool b)
 {
-    Serial.printf_P(PSTR("Pos %d:\t"),p);
+    cliSerial->printf_P(PSTR("Pos %d:\t"),p);
     print_flight_mode(m);
-    Serial.printf_P(PSTR(",\t\tSimple: "));
+    cliSerial->printf_P(PSTR(",\t\tSimple: "));
     if(b)
-        Serial.printf_P(PSTR("ON\n"));
+        cliSerial->printf_P(PSTR("ON\n"));
     else
-        Serial.printf_P(PSTR("OFF\n"));
+        cliSerial->printf_P(PSTR("OFF\n"));
 }
 
 static void
 print_done()
 {
-    Serial.printf_P(PSTR("\nSaved\n"));
+    cliSerial->printf_P(PSTR("\nSaved\n"));
 }
 
 
@@ -959,13 +984,13 @@ static void zero_eeprom(void)
 {
     byte b = 0;
 
-    Serial.printf_P(PSTR("\nErasing EEPROM\n"));
+    cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
 
     for (uintptr_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
@@ -973,7 +998,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("A_off: %4.2f, %4.2f, %4.2f\tA_scale: %4.2f, %4.2f, %4.2f\n"),
+    cliSerial->printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\tA_scale: %4.2f, %4.2f, %4.2f\n"),
                     (float)accel_offsets.x,                           // Pitch
                     (float)accel_offsets.y,                           // Roll
                     (float)accel_offsets.z,                           // YAW
@@ -986,7 +1011,7 @@ static void
 print_gyro_offsets(void)
 {
     Vector3f gyro_offsets = ins.get_gyro_offsets();
-    Serial.printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
+    cliSerial->printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
                     (float)gyro_offsets.x,
                     (float)gyro_offsets.y,
                     (float)gyro_offsets.z);
@@ -1014,11 +1039,11 @@ static int16_t read_num_from_serial() {
     char data[5] = "";
 
     do {
-        if (Serial.available() == 0) {
+        if (cliSerial->available() == 0) {
             delay(10);
             timeout++;
         }else{
-            data[index] = Serial.read();
+            data[index] = cliSerial->read();
             timeout = 0;
             index++;
         }
@@ -1035,7 +1060,7 @@ print_blanks(int16_t num)
 {
     while(num > 0) {
         num--;
-        Serial.println("");
+        cliSerial->println("");
     }
 }
 
@@ -1044,11 +1069,11 @@ static bool
 wait_for_yes()
 {
     int c;
-    Serial.flush();
-    Serial.printf_P(PSTR("Y to save\n"));
+    cliSerial->flush();
+    cliSerial->printf_P(PSTR("Y to save\n"));
 
     do {
-        c = Serial.read();
+        c = cliSerial->read();
     } while (-1 == c);
 
     if (('y' == c) || ('Y' == c))
@@ -1061,18 +1086,18 @@ static void
 print_divider(void)
 {
     for (int i = 0; i < 40; i++) {
-        Serial.print_P(PSTR("-"));
+        cliSerial->print_P(PSTR("-"));
     }
-    Serial.println();
+    cliSerial->println();
 }
 
 static void print_enabled(bool b)
 {
     if(b)
-        Serial.print_P(PSTR("en"));
+        cliSerial->print_P(PSTR("en"));
     else
-        Serial.print_P(PSTR("dis"));
-    Serial.print_P(PSTR("abled\n"));
+        cliSerial->print_P(PSTR("dis"));
+    cliSerial->print_P(PSTR("abled\n"));
 }
 
 
@@ -1094,7 +1119,7 @@ static void print_wp(struct Location *cmd, byte index)
    	//float t1 = (float)cmd->lat / t7;
     //float t2 = (float)cmd->lng / t7;
 
-    Serial.printf_P(PSTR("cmd#: %d | %d, %d, %d, %ld, %ld, %ld\n"),
+    cliSerial->printf_P(PSTR("cmd#: %d | %d, %d, %d, %ld, %ld, %ld\n"),
                     index,
                     cmd->id,
                     cmd->options,
@@ -1104,7 +1129,7 @@ static void print_wp(struct Location *cmd, byte index)
                     cmd->lng);
 
 	/*
-    Serial.printf_P(PSTR("cmd#: %d id:%d op:%d p1:%d p2:%ld p3:%4.7f p4:%4.7f \n"),
+    cliSerial->printf_P(PSTR("cmd#: %d id:%d op:%d p1:%d p2:%ld p3:%4.7f p4:%4.7f \n"),
                     (int)index,
                     (int)cmd->id,
                     (int)cmd->options,
@@ -1117,7 +1142,7 @@ static void print_wp(struct Location *cmd, byte index)
 
 static void report_version()
 {
-    Serial.printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);
+    cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);
     print_divider();
     print_blanks(2);
 }
@@ -1125,14 +1150,14 @@ static void report_version()
 
 static void report_tuning()
 {
-    Serial.printf_P(PSTR("\nTUNE:\n"));
+    cliSerial->printf_P(PSTR("\nTUNE:\n"));
     print_divider();
     if (g.radio_tuning == 0) {
         print_enabled(g.radio_tuning.get());
     }else{
         float low  = (float)g.radio_tuning_low.get() / 1000;
         float high = (float)g.radio_tuning_high.get() / 1000;
-        Serial.printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high);
+        cliSerial->printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high);
     }
     print_blanks(2);
 }
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 86ec7dd89f..6a10d1ca47 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -17,7 +17,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\n"
                          "  setup\n"
                          "  test\n"
@@ -42,8 +42,12 @@ 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)
 {
+    cliSerial = port;
+    Menu::set_port(port);
+    port->set_blocking_writes(true);
+
     while (1) {
         main_menu.run();
     }
@@ -76,7 +80,7 @@ static void init_ardupilot()
     // The console port buffers are defined to be sufficiently large to support
     // the MAVLink protocol efficiently
     //
-    Serial.begin(SERIAL0_BAUD, 128, 256);
+    cliSerial->begin(SERIAL0_BAUD, 128, 256);
 
     // GPS serial port.
     //
@@ -86,7 +90,7 @@ static void init_ardupilot()
     Serial1.begin(38400, 256, 16);
 #endif
 
-    Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
                          "\n\nFree RAM: %u\n"),
                     memcheck_available_memory());
 
@@ -164,7 +168,7 @@ static void init_ardupilot()
     if (!ap_system.usb_connected) {
         // we are not connected via USB, re-init UART0 with right
         // baud rate
-        Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
+        cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
     }
 #else
     // we have a 2nd serial port for telemetry
@@ -257,11 +261,15 @@ static void init_ardupilot()
     //
     if (check_startup_for_CLI()) {
         digitalWrite(A_LED_PIN, LED_ON);                        // turn on setup-mode LED
-        Serial.printf_P(PSTR("\nCLI:\n\n"));
-        run_cli();
+        cliSerial->printf_P(PSTR("\nCLI:\n\n"));
+        run_cli(cliSerial);
     }
 #else
-    Serial.printf_P(PSTR("\nPress ENTER 3 times for CLI\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
 #endif // CLI_ENABLED
 
 #if HIL_MODE != HIL_MODE_ATTITUDE
@@ -328,7 +336,7 @@ static void init_ardupilot()
 
 #endif
 
-    Serial.print_P(PSTR("\nReady to FLY "));
+    cliSerial->print_P(PSTR("\nReady to FLY "));
 }
 
 
@@ -409,7 +417,7 @@ static void set_mode(byte mode)
     // have we achieved the proper altitude before RTL is enabled
     set_rtl_reached_alt(false);
     // debug to Serial terminal
-    //Serial.println(flight_mode_strings[control_mode]);
+    //cliSerial->println(flight_mode_strings[control_mode]);
 
     ap.loiter_override  = false;
 
@@ -603,7 +611,7 @@ static void update_throttle_cruise(int16_t tmp)
 
     // recalc kp
     //g.pid_throttle.kP((float)g.throttle_cruise.get() / 981.0);
-    //Serial.printf("kp:%1.4f\n",kp);
+    //cliSerial->printf("kp:%1.4f\n",kp);
 }
 
 #if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED
@@ -630,7 +638,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;
 }
 
@@ -645,9 +653,9 @@ static void check_usb_mux(void)
     // the user has switched to/from the telemetry port
     ap_system.usb_connected = usb_check;
     if (ap_system.usb_connected) {
-        Serial.begin(SERIAL0_BAUD);
+        cliSerial->begin(SERIAL0_BAUD);
     } else {
-        Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
+        cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
     }
 }
 #endif
@@ -681,46 +689,46 @@ print_flight_mode(uint8_t mode)
 {
     switch (mode) {
     case STABILIZE:
-        Serial.print_P(PSTR("STABILIZE"));
+        cliSerial->print_P(PSTR("STABILIZE"));
         break;
     case ACRO:
-        Serial.print_P(PSTR("ACRO"));
+        cliSerial->print_P(PSTR("ACRO"));
         break;
     case ALT_HOLD:
-        Serial.print_P(PSTR("ALT_HOLD"));
+        cliSerial->print_P(PSTR("ALT_HOLD"));
         break;
     case AUTO:
-        Serial.print_P(PSTR("AUTO"));
+        cliSerial->print_P(PSTR("AUTO"));
         break;
     case GUIDED:
-        Serial.print_P(PSTR("GUIDED"));
+        cliSerial->print_P(PSTR("GUIDED"));
         break;
     case LOITER:
-        Serial.print_P(PSTR("LOITER"));
+        cliSerial->print_P(PSTR("LOITER"));
         break;
     case RTL:
-        Serial.print_P(PSTR("RTL"));
+        cliSerial->print_P(PSTR("RTL"));
         break;
     case CIRCLE:
-        Serial.print_P(PSTR("CIRCLE"));
+        cliSerial->print_P(PSTR("CIRCLE"));
         break;
     case POSITION:
-        Serial.print_P(PSTR("POSITION"));
+        cliSerial->print_P(PSTR("POSITION"));
         break;
     case LAND:
-        Serial.print_P(PSTR("LAND"));
+        cliSerial->print_P(PSTR("LAND"));
         break;
     case OF_LOITER:
-        Serial.print_P(PSTR("OF_LOITER"));
+        cliSerial->print_P(PSTR("OF_LOITER"));
         break;
     case TOY_M:
-        Serial.print_P(PSTR("TOY_M"));
+        cliSerial->print_P(PSTR("TOY_M"));
         break;
     case TOY_A:
-        Serial.print_P(PSTR("TOY_A"));
+        cliSerial->print_P(PSTR("TOY_A"));
         break;
     default:
-        Serial.print_P(PSTR("---"));
+        cliSerial->print_P(PSTR("---"));
         break;
     }
 }
diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde
index 1f94de5e2c..d129d14017 100644
--- a/ArduCopter/test.pde
+++ b/ArduCopter/test.pde
@@ -44,7 +44,7 @@ extern void             print_latlon(BetterStream *s, int32_t lat_or_lon);
 // printf_P is a version of printf that reads from flash memory
 /*static int8_t	help_test(uint8_t argc,             const Menu::arg *argv)
  *  {
- *       Serial.printf_P(PSTR("\n"
+ *       cliSerial->printf_P(PSTR("\n"
  *                                                "Commands:\n"
  *                                                "  radio\n"
  *                                                "  servos\n"
@@ -96,7 +96,7 @@ 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;
 }
@@ -108,10 +108,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);
 }
@@ -137,7 +137,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
         // servo Yaw
         //APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
 
-        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"),
                         g.rc_1.radio_in,
                         g.rc_2.radio_in,
                         g.rc_3.radio_in,
@@ -147,7 +147,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
                         g.rc_7.radio_in,
                         g.rc_8.radio_in);
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -170,13 +170,13 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
  *               g.rc_4.servo_out = g.rc_4.control_in;
  *               g.rc_4.calc_pwm();
  *
- *               Serial.printf_P(PSTR("input: %d\toutput%d\n"),
+ *               cliSerial->printf_P(PSTR("input: %d\toutput%d\n"),
  *                                                       g.rc_4.control_in,
  *                                                       g.rc_4.radio_out);
  *
  *               APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out);
  *
- *               if(Serial.available() > 0){
+ *               if(cliSerial->available() > 0){
  *                       return (0);
  *               }
  *       }
@@ -191,7 +191,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
 
  	for(altitude_error = 2000; altitude_error > -100; altitude_error--){
  		int16_t temp = get_desired_climb_rate();
-		Serial.printf("%ld, %d\n", altitude_error, temp);
+		cliSerial->printf("%ld, %d\n", altitude_error, temp);
 	}
  	return 0;
 }
@@ -202,7 +202,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
 	 	int32_t temp = 2 * 100 * (wp_distance - g.waypoint_radius * 100);
 		max_speed = sqrt((float)temp);
 		max_speed = min(max_speed, g.waypoint_speed_max);
-		Serial.printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance);
+		cliSerial->printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance);
 	 	wp_distance += 100;
 	}
  	return 0;
@@ -220,17 +220,17 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
  *       g.toy_yaw_rate = 3;
  *       yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
  *       roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
- *       Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
+ *       cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
  *
  *       g.toy_yaw_rate = 2;
  *       yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
  *       roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
- *       Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
+ *       cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
  *
  *       g.toy_yaw_rate = 1;
  *       yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
  *       roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40;
- *       Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
+ *       cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate);
  *  }*/
 
 static int8_t
@@ -244,7 +244,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
         read_radio();
 
 
-        Serial.printf_P(PSTR("IN  1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
+        cliSerial->printf_P(PSTR("IN  1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
                         g.rc_1.control_in,
                         g.rc_2.control_in,
                         g.rc_3.control_in,
@@ -253,9 +253,9 @@ test_radio(uint8_t argc, const Menu::arg *argv)
                         g.rc_6.control_in,
                         g.rc_7.control_in);
 
-        //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
+        //cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
 
-        /*Serial.printf_P(PSTR(	"min: %d"
+        /*cliSerial->printf_P(PSTR(	"min: %d"
          *                                               "\t in: %d"
          *                                               "\t pwm_in: %d"
          *                                               "\t sout: %d"
@@ -267,7 +267,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
          *                                               g.rc_3.pwm_out
          *                                               );
          */
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -288,7 +288,7 @@ test_radio(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.rc_3.control_in > 0){
  *               delay(20);
  *               read_radio();
@@ -299,27 +299,27 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  *               read_radio();
  *
  *               if(g.rc_3.control_in > 0){
- *                       Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in);
+ *                       cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in);
  *                       fail_test++;
  *               }
  *
  *               if(oldSwitchPosition != readSwitch()){
- *                       Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
- *                       Serial.println(flight_mode_strings[readSwitch()]);
+ *                       cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
+ *                       cliSerial->println(flight_mode_strings[readSwitch()]);
  *                       fail_test++;
  *               }
  *
  *               if(g.throttle_fs_enabled && g.rc_3.get_failsafe()){
- *                       Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
- *                       Serial.println(flight_mode_strings[readSwitch()]);
+ *                       cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
+ *                       cliSerial->println(flight_mode_strings[readSwitch()]);
  *                       fail_test++;
  *               }
  *
  *               if(fail_test > 0){
  *                       return (0);
  *               }
- *               if(Serial.available() > 0){
- *                       Serial.printf_P(PSTR("LOS caused no change in ACM.\n"));
+ *               if(cliSerial->available() > 0){
+ *                       cliSerial->printf_P(PSTR("LOS caused no change in ACM.\n"));
  *                       return (0);
  *               }
  *       }
@@ -344,8 +344,8 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  *       init_rc_in();
  *
  *       control_mode = STABILIZE;
- *       Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
- *       Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
+ *       cliSerial->printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
+ *       cliSerial->printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
  *
  *       motors.auto_armed(false);
  *       motors.armed(true);
@@ -394,7 +394,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  *                       ts_num++;
  *                       if (ts_num > 10){
  *                               ts_num = 0;
- *                               Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
+ *                               cliSerial->printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
  *                                       (int)(dcm.roll_sensor/100),
  *                                       (int)(dcm.pitch_sensor/100),
  *                                       g.rc_1.pwm_out);
@@ -403,10 +403,10 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  *                       }
  *                       // R: 1417,  L: 1453  F: 1453  B: 1417
  *
- *                       //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
- *                       //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
+ *                       //cliSerial->printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
+ *                       //cliSerial->printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
  *
- *                       if(Serial.available() > 0){
+ *                       if(cliSerial->available() > 0){
  *                               if(g.compass_enabled){
  *                                       compass.save_offsets();
  *                                       report_compass();
@@ -426,7 +426,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  *  //test_adc(uint8_t argc, const Menu::arg *argv)
  *  {
  *       print_hit_enter();
- *       Serial.printf_P(PSTR("ADC\n"));
+ *       cliSerial->printf_P(PSTR("ADC\n"));
  *       delay(1000);
  *
  *  adc.Init(&timer_scheduler);
@@ -435,11 +435,11 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  *
  *       while(1){
  *               for(int16_t i = 0; i < 9; i++){
- *                       Serial.printf_P(PSTR("%.1f,"),adc.Ch(i));
+ *                       cliSerial->printf_P(PSTR("%.1f,"),adc.Ch(i));
  *               }
- *               Serial.println();
+ *               cliSerial->println();
  *               delay(20);
- *               if(Serial.available() > 0){
+ *               if(cliSerial->available() > 0){
  *                       return (0);
  *               }
  *       }
@@ -457,7 +457,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
     Vector3f gyro, accel;
     float temp;
     print_hit_enter();
-    Serial.printf_P(PSTR("INS\n"));
+    cliSerial->printf_P(PSTR("INS\n"));
     delay(1000);
 
     ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
@@ -472,13 +472,13 @@ test_ins(uint8_t argc, const Menu::arg *argv)
 
         float test = sqrt(sq(accel.x) + sq(accel.y) + sq(accel.z)) / 9.80665;
 
-        Serial.printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"),
+        cliSerial->printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"),
             accel.x, accel.y, accel.z,
             gyro.x, gyro.y, gyro.z,
             temp, test);
 
         delay(40);
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -506,18 +506,18 @@ test_gps(uint8_t argc, const Menu::arg *argv)
         g_gps->update();
 
         if (g_gps->new_data) {
-            Serial.printf_P(PSTR("Lat: "));
+            cliSerial->printf_P(PSTR("Lat: "));
             print_latlon(&Serial, g_gps->latitude);
-            Serial.printf_P(PSTR(", Lon "));
+            cliSerial->printf_P(PSTR(", Lon "));
             print_latlon(&Serial, g_gps->longitude);
-            Serial.printf_P(PSTR(", Alt: %ldm, #sats: %d\n"),
+            cliSerial->printf_P(PSTR(", Alt: %ldm, #sats: %d\n"),
                             g_gps->altitude/100,
                             g_gps->num_sats);
             g_gps->new_data = false;
         }else{
-            Serial.print_P(PSTR("."));
+            cliSerial->print_P(PSTR("."));
         }
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -531,7 +531,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
  *  {
  *       print_hit_enter();
  *       delay(1000);
- *       Serial.printf_P(PSTR("Gyro | Accel\n"));
+ *       cliSerial->printf_P(PSTR("Gyro | Accel\n"));
  *       Vector3f   _cam_vector;
  *       Vector3f   _out_vector;
  *
@@ -548,7 +548,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
  *               Matrix3f temp = dcm.get_dcm_matrix();
  *               Matrix3f temp_t = dcm.get_dcm_transposed();
  *
- *               Serial.printf_P(PSTR("dcm\n"
+ *               cliSerial->printf_P(PSTR("dcm\n"
  *                                                        "%4.4f \t %4.4f \t %4.4f \n"
  *                                                        "%4.4f \t %4.4f \t %4.4f \n"
  *                                                        "%4.4f \t %4.4f \t %4.4f \n\n"),
@@ -559,18 +559,18 @@ test_gps(uint8_t argc, const Menu::arg *argv)
  *               int16_t _pitch         = degrees(-asin(temp.c.x));
  *               int16_t _roll      = degrees(atan2(temp.c.y, temp.c.z));
  *               int16_t _yaw       = degrees(atan2(temp.b.x, temp.a.x));
- *               Serial.printf_P(PSTR(	"angles\n"
+ *               cliSerial->printf_P(PSTR(	"angles\n"
  *                                                               "%d \t %d \t %d\n\n"),
  *                                                               _pitch,
  *                                                               _roll,
  *                                                               _yaw);
  *
  *               //_out_vector = _cam_vector * temp;
- *               //Serial.printf_P(PSTR(	"cam\n"
+ *               //cliSerial->printf_P(PSTR(	"cam\n"
  *               //						"%d \t %d \t %d\n\n"),
  *               //						(int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100);
  *
- *               if(Serial.available() > 0){
+ *               if(cliSerial->available() > 0){
  *                       return (0);
  *               }
  *       }
@@ -582,18 +582,18 @@ test_gps(uint8_t argc, const Menu::arg *argv)
  *  {
  *       print_hit_enter();
  *       delay(1000);
- *       Serial.printf_P(PSTR("Gyro | Accel\n"));
+ *       cliSerial->printf_P(PSTR("Gyro | Accel\n"));
  *       delay(1000);
  *
  *       while(1){
  *               Vector3f accels = dcm.get_accel();
- *               Serial.print("accels.z:");
- *               Serial.print(accels.z);
- *               Serial.print("omega.z:");
- *               Serial.print(omega.z);
+ *               cliSerial->print("accels.z:");
+ *               cliSerial->print(accels.z);
+ *               cliSerial->print("omega.z:");
+ *               cliSerial->print(omega.z);
  *               delay(100);
  *
- *               if(Serial.available() > 0){
+ *               if(cliSerial->available() > 0){
  *                       return (0);
  *               }
  *       }
@@ -608,7 +608,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
  *
  *       print_hit_enter();
  *       delay(1000);
- *       Serial.printf_P(PSTR("Omega"));
+ *       cliSerial->printf_P(PSTR("Omega"));
  *       delay(1000);
  *
  *       G_Dt = .02;
@@ -626,11 +626,11 @@ test_gps(uint8_t argc, const Menu::arg *argv)
  *               ts_num++;
  *               if (ts_num > 2){
  *                       ts_num = 0;
- *                       //Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz);
- *                       Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz);
+ *                       //cliSerial->printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz);
+ *                       cliSerial->printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz);
  *               }
  *
- *               if(Serial.available() > 0){
+ *               if(cliSerial->available() > 0){
  *                       return (0);
  *               }
  *       }
@@ -647,9 +647,9 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
         delay(200);
         read_radio();
         tuning();
-        Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value);
+        cliSerial->printf_P(PSTR("tune: %1.3f\n"), tuning_value);
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -662,12 +662,12 @@ test_battery(uint8_t argc, const Menu::arg *argv)
     print_test_disabled();
     return (0);
 #else
-    Serial.printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
-    Serial.flush();
-    while(!Serial.available()) {
+    cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
+    cliSerial->flush();
+    while(!cliSerial->available()) {
         delay(100);
     }
-    Serial.flush();
+    cliSerial->flush();
     print_hit_enter();
 
     // allow motors to spin
@@ -679,19 +679,19 @@ 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, Ah: %4.4f\n"),
+            cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
                             battery_voltage1,
                             current_amps1,
                             current_total1);
         }
         motors.throttle_pass_through();
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             motors.armed(false);
             return (0);
         }
@@ -712,17 +712,17 @@ static int8_t 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);
         }
     }
@@ -736,16 +736,16 @@ test_wp(uint8_t argc, const Menu::arg *argv)
     delay(1000);
 
     // save the alitude above home option
-    Serial.printf_P(PSTR("Hold alt "));
+    cliSerial->printf_P(PSTR("Hold alt "));
     if(g.RTL_altitude < 0) {
-        Serial.printf_P(PSTR("\n"));
+        cliSerial->printf_P(PSTR("\n"));
     }else{
-        Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
+        cliSerial->printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
     }
 
-    Serial.printf_P(PSTR("%d wp\n"), (int)g.command_total);
-    Serial.printf_P(PSTR("Hit rad: %dm\n"), (int)g.waypoint_radius);
-    //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
+    cliSerial->printf_P(PSTR("%d wp\n"), (int)g.command_total);
+    cliSerial->printf_P(PSTR("Hit rad: %dm\n"), (int)g.waypoint_radius);
+    //cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
 
     report_wp();
 
@@ -767,7 +767,7 @@ test_wp(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);
  *  }
  *  }
@@ -779,13 +779,13 @@ test_wp(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);
  *               }
  *       }
@@ -811,10 +811,10 @@ test_baro(uint8_t argc, const Menu::arg *argv)
         int16_t temp = barometer.get_temperature();
         int32_t raw_pres = barometer.get_raw_pressure();
         int32_t raw_temp = barometer.get_raw_temp();
-        Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC,"
+        cliSerial->printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC,"
                              " raw pres: %ld, raw temp: %ld\n"),
                         alt, pres,temp, raw_pres, raw_temp);
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -838,21 +838,21 @@ test_mag(uint8_t argc, const Menu::arg *argv)
             delay(100);
             if (compass.read()) {
                 float heading = compass.calculate_heading(ahrs.get_dcm_matrix());
-                Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
+                cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
                                 (wrap_360(ToDeg(heading) * 100)) /100,
                                 compass.mag_x,
                                 compass.mag_y,
                                 compass.mag_z);
             } else {
-                Serial.println_P(PSTR("not healthy"));
+                cliSerial->println_P(PSTR("not healthy"));
             }
 
-            if(Serial.available() > 0) {
+            if(cliSerial->available() > 0) {
                 return (0);
             }
         }
     } else {
-        Serial.printf_P(PSTR("Compass: "));
+        cliSerial->printf_P(PSTR("Compass: "));
         print_enabled(false);
         return (0);
     }
@@ -876,7 +876,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
  *               g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
  *               g.rc_4.servo_out = g.rc_4.control_in;
  *               g.rc_4.calc_pwm();
- *               Serial.printf_P(PSTR("PWM:%d input: %d\toutput%d "),
+ *               cliSerial->printf_P(PSTR("PWM:%d input: %d\toutput%d "),
  *                                                       APM_RC.InputCh(CH_4),
  *                                                       g.rc_4.control_in,
  *                                                       g.rc_4.radio_out);
@@ -887,13 +887,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
  *               g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
  *               g.rc_4.servo_out = g.rc_4.control_in;
  *               g.rc_4.calc_pwm();
- *               Serial.printf_P(PSTR("\trev input: %d\toutput%d\n"),
+ *               cliSerial->printf_P(PSTR("\trev input: %d\toutput%d\n"),
  *                                                       g.rc_4.control_in,
  *                                                       g.rc_4.radio_out);
  *
  *               APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
  *
- *               if(Serial.available() > 0){
+ *               if(cliSerial->available() > 0){
  *                       g.rc_4.set_reverse(0);
  *                       return (0);
  *               }
@@ -908,7 +908,7 @@ static int8_t
 test_sonar(uint8_t argc, const Menu::arg *argv)
 {
     if(g.sonar_enabled == false) {
-        Serial.printf_P(PSTR("Sonar disabled\n"));
+        cliSerial->printf_P(PSTR("Sonar disabled\n"));
         return (0);
     }
 
@@ -919,10 +919,10 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
     while(1) {
         delay(100);
 
-        Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read());
-        //Serial.printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value);
+        cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar.read());
+        //cliSerial->printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value);
 
-        if(Serial.available() > 0) {
+        if(cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -937,26 +937,26 @@ test_optflow(uint8_t argc, const Menu::arg *argv)
 {
 #if OPTFLOW == ENABLED
     if(g.optflow_enabled) {
-        Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
+        cliSerial->printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
         print_hit_enter();
 
         while(1) {
             delay(200);
             optflow.update(millis());
             Log_Write_Optflow();
-            Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
+            cliSerial->printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
                             optflow.x,
                             optflow.dx,
                             optflow.y,
                             optflow.dy,
                             optflow.surface_quality);
 
-            if(Serial.available() > 0) {
+            if(cliSerial->available() > 0) {
                 return (0);
             }
         }
     } else {
-        Serial.printf_P(PSTR("OptFlow: "));
+        cliSerial->printf_P(PSTR("OptFlow: "));
         print_enabled(false);
     }
     return (0);
@@ -979,7 +979,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
 
     // got 23506;, should be 22800
     update_navigation();
-    Serial.printf_P(PSTR("bear: %ld\n"), target_bearing);
+    cliSerial->printf_P(PSTR("bear: %ld\n"), target_bearing);
     return 0;
 }
 
@@ -994,20 +994,20 @@ test_logging(uint8_t argc, const Menu::arg *argv)
     print_test_disabled();
     return (0);
 #else
-    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;
 #endif
@@ -1077,12 +1077,12 @@ test_logging(uint8_t argc, const Menu::arg *argv)
 
 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 void print_test_disabled()
 {
-    Serial.printf_P(PSTR("Sorry, not 1280 compat.\n"));
+    cliSerial->printf_P(PSTR("Sorry, not 1280 compat.\n"));
 }
 
 /*
@@ -1110,7 +1110,7 @@ static void print_test_disabled()
  */
 /*
  *  //static void print_motor_out(){
- *       Serial.printf("out: R: %d,  L: %d  F: %d  B: %d\n",
+ *       cliSerial->printf("out: R: %d,  L: %d  F: %d  B: %d\n",
  *                               (motor_out[CH_1]   - g.rc_3.radio_min),
  *                               (motor_out[CH_2]   - g.rc_3.radio_min),
  *                               (motor_out[CH_3]   - g.rc_3.radio_min),
diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde
index 9778018451..bb5feed4db 100644
--- a/ArduCopter/toy.pde
+++ b/ArduCopter/toy.pde
@@ -156,7 +156,7 @@ void roll_pitch_toy()
 
 #elif TOY_MIXER == TOY_LINEAR_MIXER
     roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30;
-    //Serial.printf("roll_rate: %d\n",roll_rate);
+    //cliSerial->printf("roll_rate: %d\n",roll_rate);
     roll_rate = constrain(roll_rate, -2000, 2000);
 
 #elif TOY_MIXER == TOY_EXTERNAL_MIXER
@@ -179,4 +179,4 @@ void roll_pitch_toy()
     get_stabilize_pitch(g.rc_2.control_in);
 #endif
 
-}
\ No newline at end of file
+}