diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 620ce1514c..44ade31c04 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -134,6 +134,8 @@ FastSerialPort1(Serial1);       // GPS port
  FastSerialPort3(Serial3);       // Telemetry port for APM1
 #endif
 
+static FastSerial *cliSerial = &Serial3;
+
 // 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
@@ -812,13 +814,13 @@ static void medium_loop()
 			#endif
 //#endif
 /*{
-Serial.print(ahrs.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.print(ahrs.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.print(ahrs.yaw_sensor, DEC);	Serial.printf_P(PSTR("\t"));
+cliSerial->print(ahrs.roll_sensor, DEC);	cliSerial->printf_P(PSTR("\t"));
+cliSerial->print(ahrs.pitch_sensor, DEC);	cliSerial->printf_P(PSTR("\t"));
+cliSerial->print(ahrs.yaw_sensor, DEC);	cliSerial->printf_P(PSTR("\t"));
 Vector3f tempaccel = ins.get_accel();
-Serial.print(tempaccel.x, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.print(tempaccel.y, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.println(tempaccel.z, DEC);
+cliSerial->print(tempaccel.x, DEC);	cliSerial->printf_P(PSTR("\t"));
+cliSerial->print(tempaccel.y, DEC);	cliSerial->printf_P(PSTR("\t"));
+cliSerial->println(tempaccel.z, DEC);
 }*/
 
 			break;
@@ -939,11 +941,11 @@ static void slow_loop()
 #endif
 
 #if TRACE == ENABLED
-         //     Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), 
+         //     cliSerial->printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), 
          //           ahrs.yaw_sensor*0.01, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);           
-             // Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"), 
+             // cliSerial->printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"), 
                 //    g.command_total, g.command_index, nav_command_index);      
-    	   Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f,  sonar_dist = %d    obstacle = %d\n"), ahrs.yaw_sensor*0.01, (int)sonar_dist, obstacle);                
+    	   cliSerial->printf_P(PSTR("NAV->gnd_crs=%3.0f,  sonar_dist = %d    obstacle = %d\n"), ahrs.yaw_sensor*0.01, (int)sonar_dist, obstacle);                
 #endif                          
 			break;
 	}
diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index 7f7845f6aa..d1ca8e69a2 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -756,7 +756,7 @@ GCS_MAVLINK::update(void)
                 crlf_count = 0;
             }
             if (crlf_count == 3) {
-                run_cli();
+                run_cli(_port);
             }
         }
 #endif
@@ -1692,8 +1692,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: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
-                  //Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
+                  //cliSerial->printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
+                  //cliSerial->printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
 
             // rad/sec
             Vector3f gyros;
diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde
index 6297ff22a6..185055ef2f 100644
--- a/APMrover2/Log.pde
+++ b/APMrover2/Log.pde
@@ -23,7 +23,7 @@ static int16_t cur_throttle =0;
 // 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"
@@ -57,16 +57,16 @@ print_log_menu(void)
 	
 	uint16_t num_logs = DataFlash.get_num_logs();
 
-	Serial.printf_P(PSTR("logs enabled: "));
+	cliSerial->printf_P(PSTR("logs enabled: "));
 
 	if (0 == g.log_bitmask) {
-		Serial.printf_P(PSTR("none"));
+		cliSerial->printf_P(PSTR("none"));
 	}else{
 		// Macro to make the following code a bit easier on the eye.
 		// Pass it the capitalised name of the log option, as defined
 		// in defines.h but without the LOG_ prefix.  It will check for
 		// the bit being set and print the name of the log option to suit.
-		#define PLOG(_s)	if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
+		#define PLOG(_s)	if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(#_s))
 		PLOG(ATTITUDE_FAST);
 		PLOG(ATTITUDE_MED);
 		PLOG(GPS);
@@ -80,24 +80,24 @@ print_log_menu(void)
 		#undef PLOG
 	}
 
-	Serial.println();
+	cliSerial->println();
 
 	if (num_logs == 0) {
-		Serial.printf_P(PSTR("\nNo logs\n\n"));
+		cliSerial->printf_P(PSTR("\nNo logs\n\n"));
 	}else{
-		Serial.printf_P(PSTR("\n%d logs\n"), num_logs);
+		cliSerial->printf_P(PSTR("\n%d logs\n"), num_logs);
 
 		for(int i=num_logs;i>=1;i--) {
             int 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"), temp, log_start, log_end);
+			cliSerial->printf_P(PSTR("Log %d,    start %d,   end %d\n"), temp, log_start, 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);
 }
@@ -117,28 +117,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"), count);
-			Serial.printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());
-			Serial.printf_P(PSTR("%d\n"), DataFlash.GetFilePage());
+			cliSerial->printf_P(PSTR("DF page, log file #, log page: %d,\t"), count);
+			cliSerial->printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());
+			cliSerial->printf_P(PSTR("%d\n"), DataFlash.GetFilePage());
 		}
 		return(-1);
 	} else if (dump_log <= 0) {
-		Serial.printf_P(PSTR("dumping all\n"));
+		cliSerial->printf_P(PSTR("dumping all\n"));
 		Log_Read(1, DataFlash.df_NumPages);
 		return(-1);
 	} else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) {
-		Serial.printf_P(PSTR("bad log number\n"));
+		cliSerial->printf_P(PSTR("bad log number\n"));
 		return(-1);
 	}
 
 	DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
-	Serial.printf_P(PSTR("Dumping Log %d,    start pg %d,   end pg %d\n"),
+	cliSerial->printf_P(PSTR("Dumping Log %d,    start pg %d,   end pg %d\n"),
 				  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;
 }
 
@@ -146,15 +146,15 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 void erase_callback(unsigned long t) {
     mavlink_delay(t);
     if (DataFlash.GetWritePage() % 128 == 0) {
-        Serial.printf_P(PSTR("+"));
+        cliSerial->printf_P(PSTR("+"));
     }
 }
 
 static void do_erase_logs(void)
 {
-	Serial.printf_P(PSTR("\nErasing log...\n"));
+	cliSerial->printf_P(PSTR("\nErasing log...\n"));
     DataFlash.EraseAll(erase_callback);
-	Serial.printf_P(PSTR("\nLog erased.\n"));
+	cliSerial->printf_P(PSTR("\nLog erased.\n"));
 }
 
 static int8_t
@@ -172,7 +172,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);
 	}
 
