use gcs_send_text_fmt() and cleanup a few old debug lines

ArduPlane/commands.pde

@@ -105,12 +105,7 @@ static void increment_WP_index()
 {
     if (g.waypoint_index <= g.waypoint_total) {
         g.waypoint_index.set_and_save(g.waypoint_index + 1);
-		SendDebug_P("MSG - WP index is incremented to ");
-	}else{
-		//SendDebug_P("MSG <increment_WP_index> Failed to increment WP index of ");
-		// This message is used excessively at the end of a mission
 	}
-    //SendDebugln(g.waypoint_index,DEC);
 }
 
 static void decrement_WP_index()
@@ -178,8 +173,6 @@ static void set_next_WP(struct Location *wp)
 
 static void set_guided_WP(void)
 {
-	SendDebug_P("MSG - set_guided_wp");
-
 	// copy the current location into the OldWP slot
 	// ---------------------------------------
 	prev_WP = current_loc;
@@ -216,7 +209,7 @@ static void set_guided_WP(void)
 // -------------------------------
 void init_home()
 {
-	SendDebugln("MSG: init home");
+	gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
 
 	// block until we get a good fix
 	// -----------------------------
@@ -230,7 +223,7 @@ void init_home()
 	home.alt 	= max(g_gps->altitude, 0);
 	home_is_set = true;
 
-	Serial.printf_P(PSTR("gps alt: %ld"), home.alt);
+    gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
 
 	// Save Home to EEPROM
 	// -------------------

ArduPlane/commands_logic.pde

@@ -316,11 +316,7 @@ static bool verify_nav_wp()
 	hold_course = -1;
 	update_crosstrack();
 	if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-		//SendDebug_P("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
+		gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),command_must_index);
-		//SendDebugln(command_must_index,DEC);
-		char message[30];
-		sprintf(message,"Reached Waypoint #%i",command_must_index);
-		gcs_send_text(SEVERITY_LOW,message);
 		return true;
 	}
 	// add in a more complex case

ArduPlane/control_modes.pde

@@ -36,8 +36,6 @@ static void reset_control_switch()
 {
 	oldSwitchPosition = 0;
 	read_control_switch();
-	SendDebug_P("MSG: reset_control_switch");
-	SendDebugln(oldSwitchPosition , DEC);
 }
 
 static void update_servo_switches()

ArduPlane/events.pde

@@ -6,7 +6,7 @@ static void failsafe_short_on_event()
 	// This is how to handle a short loss of control signal failsafe.
 	failsafe = FAILSAFE_SHORT;
 	ch3_failsafe_timer = millis();
-	SendDebug_P("Failsafe - Short event on");
+    gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on"));
 	switch(control_mode)
 	{
 		case MANUAL: 
@@ -28,14 +28,13 @@ static void failsafe_short_on_event()
 		default:
 			break;
 	}
-				SendDebug_P("flight mode = ");
+    gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
-				SendDebugln(control_mode, DEC);
 }
 
 static void failsafe_long_on_event()
 {
 	// This is how to handle a long loss of control signal failsafe.
-	SendDebug_P("Failsafe - Long event on");
+	gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on"));
 	APM_RC.clearOverride();		//  If the GCS is locked up we allow control to revert to RC
 	switch(control_mode)
 	{
@@ -63,7 +62,7 @@ static void failsafe_long_on_event()
 static void failsafe_short_off_event()
 {
 	// We're back in radio contact
-	SendDebug_P("Failsafe - Short event off");
+	gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
 	failsafe = FAILSAFE_NONE;
 
 	// re-read the switch so we can return to our preferred mode