diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde
index 991d174ee0..af46d507a1 100644
--- a/ArduCopterMega/GCS_Mavlink.pde
+++ b/ArduCopterMega/GCS_Mavlink.pde
@@ -6,10 +6,12 @@
 
 GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
 packet_drops(0),
+
 // parameters
 // note, all values not explicitly initialised here are zeroed
 waypoint_send_timeout(1000), // 1 second
 waypoint_receive_timeout(1000), // 1 second
+
 // stream rates
 _group	(key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
 streamRateRawSensors		(&_group, 0, 0, PSTR("RAW_SENS")),
@@ -385,6 +387,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             } else {
                 frame = MAV_FRAME_GLOBAL; // reference frame
             }
+
             float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
 
             // time that the mav should loiter in milliseconds
@@ -404,6 +407,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 }
             }
 
+
 			switch (tell_command.id) {					// Switch to map APM command fields inot MAVLink command fields
 			case MAV_CMD_NAV_LOITER_TURNS:
 			case MAV_CMD_NAV_TAKEOFF:
@@ -515,14 +519,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_msg_waypoint_set_current_decode(msg, &packet);
             if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
-            // set current waypoint
-            g.waypoint_index.set_and_save(packet.seq);
-
-            {
-                Location temp;  // XXX this is gross
-                temp = get_command_with_index(packet.seq);
-                set_next_WP(&temp);
-            }
+            // set current command
+            change_command(packet.seq);
 
             mavlink_msg_waypoint_current_send(chan, g.waypoint_index);
             break;
diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h
index 8104befbb2..b782f61b7b 100644
--- a/ArduCopterMega/Mavlink_Common.h
+++ b/ArduCopterMega/Mavlink_Common.h
@@ -207,8 +207,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					motor_out[2],
 					motor_out[3],
 					motor_out[4],
-					motor_out[5],
-					motor_out[6],
+					g.rc_5.radio_out,
+					g.rc_6.radio_out,
 					motor_out[7]);
 			break;
 		}
@@ -220,7 +220,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					(float)airspeed / 100.0,
 					(float)g_gps->ground_speed / 100.0,
 					(dcm.yaw_sensor / 100) % 360,
-					nav_throttle,
+					(int)g.rc_3.servo_out,
 					current_loc.alt / 100.0,
 					climb_rate);
 			break;
diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde
index c7bcb31bd0..3b7f28ae9b 100644
--- a/ArduCopterMega/commands_process.pde
+++ b/ArduCopterMega/commands_process.pde
@@ -1,5 +1,22 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+// For changing active command mid-mission
+//----------------------------------------
+void change_command(uint8_t index)
+{
+	struct Location temp = get_command_with_index(index);
+
+	if (next_command.id > MAV_CMD_NAV_LAST ){
+		gcs.send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
+	} else {
+		command_must_index 	= NO_COMMAND;
+		next_command.id 	= NO_COMMAND;
+		g.waypoint_index.set_and_save(index - 1);
+		update_commands();
+	}
+}
+
+
 // called by 10 Hz Medium loop
 // ---------------------------
 void update_commands(void)
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index e98d5dc401..1969fd8e84 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -773,7 +773,8 @@ test_altitude(uint8_t argc, const Menu::arg *argv)
 			// decide which sensor we're usings
 			sonar_alt 		= sonar.read();
 		}
-		Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld\n"),
+
+		Serial.printf_P(PSTR("B_alt: %d, S_alt: %d\n"),
 					baro_alt,
 					sonar_alt);