@@ -400,7 +400,7 @@ static void Log_Write_Current()
 // Read a Current packet
 static void Log_Read_Current()
 {
-	Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
+	cliSerial->printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
 			DataFlash.ReadInt(),
 			((float)DataFlash.ReadInt() / 100.f),
 			((float)DataFlash.ReadInt() / 100.f),
@@ -412,17 +412,17 @@ static void Log_Read_Control_Tuning()
 {
 	float logvar;
 
-	Serial.printf_P(PSTR("CTUN: "));
+	cliSerial->printf_P(PSTR("CTUN: "));
 	for (int y = 1; y < 10; y++) {
 		logvar = DataFlash.ReadInt();
                 if(y == 7) cur_throttle = logvar;
 		if(y < 8) logvar 	= logvar/100.f;
 		if(y == 9) logvar 	= logvar/10000.f;
-		Serial.print(logvar);
-		Serial.print(comma);
-	        Serial.print(" ");
+		cliSerial->print(logvar);
+		cliSerial->print(comma);
+	        cliSerial->print(" ");
 	}
-	Serial.println("");
+	cliSerial->println("");
 }
 
 // Read a nav tuning packet
@@ -432,7 +432,7 @@ static void Log_Read_Nav_Tuning()
     for (int8_t i=0; i<7; i++) {
         d[i] = DataFlash.ReadInt();
     }
-	Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"),		// \n
+	cliSerial->printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"),		// \n
                     d[0]/100.0,
                     d[1],
                     ((uint16_t)d[2])/100.0,
@@ -448,21 +448,21 @@ static void Log_Read_Performance()
 	int32_t pm_time;
 	int logvar;
 
-	Serial.printf_P(PSTR("PM: "));
+	cliSerial->printf_P(PSTR("PM: "));
 	pm_time = DataFlash.ReadLong();
-	Serial.print(pm_time);
-	Serial.print(comma);
+	cliSerial->print(pm_time);
+	cliSerial->print(comma);
 	for (int y = 1; y <= 12; y++) {
 		if(y < 3 || y > 7){
 			logvar = DataFlash.ReadInt();
 		}else{
 			logvar = DataFlash.ReadByte();
 		}
-		Serial.print(logvar);
-		Serial.print(comma);
-	        Serial.print(" ");
+		cliSerial->print(logvar);
+		cliSerial->print(comma);
+	        cliSerial->print(" ");
 	}
-	Serial.println("");
+	cliSerial->println("");
 }
 
 // Read a command processing packet
@@ -471,20 +471,20 @@ static void Log_Read_Cmd()
 	byte logvarb;
 	int32_t logvarl;
 
-	Serial.printf_P(PSTR("CMD: "));
+	cliSerial->printf_P(PSTR("CMD: "));
 	for(int i = 1; i < 4; i++) {
 		logvarb = DataFlash.ReadByte();
-		Serial.print(logvarb, DEC);
-		Serial.print(comma);
-	        Serial.print(" ");
+		cliSerial->print(logvarb, DEC);
+		cliSerial->print(comma);
+	        cliSerial->print(" ");
 	}
 	for(int i = 1; i < 4; i++) {
 		logvarl = DataFlash.ReadLong();
-		Serial.print(logvarl, DEC);
-		Serial.print(comma);
-	        Serial.print(" ");
+		cliSerial->print(logvarl, DEC);
+		cliSerial->print(comma);
+	        cliSerial->print(" ");
 	}
-	Serial.println("");
+	cliSerial->println("");
 }
 
 static void Log_Read_Startup()
@@ -492,13 +492,13 @@ static void Log_Read_Startup()
 	byte logbyte = DataFlash.ReadByte();
 
 	if (logbyte == TYPE_AIRSTART_MSG)
-		Serial.printf_P(PSTR("AIR START - "));
+		cliSerial->printf_P(PSTR("AIR START - "));
 	else if (logbyte == TYPE_GROUNDSTART_MSG)
-		Serial.printf_P(PSTR("GROUND START - "));
+		cliSerial->printf_P(PSTR("GROUND START - "));
 	else
-		Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
+		cliSerial->printf_P(PSTR("UNKNOWN STARTUP - "));
 
-	Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
+	cliSerial->printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
 }
 
 // Read an attitude packet
@@ -508,7 +508,7 @@ static void Log_Read_Attitude()
     d[0] = DataFlash.ReadInt();
     d[1] = DataFlash.ReadInt();
     d[2] = DataFlash.ReadInt();
-	Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
+	cliSerial->printf_P(PSTR("ATT: %d, %d, %u\n"),
                     d[0], d[1],
                     (uint16_t)d[2]);
 }
@@ -516,8 +516,8 @@ static void Log_Read_Attitude()
 // Read a mode packet
 static void Log_Read_Mode()
 {
-	Serial.printf_P(PSTR("MOD: "));
-	Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
+	cliSerial->printf_P(PSTR("MOD: "));
+	cliSerial->println(flight_mode_strings[DataFlash.ReadByte()]);
 }
 
 // Read a GPS packet
@@ -540,17 +540,17 @@ static void Log_Read_GPS()
     k = DataFlash.ReadInt();
     m = DataFlash.ReadInt();
     /*
-	Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+	cliSerial->printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
                     (long)l[0], (int)b[0], (int)b[1],
                     l[1]/t7, l[2]/t7, 
                     (int)i,
                     l[3]/100.0, l[4]/100.0, l[5]/100.0, l[6]/100.0); */
-	Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+	cliSerial->printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
                     (long)l[0], (int)b[0], (int)b[1],
                     l[1]/t7, l[2]/t7, 
                     l[4]/100.0, l[3]/100.0, l[5]/100.0, l[6]/100.0);
                                         
-        Serial.printf_P(PSTR("THP: %4.7f, %4.7f, %4.4f, %2.1f, %2.1f, %2.1f, %d\n"),
+        cliSerial->printf_P(PSTR("THP: %4.7f, %4.7f, %4.4f, %2.1f, %2.1f, %2.1f, %d\n"),
                     l[1]/t7, l[2]/t7, l[3]/100.0,
                     (float)j/100.0, (float)k/100.0, (float)m/100.0, cur_throttle);
 }
@@ -559,14 +559,14 @@ static void Log_Read_GPS()
 static void Log_Read_Raw()
 {
 	float logvar;
-	Serial.printf_P(PSTR("RAW: "));
+	cliSerial->printf_P(PSTR("RAW: "));
 	for (int y = 0; y < 6; y++) {
 		logvar = (float)DataFlash.ReadLong() / t7;
-		Serial.print(logvar);
-		Serial.print(comma);
-	        Serial.print(" ");
+		cliSerial->print(logvar);
+		cliSerial->print(comma);
+	        cliSerial->print(" ");
 	}
-	Serial.println("");
+	cliSerial->println("");
 }
 
 // Read the DataFlash log memory : Packet Parser
@@ -575,9 +575,9 @@ static void Log_Read(int16_t start_page, int16_t end_page)
 	int packet_count = 0;
 
 	#ifdef AIRFRAME_NAME
-		Serial.printf_P(PSTR((AIRFRAME_NAME)
+		cliSerial->printf_P(PSTR((AIRFRAME_NAME)
 	#endif
-	Serial.printf_P(PSTR("\n" THISFIRMWARE
+	cliSerial->printf_P(PSTR("\n" THISFIRMWARE
 						 "\nFree RAM: %u\n"),
                     memcheck_available_memory());
 
@@ -589,7 +589,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"), packet_count);
+	cliSerial->printf_P(PSTR("Number of packets read: %d\n"), packet_count);
 }
 
 // Read the DataFlash log memory : Packet Parser
@@ -657,7 +657,7 @@ static int Log_Read_Process(int16_t start_page, int16_t end_page)
 						Log_Read_GPS();
 						log_step++;
 					}else{
-						Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
+						cliSerial->printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
 						log_step = 0;	 // Restart, we have a problem...
 					}
 				}
@@ -666,7 +666,7 @@ static int 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/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
index b9ae628a5f..a9596b62d5 100644
--- a/APMrover2/Parameters.pde
+++ b/APMrover2/Parameters.pde
@@ -150,17 +150,17 @@ static void load_parameters(void)
 	     g.format_version != Parameters::k_format_version) {
 
 		// erase all parameters
-		Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
+		cliSerial->printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
 		AP_Param::erase_all();
 
 		// save the current format version
 		g.format_version.set_and_save(Parameters::k_format_version);
-		Serial.println_P(PSTR("done."));
+		cliSerial->println_P(PSTR("done."));
     } else {
 	    unsigned long 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/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index 95c5b1664a..b8db2df034 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -248,7 +248,7 @@ static bool verify_takeoff()
 static void calc_turn_radius(void)    // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
 {
   wp_radius = ground_speed * 150 / g.roll_limit.get();
-  //Serial.println(wp_radius, DEC);
+  //cliSerial->println(wp_radius, DEC);
 }
 
 
diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde
index a51015fea6..9e54f522a8 100644
--- a/APMrover2/control_modes.pde
+++ b/APMrover2/control_modes.pde
@@ -73,7 +73,7 @@ if (g.ch7_option == CH7_SAVE_WP){         // set to 1
                                         if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
                                             ground_start_count = 5;
                                         #if X_PLANE == ENABLED
-                                                Serial.printf_P(PSTR("*** RESET the FPL\n"));
+                                                cliSerial->printf_P(PSTR("*** RESET the FPL\n"));
                                         #endif
                                         CH7_wp_index = 1;                    
 					return;
@@ -94,7 +94,7 @@ if (g.ch7_option == CH7_SAVE_WP){         // set to 1
         			  set_cmd_with_index(current_loc, CH7_wp_index);
                                                                              
                                   #if X_PLANE == ENABLED
-                                     Serial.printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index);
+                                     cliSerial->printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index);
                                   #endif 
                                   
                                     // increment index
diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde
index 7265207c75..ff3bf0029c 100644
--- a/APMrover2/navigation.pde
+++ b/APMrover2/navigation.pde
@@ -29,7 +29,7 @@ static void navigate()
 
 	if (wp_distance < 0){
 		gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
-		//Serial.println(wp_distance,DEC);
+		//cliSerial->println(wp_distance,DEC);
 		return;
 	}
 
diff --git a/APMrover2/planner.pde b/APMrover2/planner.pde
index 1a5d8ee4af..30b15fbbc3 100644
--- a/APMrover2/planner.pde
+++ b/APMrover2/planner.pde
@@ -18,7 +18,7 @@ MENU(planner_menu, "planner", planner_menu_commands);
 static int8_t
 planner_mode(uint8_t argc, const Menu::arg *argv)
 {
-	Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
+	cliSerial->printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
 	planner_menu.run();
     return 0;
 }
diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde
index e1ceb1e987..23685576dc 100644
--- a/APMrover2/radio.pde
+++ b/APMrover2/radio.pde
@@ -104,7 +104,7 @@ static void read_radio()
 	}
 
 	/*
-	Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
+	cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
 				g.rc_1.control_in,
 				g.rc_2.control_in,
 				g.rc_3.control_in,
diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde
index 7d06724e13..9fba5a6cc5 100644
--- a/APMrover2/setup.pde
+++ b/APMrover2/setup.pde
@@ -36,7 +36,7 @@ static int8_t
 setup_mode(uint8_t argc, const Menu::arg *argv)
 {
 	// Give the user some guidance
-	Serial.printf_P(PSTR("Setup Mode\n"
+	cliSerial->printf_P(PSTR("Setup Mode\n"
 						 "\n"
 						 "IMPORTANT: if you have not previously set this system up, use the\n"
 						 "'reset' command to initialize the EEPROM to sensible default values\n"
@@ -65,7 +65,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
 	report_ins();
 	report_compass();
 
-	Serial.printf_P(PSTR("Raw Values\n"));
+	cliSerial->printf_P(PSTR("Raw Values\n"));
 	print_divider();
 
     AP_Param::show_all();
@@ -80,16 +80,16 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 {
 	int			c;
 
-	Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
+	cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
 
 	do {
-		c = Serial.read();
+		c = cliSerial->read();
 	} while (-1 == c);
 
 	if (('y' != c) && ('Y' != c))
 		return(-1);
 	AP_Param::erase_all();
-	Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
+	cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
 
 	//default_flight_modes();   // This will not work here.  Replacement code located in init_ardupilot()
 
@@ -105,7 +105,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_radio(uint8_t argc, const Menu::arg *argv)
 {
-	Serial.printf_P(PSTR("\n\nRadio Setup:\n"));
+	cliSerial->printf_P(PSTR("\n\nRadio Setup:\n"));
 	uint8_t i;
 
 	for(i = 0; i < 100;i++){
@@ -116,7 +116,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 
 	if(g.channel_roll.radio_in < 500){
 		while(1){
-			Serial.printf_P(PSTR("\nNo radio; Check connectors."));
+			cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
 			delay(1000);
 			// stop here
 		}
@@ -148,7 +148,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 	g.rc_7.radio_trim = 1500;
 	g.rc_8.radio_trim = 1500;
 
-	Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
+	cliSerial->printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
 	while(1){
 
 		delay(20);
@@ -165,8 +165,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 		g.rc_7.update_min_max();
 		g.rc_8.update_min_max();
 
-		if(Serial.available() > 0){
-			Serial.flush();
+		if(cliSerial->available() > 0){
+			cliSerial->flush();
 			g.channel_roll.save_eeprom();
 			g.channel_pitch.save_eeprom();
 			g.channel_throttle.save_eeprom();
@@ -190,7 +190,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 {
 	byte switchPosition, mode = 0;
 
-	Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
+	cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
 	print_hit_enter();
 	trim_radio();
 
@@ -253,7 +253,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 		}
 
 		// escape hatch
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 		    // save changes
             for (mode=0; mode<6; mode++)
                 flight_modes[mode].save();
@@ -278,10 +278,10 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
 {
 	int			c;
 
-	Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
+	cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
 
 	do {
-		c = Serial.read();
+		c = cliSerial->read();
 	} while (-1 == c);
 
 	if (('y' != c) && ('Y' != c))
@@ -290,11 +290,41 @@ setup_erase(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);
+    if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
+        if (g.manual_level == 0) {
+            cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1"));
+            g.manual_level.set_and_save(1);
+        }
+    }
     report_ins();
     return(0);
 }
@@ -305,7 +335,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
 	if (!strcmp_P(argv[1].str, PSTR("on"))) {
         compass.set_orientation(MAG_ORIENTATION);	// set compass's orientation on aircraft
 		if (!compass.init()) {
-            Serial.println_P(PSTR("Compass initialisation failed!"));
+            cliSerial->println_P(PSTR("Compass initialisation failed!"));
             g.compass_enabled = false;
         } else {
             g.compass_enabled = true;
@@ -317,7 +347,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
 		compass.set_offsets(0,0,0);
 
 	} else {
-		Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n"));
+		cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
 		report_compass();
 		return 0;
 	}
@@ -334,7 +364,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
 		g.battery_monitoring.set_and_save(argv[1].i);
 
 	} else {
-		Serial.printf_P(PSTR("\nOptions: 3-4"));
+		cliSerial->printf_P(PSTR("\nOptions: 3-4"));
 	}
 
 	report_batt_monitor();
@@ -348,17 +378,17 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
 static void report_batt_monitor()
 {
 	//print_blanks(2);
-	Serial.printf_P(PSTR("Batt Mointor\n"));
+	cliSerial->printf_P(PSTR("Batt Mointor\n"));
 	print_divider();
-	if(g.battery_monitoring == 0)	Serial.printf_P(PSTR("Batt monitoring disabled"));
-	if(g.battery_monitoring == 3)	Serial.printf_P(PSTR("Monitoring batt volts"));
-	if(g.battery_monitoring == 4)	Serial.printf_P(PSTR("Monitoring volts and current"));
+	if(g.battery_monitoring == 0)	cliSerial->printf_P(PSTR("Batt monitoring disabled"));
+	if(g.battery_monitoring == 3)	cliSerial->printf_P(PSTR("Monitoring batt volts"));
+	if(g.battery_monitoring == 4)	cliSerial->printf_P(PSTR("Monitoring volts and current"));
 	print_blanks(2);
 }
 static void report_radio()
 {
 	//print_blanks(2);
-	Serial.printf_P(PSTR("Radio\n"));
+	cliSerial->printf_P(PSTR("Radio\n"));
 	print_divider();
 	// radio
 	print_radio_values();
@@ -368,28 +398,28 @@ static void report_radio()
 static void report_gains()
 {
 	//print_blanks(2);
-	Serial.printf_P(PSTR("Gains\n"));
+	cliSerial->printf_P(PSTR("Gains\n"));
 	print_divider();
 
-	Serial.printf_P(PSTR("servo roll:\n"));
+	cliSerial->printf_P(PSTR("servo roll:\n"));
 	print_PID(&g.pidServoRoll);
 
-	Serial.printf_P(PSTR("servo pitch:\n"));
+	cliSerial->printf_P(PSTR("servo pitch:\n"));
 	print_PID(&g.pidServoPitch);
 
-	Serial.printf_P(PSTR("servo rudder:\n"));
+	cliSerial->printf_P(PSTR("servo rudder:\n"));
 	print_PID(&g.pidServoRudder);
 
-	Serial.printf_P(PSTR("nav roll:\n"));
+	cliSerial->printf_P(PSTR("nav roll:\n"));
 	print_PID(&g.pidNavRoll);
 
-	Serial.printf_P(PSTR("nav pitch airspeed:\n"));
+	cliSerial->printf_P(PSTR("nav pitch airspeed:\n"));
 	print_PID(&g.pidNavPitchAirspeed);
 
-	Serial.printf_P(PSTR("energry throttle:\n"));
+	cliSerial->printf_P(PSTR("energry throttle:\n"));
 	print_PID(&g.pidTeThrottle);
 
-	Serial.printf_P(PSTR("nav pitch alt:\n"));
+	cliSerial->printf_P(PSTR("nav pitch alt:\n"));
 	print_PID(&g.pidNavPitchAltitude);
 
 	print_blanks(2);
@@ -398,10 +428,10 @@ static void report_gains()
 static void report_xtrack()
 {
 	//print_blanks(2);
-	Serial.printf_P(PSTR("Crosstrack\n"));
+	cliSerial->printf_P(PSTR("Crosstrack\n"));
 	print_divider();
 	// radio
-	Serial.printf_P(PSTR("XTRACK: %4.2f\n"
+	cliSerial->printf_P(PSTR("XTRACK: %4.2f\n"
 						 "XTRACK angle: %d\n"),
 						 (float)g.crosstrack_gain,
 						 (int)g.crosstrack_entry_angle);
@@ -411,10 +441,10 @@ static void report_xtrack()
 static void report_throttle()
 {
 	//print_blanks(2);
-	Serial.printf_P(PSTR("Throttle\n"));
+	cliSerial->printf_P(PSTR("Throttle\n"));
 	print_divider();
 
-	Serial.printf_P(PSTR("min: %d\n"
+	cliSerial->printf_P(PSTR("min: %d\n"
 						 "max: %d\n"
 						 "cruise: %d\n"
 						 "failsafe_enabled: %d\n"
@@ -430,7 +460,7 @@ static void report_throttle()
 static void report_ins()
 {
     //print_blanks(2);
-    Serial.printf_P(PSTR("INS\n"));
+    cliSerial->printf_P(PSTR("INS\n"));
     print_divider();
 
     print_gyro_offsets();
@@ -441,20 +471,20 @@ static void report_ins()
 static void report_compass()
 {
 	//print_blanks(2);
-	Serial.printf_P(PSTR("Compass: "));
+	cliSerial->printf_P(PSTR("Compass: "));
 
     switch (compass.product_id) {
     case AP_COMPASS_TYPE_HMC5883L:
-        Serial.println_P(PSTR("HMC5883L"));
+        cliSerial->println_P(PSTR("HMC5883L"));
         break;
     case AP_COMPASS_TYPE_HMC5843:
-        Serial.println_P(PSTR("HMC5843"));
+        cliSerial->println_P(PSTR("HMC5843"));
         break;
     case AP_COMPASS_TYPE_HIL:
-        Serial.println_P(PSTR("HIL"));
+        cliSerial->println_P(PSTR("HIL"));
         break;
     default:
-        Serial.println_P(PSTR("??"));
+        cliSerial->println_P(PSTR("??"));
         break;
     }
 
@@ -463,13 +493,13 @@ static void report_compass()
 	print_enabled(g.compass_enabled);
 
 	// mag declination
-	Serial.printf_P(PSTR("Mag Declination: %4.4f\n"),
+	cliSerial->printf_P(PSTR("Mag Declination: %4.4f\n"),
 							degrees(compass.get_declination()));
 
 	Vector3f offsets = compass.get_offsets();
 
 	// mag offsets
-	Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
+	cliSerial->printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
 							offsets.x,
 							offsets.y,
 							offsets.z);
@@ -479,7 +509,7 @@ static void report_compass()
 static void report_flight_modes()
 {
 	//print_blanks(2);
-	Serial.printf_P(PSTR("Flight modes\n"));
+	cliSerial->printf_P(PSTR("Flight modes\n"));
 	print_divider();
 
 	for(int i = 0; i < 6; i++ ){
@@ -495,7 +525,7 @@ static void report_flight_modes()
 static void
 print_PID(PID * pid)
 {
-	Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
+	cliSerial->printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
 					pid->kP(),
 					pid->kI(),
 					pid->kD(),
@@ -505,28 +535,28 @@ print_PID(PID * pid)
 static void
 print_radio_values()
 {
-	Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
-	Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
-	Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
-	Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
-	Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
-	Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
-	Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
-	Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
+	cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
+	cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
+	cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
+	cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
+	cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
+	cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
+	cliSerial->printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
+	cliSerial->printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
 
 }
 
 static void
 print_switch(byte p, byte m)
 {
-	Serial.printf_P(PSTR("Pos %d: "),p);
-	Serial.println(flight_mode_strings[m]);
+	cliSerial->printf_P(PSTR("Pos %d: "),p);
+	cliSerial->println(flight_mode_strings[m]);
 }
 
 static void
 print_done()
 {
-	Serial.printf_P(PSTR("\nSaved Settings\n\n"));
+	cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
 }
 
 static void
@@ -534,7 +564,7 @@ print_blanks(int num)
 {
 	while(num > 0){
 		num--;
-		Serial.println("");
+		cliSerial->println("");
 	}
 }
 
@@ -542,9 +572,9 @@ static void
 print_divider(void)
 {
 	for (int i = 0; i < 40; i++) {
-		Serial.printf_P(PSTR("-"));
+		cliSerial->printf_P(PSTR("-"));
 	}
-	Serial.println("");
+	cliSerial->println("");
 }
 
 static int8_t
@@ -577,20 +607,20 @@ radio_input_switch(void)
 static void zero_eeprom(void)
 {
 	byte b = 0;
-	Serial.printf_P(PSTR("\nErasing EEPROM\n"));
+	cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
 	for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) {
 		eeprom_write_byte((uint8_t *) i, b);
 	}
-	Serial.printf_P(PSTR("done\n"));
+	cliSerial->printf_P(PSTR("done\n"));
 }
 
 static void print_enabled(bool b)
 {
 	if(b)
-		Serial.printf_P(PSTR("en"));
+		cliSerial->printf_P(PSTR("en"));
 	else
-		Serial.printf_P(PSTR("dis"));
-	Serial.printf_P(PSTR("abled\n"));
+		cliSerial->printf_P(PSTR("dis"));
+	cliSerial->printf_P(PSTR("abled\n"));
 }
 
 static void
@@ -598,7 +628,7 @@ print_accel_offsets_and_scaling(void)
 {
     Vector3f accel_offsets = ins.get_accel_offsets();
     Vector3f accel_scale = ins.get_accel_scale();
-    Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
+    cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
                     (float)accel_offsets.x,                           // Pitch
                     (float)accel_offsets.y,                           // Roll
                     (float)accel_offsets.z,                           // YAW
@@ -610,7 +640,7 @@ print_accel_offsets_and_scaling(void)
 static void
 print_gyro_offsets(void)
 {
-	Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
+	cliSerial->printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
 						(float)ins.gx(),
 						(float)ins.gy(),
 						(float)ins.gz());
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 23e46136ca..864d7f71f6 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -21,7 +21,7 @@ static int8_t	planner_mode(uint8_t argc, const Menu::arg *argv);	// in planner.p
 // printf_P is a version of print_f that reads from flash memory
 static int8_t	main_menu_help(uint8_t argc, const Menu::arg *argv)
 {
-	Serial.printf_P(PSTR("Commands:\n"
+	cliSerial->printf_P(PSTR("Commands:\n"
 						 "  logs        log readback/setup mode\n"
 						 "  setup       setup mode\n"
 						 "  test        test mode\n"
@@ -48,11 +48,15 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
 MENU(main_menu, THISFIRMWARE, main_menu_commands);
 
 // the user wants the CLI. It never exits
-static void run_cli(void)
+static void run_cli(FastSerial *port)
 {
     // disable the failsafe code in the CLI
     timer_scheduler.set_failsafe(NULL);
 
+    cliSerial = port;
+    Menu::set_port(port);
+    port->set_blocking_writes(true);
+
     while (1) {
         main_menu.run();
     }
@@ -89,7 +93,7 @@ static void init_ardupilot()
 	// XXX This could be optimised to reduce the buffer sizes in the cases
 	// where they are not otherwise required.
 	//
-	Serial.begin(SERIAL0_BAUD, 128, 128);
+	cliSerial->begin(SERIAL0_BAUD, 128, 128);
 
 	// GPS serial port.
 	//
@@ -103,7 +107,7 @@ static void init_ardupilot()
     // standard gps running
     Serial1.begin(115200, 128, 16);
 
-	Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
+	cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
 						 "\n\nFree RAM: %u\n"),
                     memcheck_available_memory());
                     
@@ -149,7 +153,7 @@ static void init_ardupilot()
     if (!usb_connected) {
         // we are not connected via USB, re-init UART0 with right
         // baud rate
-        Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+        cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
     }
 #else
     // we have a 2nd serial port for telemetry
@@ -185,7 +189,7 @@ static void init_ardupilot()
 	if (g.compass_enabled==true) {
         compass.set_orientation(MAG_ORIENTATION);							// set compass's orientation on aircraft
 		if (!compass.init()|| !compass.read()) {
-            Serial.println_P(PSTR("Compass initialisation failed!"));
+            cliSerial->println_P(PSTR("Compass initialisation failed!"));
             g.compass_enabled = false;
         } else {
             ahrs.set_compass(&compass);
@@ -199,7 +203,7 @@ static void init_ardupilot()
   // I2c.setSpeed(true);
 
   if (!compass.init()) {
-	  Serial.println("compass initialisation failed!");
+	  cliSerial->println("compass initialisation failed!");
 	  while (1) ;
   }
 
@@ -207,19 +211,19 @@ static void init_ardupilot()
   compass.set_offsets(0,0,0);  // set offsets to account for surrounding interference
   compass.set_declination(ToRad(0.0));  // set local difference between magnetic north and true north
 
-  Serial.print("Compass auto-detected as: ");
+  cliSerial->print("Compass auto-detected as: ");
   switch( compass.product_id ) {
       case AP_COMPASS_TYPE_HIL:
-	      Serial.println("HIL");
+	      cliSerial->println("HIL");
 		  break;
       case AP_COMPASS_TYPE_HMC5843:
-	      Serial.println("HMC5843");
+	      cliSerial->println("HMC5843");
 		  break;
       case AP_COMPASS_TYPE_HMC5883L:
-	      Serial.println("HMC5883L");
+	      cliSerial->println("HMC5883L");
 		  break;
       default:
-	      Serial.println("unknown");
+	      cliSerial->println("unknown");
 		  break;
   }
   
@@ -276,17 +280,21 @@ static void init_ardupilot()
 #if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED
 	if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
 		digitalWrite(A_LED_PIN,LED_ON);		// turn on setup-mode LED
-		Serial.printf_P(PSTR("\n"
+		cliSerial->printf_P(PSTR("\n"
 							 "Entering interactive setup mode...\n"
 							 "\n"
 							 "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n"
 							 "Type 'help' to list commands, 'exit' to leave a submenu.\n"
 							 "Visit the 'setup' menu for first-time configuration.\n"));
-        Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
-        run_cli();
+        cliSerial->println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
+        run_cli(&cliSerial);
 	}
 #else
-    Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n"));
+    const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
+    cliSerial->println_P(msg);
+#if USB_MUX_PIN == 0
+    Serial3.println_P(msg);
+#endif
 #endif // CLI_ENABLED
 
 	// read in the flight switches
@@ -526,7 +534,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;
 }
 
@@ -542,9 +550,9 @@ static void check_usb_mux(void)
     // the user has switched to/from the telemetry port
     usb_connected = usb_check;
     if (usb_connected) {
-        Serial.begin(SERIAL0_BAUD, 128, 128);
+        cliSerial->begin(SERIAL0_BAUD, 128, 128);
     } else {
-        Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+        cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
     }
 }
 #endif
diff --git a/APMrover2/test.pde b/APMrover2/test.pde
index df0f60da08..2cf1ab26f9 100644
--- a/APMrover2/test.pde
+++ b/APMrover2/test.pde
@@ -78,14 +78,14 @@ MENU(test_menu, "test", test_menu_commands);
 static int8_t
 test_mode(uint8_t argc, const Menu::arg *argv)
 {
-	Serial.printf_P(PSTR("Test Mode\n\n"));
+	cliSerial->printf_P(PSTR("Test Mode\n\n"));
 	test_menu.run();
     return 0;
 }
 
 static void print_hit_enter()
 {
-	Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
+	cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
 }
 
 static int8_t
@@ -95,10 +95,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);
 }
@@ -116,7 +116,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
 		// ----------------------------------------------------------
 		read_radio();
 
-		Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
+		cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
 							g.channel_roll.radio_in,
 							g.channel_pitch.radio_in,
 							g.channel_throttle.radio_in,
@@ -126,7 +126,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);
 		}
 	}
@@ -144,15 +144,15 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
 
         // New radio frame? (we could use also if((millis()- timer) > 20)
         if (APM_RC.GetState() == 1){
-            Serial.print("CH:");
+            cliSerial->print("CH:");
             for(int i = 0; i < 8; i++){
-                Serial.print(APM_RC.InputCh(i));	// Print channel values
-                Serial.print(",");
+                cliSerial->print(APM_RC.InputCh(i));	// Print channel values
+                cliSerial->print(",");
                 APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
             }
-            Serial.println();
+            cliSerial->println();
         }
-        if (Serial.available() > 0){
+        if (cliSerial->available() > 0){
             return (0);
         }
     }
@@ -166,7 +166,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 	delay(1000);
 
 	#if THROTTLE_REVERSE == 1
-		Serial.printf_P(PSTR("Throttle is reversed in config: \n"));
+		cliSerial->printf_P(PSTR("Throttle is reversed in config: \n"));
 		delay(1000);
 	#endif
 
@@ -190,7 +190,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 
                 tuning_value = constrain(((float)(g.rc_7.radio_in - g.rc_7.radio_min) / (float)(g.rc_7.radio_max - g.rc_7.radio_min)),0,1);
                 
-		Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d  Tuning = %2.3f\n"),
+		cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d  Tuning = %2.3f\n"),
 							g.channel_roll.control_in,
 							g.channel_pitch.control_in,
 							g.channel_throttle.control_in,
@@ -201,7 +201,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 							g.rc_8.control_in,
                                                         tuning_value);
 
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -223,7 +223,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
 
 	oldSwitchPosition = readSwitch();
 
-	Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
+	cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
 	while(g.channel_throttle.control_in > 0){
 		delay(20);
 		read_radio();
@@ -234,27 +234,27 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
 		read_radio();
 
 		if(g.channel_throttle.control_in > 0){
-			Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in);
+			cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.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.channel_throttle.get_failsafe()){
-			Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
-			Serial.println(flight_mode_strings[readSwitch()]);
+			cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.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 APM.\n"));
+		if(cliSerial->available() > 0){
+			cliSerial->printf_P(PSTR("LOS caused no change in APM.\n"));
 			return (0);
 		}
 	}
@@ -272,12 +272,12 @@ if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
 		read_radio();
 		read_battery();
 		if (g.battery_monitoring == 3){
-			Serial.printf_P(PSTR("V: %4.4f\n"),
+			cliSerial->printf_P(PSTR("V: %4.4f\n"),
 						battery_voltage1,
 						current_amps1,
 						current_total1);
 		} else {
-			Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
+			cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
 						battery_voltage1,
 						current_amps1,
 						current_total1);
@@ -287,12 +287,12 @@ if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
 		// ------------------------------
 		set_servos();
 
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
 } else {
-	Serial.printf_P(PSTR("Not enabled\n"));
+	cliSerial->printf_P(PSTR("Not enabled\n"));
 	return (0);
 }
 
@@ -305,17 +305,17 @@ test_relay(uint8_t argc, const Menu::arg *argv)
 	delay(1000);
 
 	while(1){
-		Serial.printf_P(PSTR("Relay on\n"));
+		cliSerial->printf_P(PSTR("Relay on\n"));
 		relay.on();
 		delay(3000);
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 
-		Serial.printf_P(PSTR("Relay off\n"));
+		cliSerial->printf_P(PSTR("Relay off\n"));
 		relay.off();
 		delay(3000);
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -326,9 +326,9 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 {
 	delay(1000);
 
-	Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
-	Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
-	Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
+	cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
+	cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
+	cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
 
 	for(byte i = 0; i <= g.command_total; i++){
 		struct Location temp = get_cmd_with_index(i);
@@ -341,7 +341,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 static void
 test_wp_print(struct Location *cmd, byte wp_index)
 {
-	Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
+	cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
 		(int)wp_index,
 		(int)cmd->id,
 		(int)cmd->options,
@@ -356,14 +356,14 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
 {
 	print_hit_enter();
 	delay(1000);
-	Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
+	cliSerial->printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
 
 	while(1){
 
 	    if (Serial3.available())
    			Serial3.write(Serial3.read());
 
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -376,18 +376,18 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
 	print_hit_enter();
 	delay(1000);
 
-	Serial.printf_P(PSTR("Control CH "));
+	cliSerial->printf_P(PSTR("Control CH "));
 
-	Serial.println(FLIGHT_MODE_CHANNEL, DEC);
+	cliSerial->println(FLIGHT_MODE_CHANNEL, DEC);
 
 	while(1){
 		delay(20);
 		byte switchPosition = readSwitch();
 		if (oldSwitchPosition != switchPosition){
-			Serial.printf_P(PSTR("Position %d\n"),  switchPosition);
+			cliSerial->printf_P(PSTR("Position %d\n"),  switchPosition);
 			oldSwitchPosition = switchPosition;
 		}
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -399,20 +399,20 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_logging(uint8_t argc, const Menu::arg *argv)
 {
-	Serial.println_P(PSTR("Testing dataflash logging"));
+	cliSerial->println_P(PSTR("Testing dataflash logging"));
     if (!DataFlash.CardInserted()) {
-        Serial.println_P(PSTR("ERR: No dataflash inserted"));
+        cliSerial->println_P(PSTR("ERR: No dataflash inserted"));
         return 0;
     }
     DataFlash.ReadManufacturerID();
-    Serial.printf_P(PSTR("Manufacturer: 0x%02x   Device: 0x%04x\n"),
+    cliSerial->printf_P(PSTR("Manufacturer: 0x%02x   Device: 0x%04x\n"),
                     (unsigned)DataFlash.df_manufacturer,
                     (unsigned)DataFlash.df_device);
-    Serial.printf_P(PSTR("NumPages: %u  PageSize: %u\n"),
+    cliSerial->printf_P(PSTR("NumPages: %u  PageSize: %u\n"),
                     (unsigned)DataFlash.df_NumPages+1,
                     (unsigned)DataFlash.df_PageSize);
     DataFlash.StartRead(DataFlash.df_NumPages+1);
-    Serial.printf_P(PSTR("Format version: %lx  Expected format version: %lx\n"),
+    cliSerial->printf_P(PSTR("Format version: %lx  Expected format version: %lx\n"),
                     (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT);
     return 0;
 }
@@ -425,7 +425,7 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
 	delay(1000);
 
     if (!g.switch_enable) {
-        Serial.println_P(PSTR("dip switches disabled, using EEPROM"));
+        cliSerial->println_P(PSTR("dip switches disabled, using EEPROM"));
     }
 
 	while(1){
@@ -433,17 +433,17 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
 		update_servo_switches();
 
 		if (g.mix_mode == 0) {
-			Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
+			cliSerial->printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
 				(int)g.channel_roll.get_reverse(),
 				(int)g.channel_pitch.get_reverse(),
 				(int)g.channel_rudder.get_reverse());
 		} else {
-			Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
+			cliSerial->printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
 				(int)g.reverse_elevons,
 				(int)g.reverse_ch1_elevon,
 				(int)g.reverse_ch2_elevon);
 		}
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -463,14 +463,14 @@ test_adc(uint8_t argc, const Menu::arg *argv)
 	print_hit_enter();
 	adc.Init(&timer_scheduler);
 	delay(1000);
-	Serial.printf_P(PSTR("ADC\n"));
+	cliSerial->printf_P(PSTR("ADC\n"));
 	delay(1000);
 
 	while(1){
-		for (int i=0;i<9;i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i));
-		Serial.println();
+		for (int i=0;i<9;i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i));
+		cliSerial->println();
 		delay(100);
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -493,15 +493,15 @@ test_gps(uint8_t argc, const Menu::arg *argv)
 		g_gps->update();
 
 		if (g_gps->new_data){
-			Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
+			cliSerial->printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
 					g_gps->latitude,
 					g_gps->longitude,
 					g_gps->altitude/100,
 					g_gps->num_sats);
 		}else{
-			Serial.printf_P(PSTR("."));
+			cliSerial->printf_P(PSTR("."));
 		}
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -510,7 +510,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_ins(uint8_t argc, const Menu::arg *argv)
 {
-	//Serial.printf_P(PSTR("Calibrating."));
+	//cliSerial->printf_P(PSTR("Calibrating."));
 	ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
     ahrs.reset();
 
@@ -540,14 +540,14 @@ test_ins(uint8_t argc, const Menu::arg *argv)
 			// ---------------------
             Vector3f gyros 	= ins.get_gyro();
             Vector3f accels = ins.get_accel();
-			Serial.printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
+			cliSerial->printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
                             (int)ahrs.roll_sensor / 100,
                             (int)ahrs.pitch_sensor / 100,
                             (uint16_t)ahrs.yaw_sensor / 100,
                             gyros.x, gyros.y, gyros.z,
                             accels.x, accels.y, accels.z);
 		}
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
 	}
@@ -558,14 +558,14 @@ static int8_t
 test_mag(uint8_t argc, const Menu::arg *argv)
 {
 	if (!g.compass_enabled) {
-        Serial.printf_P(PSTR("Compass: "));
+        cliSerial->printf_P(PSTR("Compass: "));
 		print_enabled(false);
 		return (0);
     }
 
     compass.set_orientation(MAG_ORIENTATION);
     if (!compass.init()) {
-        Serial.println_P(PSTR("Compass initialisation failed!"));
+        cliSerial->println_P(PSTR("Compass initialisation failed!"));
         return 0;
     }
     ahrs.set_compass(&compass);
@@ -578,7 +578,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 	int counter = 0;
     float heading = 0;
 
-		//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
+		//cliSerial->printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
 
     print_hit_enter();
 
@@ -608,7 +608,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 			if (counter>20) {
                 if (compass.healthy) {
                     Vector3f maggy = compass.get_offsets();
-                    Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
+                    cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
                                     (wrap_360(ToDeg(heading) * 100)) /100,
                                     (int)compass.mag_x,
                                     (int)compass.mag_y,
@@ -617,19 +617,19 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                                     maggy.y,
                                     maggy.z);
                 } else {
-                    Serial.println_P(PSTR("compass not healthy"));
+                    cliSerial->println_P(PSTR("compass not healthy"));
                 }
                 counter=0;
             }
 		}
-        if (Serial.available() > 0) {
+        if (cliSerial->available() > 0) {
             break;
         }
     }
 
     // save offsets. This allows you to get sane offset values using
     // the CLI before you go flying.    
-    Serial.println_P(PSTR("saving offsets"));
+    cliSerial->println_P(PSTR("saving offsets"));
     compass.save_offsets();
     return (0);
 }
@@ -658,7 +658,7 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
 			Serial3.write(Serial1.read());
 			digitalWrite(C_LED_PIN, LED_OFF);
 		}
-		if(Serial.available() > 0){
+		if(cliSerial->available() > 0){
 			return (0);
 		}
   }
@@ -678,9 +678,9 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
 	  if(g.sonar_enabled){
 		sonar_dist = sonar.read();
 	  }
-    	  Serial.printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
+    	  cliSerial->printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
 
-          if(Serial.available() > 0){
+          if(cliSerial->available() > 0){
   		break;
 	    }
   }