diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 3aee45bbfe..6fff40cfa1 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -139,6 +139,7 @@ GPS         *g_gps;
 	#error Unrecognised HIL_MODE setting.
 #endif // HIL MODE
 
+// HIL
 #if HIL_MODE != HIL_MODE_DISABLED
 	#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
 		GCS_MAVLINK hil;
@@ -149,11 +150,14 @@ GPS         *g_gps;
 
 #if HIL_MODE != HIL_MODE_ATTITUDE
 	#if HIL_MODE != HIL_MODE_SENSORS
-		AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); // normal imu
+		// Normal
+		AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration);
 	#else
-		AP_IMU_Shim imu; // hil imu
+		// hil imu
+		AP_IMU_Shim imu;
 	#endif
-	AP_DCM  dcm(&imu, g_gps); // normal dcm
+	// normal dcm
+	AP_DCM  dcm(&imu, g_gps);
 #endif
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -184,8 +188,8 @@ const char* flight_mode_strings[] = {
 	"ALT_HOLD",
 	"FBW",
 	"AUTO",
-	"LOITER",
-	"POSITION_HOLD",
+	"GCS_AUTO",
+	"POS_HOLD",
 	"RTL",
 	"TAKEOFF",
 	"LAND"};
@@ -296,7 +300,7 @@ byte 	altitude_sensor = BARO;				// used to know which sensor is active, BARO or
 // --------------------
 boolean	takeoff_complete;					// Flag for using take-off controls
 boolean	land_complete;
-int		takeoff_altitude;
+//int		takeoff_altitude;
 int		landing_distance;					// meters;
 long 	old_alt;							// used for managing altitude rates
 int		velocity_land;
@@ -356,7 +360,7 @@ struct 	Location home;						// home location
 struct 	Location prev_WP;					// last waypoint
 struct 	Location current_loc;				// current location
 struct 	Location next_WP;					// next waypoint
-struct 	Location tell_command;				// command for telemetry
+//struct 	Location tell_command;				// command for telemetry
 struct 	Location next_command;				// command preloaded
 long 	target_altitude;					// used for
 long 	offset_altitude;					// used for
@@ -542,7 +546,9 @@ void medium_loop()
 
 			// perform next command
 			// --------------------
-			update_commands();
+			if(control_mode == AUTO || control_mode == GCS_AUTO){
+				update_commands();
+			}
 			break;
 
 		// This case deals with sending high rate telemetry
@@ -683,7 +689,6 @@ void super_slow_loop()
 		Log_Write_Current();
 }
 
-
 void update_GPS(void)
 {
 	g_gps->update();
@@ -800,7 +805,7 @@ void update_current_flight_mode(void)
 				}
 				break;
 
-			case LOITER:
+			//case LOITER:
 			case STABILIZE:
 				// clear any AP naviagtion values
 				nav_pitch 		= 0;
@@ -905,7 +910,7 @@ void update_current_flight_mode(void)
 				// apply throttle control
 				output_auto_throttle();
 
-				// mix in user control
+				// mix in user control with Nav control
 				control_nav_mixer();
 
 				// perform stabilzation
@@ -925,7 +930,7 @@ void update_current_flight_mode(void)
 				// apply throttle control
 				output_auto_throttle();
 
-				// mix in user control
+				// mix in user control with Nav control
 				control_nav_mixer();
 
 				// perform stabilzation
@@ -948,9 +953,8 @@ void update_navigation()
 	// ------------------------------------------------------------------------
 
 	// distance and bearing calcs only
-	if(control_mode == AUTO){
-		verify_must();
-		verify_may();
+	if(control_mode == AUTO || control_mode == GCS_AUTO){
+		verify_commands();
 	}else{
 		switch(control_mode){
 			case RTL:
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 44727c3419..d4bb8d3711 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -246,7 +246,6 @@ void output_manual_yaw()
 	}else{
 		// Yaw control
 		if(g.rc_4.control_in == 0){
-			//clear_yaw_control();
 			output_yaw_with_hold(true); // hold yaw
 		}else{
 			output_yaw_with_hold(false); // rate control yaw
diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde
index 79e510a0ec..4777ff6fc4 100644
--- a/ArduCopterMega/GCS_Mavlink.pde
+++ b/ArduCopterMega/GCS_Mavlink.pde
@@ -213,7 +213,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 break;
 
             case MAV_ACTION_HALT:
-                loiter_at_location();
+                do_hold_position();
                 break;
 
                 // can't implement due to APM configuration
@@ -285,7 +285,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 break;
 
             case MAV_ACTION_LOITER:
-                set_mode(LOITER);
+                //set_mode(LOITER);
                 break;
 
             default: break;
diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h
index 5c7ebbadd6..f6a449b3ec 100644
--- a/ArduCopterMega/Mavlink_Common.h
+++ b/ArduCopterMega/Mavlink_Common.h
@@ -27,164 +27,242 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
     switch(id) {
 
     case MSG_HEARTBEAT:
-		mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_ARDUPILOTMEGA);
+		mavlink_msg_heartbeat_send(
+				chan,
+				system_type,
+				MAV_AUTOPILOT_ARDUPILOTMEGA);
 		break;
 
     case MSG_EXTENDED_STATUS:
     {
-        uint8_t mode = MAV_MODE_UNINIT;
+        uint8_t mode 	 = MAV_MODE_UNINIT;
         uint8_t nav_mode = MAV_NAV_VECTOR;
 
 		switch(control_mode) {
 		case MANUAL:
-			mode = MAV_MODE_MANUAL;
+			mode 		= MAV_MODE_MANUAL;
 			break;
 		case CIRCLE:
-			mode = MAV_MODE_TEST3;
+			mode 		= MAV_MODE_TEST3;
 			break;
 		case STABILIZE:
-			mode = MAV_MODE_GUIDED;
+			mode 		= MAV_MODE_GUIDED;
 			break;
 		case FLY_BY_WIRE_A:
-			mode = MAV_MODE_TEST1;
+			mode 		= MAV_MODE_TEST1;
 			break;
 		case FLY_BY_WIRE_B:
-			mode = MAV_MODE_TEST2;
+			mode 		= MAV_MODE_TEST2;
 			break;
 		case AUTO:
-			mode = MAV_MODE_AUTO;
-			nav_mode = MAV_NAV_WAYPOINT;
+			mode 		= MAV_MODE_AUTO;
+			nav_mode 	= MAV_NAV_WAYPOINT;
 			break;
 		case RTL:
-			mode = MAV_MODE_AUTO;
-			nav_mode = MAV_NAV_RETURNING;
+			mode 		= MAV_MODE_AUTO;
+			nav_mode 	= MAV_NAV_RETURNING;
 			break;
 		case LOITER:
-			mode = MAV_MODE_AUTO;
-			nav_mode = MAV_NAV_HOLD;
+			mode 		= MAV_MODE_AUTO;
+			nav_mode 	= MAV_NAV_HOLD;
 			break;
 		case TAKEOFF:
-			mode = MAV_MODE_AUTO;
-			nav_mode = MAV_NAV_LIFTOFF;
+			mode 		= MAV_MODE_AUTO;
+			nav_mode 	= MAV_NAV_LIFTOFF;
 			break;
 		case LAND:
-			mode = MAV_MODE_AUTO;
-			nav_mode = MAV_NAV_LANDING;
+			mode 		= MAV_MODE_AUTO;
+			nav_mode 	= MAV_NAV_LANDING;
 			break;
 		}
-		uint8_t status = MAV_STATE_ACTIVE;
+		uint8_t status 		= MAV_STATE_ACTIVE;
         uint8_t motor_block = false;
 
-        mavlink_msg_sys_status_send(chan,mode,nav_mode,status,load*1000,
-            battery_voltage1*1000,motor_block,packet_drops);
+		mavlink_msg_sys_status_send(
+				chan,
+				mode,
+				nav_mode,
+				status,
+				load * 1000,
+				battery_voltage1 * 1000,
+				motor_block,
+				packet_drops);
 		break;
     }
 
     case MSG_ATTITUDE:
     {
 		Vector3f omega = dcm.get_gyro();
-        mavlink_msg_attitude_send(chan,timeStamp,dcm.roll,dcm.pitch,dcm.yaw,
-		omega.x,omega.y,omega.z);
-        break;
-    }
+		mavlink_msg_attitude_send(
+				chan,
+				timeStamp,
+				dcm.roll,
+				dcm.pitch,
+				dcm.yaw,
+				omega.x,
+				omega.y,
+				omega.z);
+		break;
+	}
+
     case MSG_LOCATION:
     {
         Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
-        mavlink_msg_global_position_int_send(chan,current_loc.lat,
-			current_loc.lng,current_loc.alt*10,g_gps->ground_speed/1.0e2*rot.a.x,
-			g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
+        mavlink_msg_global_position_int_send(
+        		chan,
+        		current_loc.lat,
+				current_loc.lng,
+				current_loc.alt * 10,
+				g_gps->ground_speed / 1.0e2 * rot.a.x,
+				g_gps->ground_speed / 1.0e2 * rot.b.x,
+				g_gps->ground_speed / 1.0e2 * rot.c.x);
         break;
     }
+
     case MSG_LOCAL_LOCATION:
     {
         Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
-        mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth,
-			ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)),
-			(current_loc.alt-home.alt)/1.0e2, g_gps->ground_speed/1.0e2*rot.a.x,
-			g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
+        mavlink_msg_local_position_send(
+        		chan,
+				timeStamp,
+				ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
+				ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
+				(current_loc.alt - home.alt) / 1.0e2,
+				g_gps->ground_speed / 1.0e2 * rot.a.x,
+				g_gps->ground_speed / 1.0e2 * rot.b.x,
+				g_gps->ground_speed / 1.0e2 * rot.c.x);
         break;
     }
+
     case MSG_GPS_RAW:
     {
-        mavlink_msg_gps_raw_send(chan,timeStamp,g_gps->status(),
-			g_gps->latitude/1.0e7,g_gps->longitude/1.0e7,g_gps->altitude/100.0,
-			g_gps->hdop,0.0,g_gps->ground_speed/100.0,g_gps->ground_course/100.0);
-        break;
+        mavlink_msg_gps_raw_send(
+				chan,
+				timeStamp,
+				g_gps->status(),
+				g_gps->latitude / 1.0e7,
+				g_gps->longitude / 1.0e7,
+				g_gps->altitude / 100.0,
+				g_gps->hdop,
+				0.0,
+				g_gps->ground_speed / 100.0,
+				g_gps->ground_course / 100.0);
+		break;
     }
+
     case MSG_SERVO_OUT:
     {
         uint8_t rssi = 1;
 		// normalized values scaled to -10000 to 10000
 		// This is used for HIL.  Do not change without discussing with HIL maintainers
-        mavlink_msg_rc_channels_scaled_send(chan,
-			10000*rc[0]->norm_output(),
-			10000*rc[1]->norm_output(),
-			10000*rc[2]->norm_output(),
-			10000*rc[3]->norm_output(),
-			0,0,0,0,rssi);
+        mavlink_msg_rc_channels_scaled_send(
+				chan,
+				10000 * g.channel_roll.norm_output(),
+				10000 * g.channel_pitch.norm_output(),
+				10000 * g.channel_throttle.norm_output(),
+				10000 * g.channel_rudder.norm_output(),
+				0,
+				0,
+				0,
+				0,
+				rssi);
         break;
     }
+
     case MSG_RADIO_IN:
     {
         uint8_t rssi = 1;
-        mavlink_msg_rc_channels_raw_send(chan,
-			rc[0]->radio_in,
-			rc[1]->radio_in,
-			rc[2]->radio_in,
-			rc[3]->radio_in,
-			0/*rc[4]->radio_in*/,       // XXX currently only 4 RC channels defined
-			0/*rc[5]->radio_in*/,
-			0/*rc[6]->radio_in*/,
-			0/*rc[7]->radio_in*/,
-			rssi);
+        mavlink_msg_rc_channels_raw_send(
+				chan,
+				g.channel_roll.radio_in,
+				g.channel_pitch.radio_in,
+				g.channel_throttle.radio_in,
+				g.channel_rudder.radio_in,
+				g.rc_5.radio_in,       // XXX currently only 4 RC channels defined
+				g.rc_6.radio_in,
+				g.rc_7.radio_in,
+				g.rc_8.radio_in,
+				rssi);
         break;
     }
+
     case MSG_RADIO_OUT:
     {
-        mavlink_msg_servo_output_raw_send(chan,
-			rc[0]->radio_out,
-			rc[1]->radio_out,
-			rc[2]->radio_out,
-			rc[3]->radio_out,
-			0/*rc[4]->radio_out*/,       // XXX currently only 4 RC channels defined
-			0/*rc[5]->radio_out*/,
-			0/*rc[6]->radio_out*/,
-			0/*rc[7]->radio_out*/);
+        mavlink_msg_servo_output_raw_send(
+				chan,
+				g.channel_roll.radio_out,
+				g.channel_pitch.radio_out,
+				g.channel_throttle.radio_out,
+				g.channel_rudder.radio_out,
+				g.rc_5.radio_out,       // XXX currently only 4 RC channels defined
+				g.rc_6.radio_out,
+				g.rc_7.radio_out,
+				g.rc_8.radio_out);
         break;
     }
+
     case MSG_VFR_HUD:
     {
-        mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)g_gps->ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
-			climb_rate, (int)rc[CH_THROTTLE]->servo_out);
+        mavlink_msg_vfr_hud_send(
+        		chan,
+				(float)airspeed / 100.0,
+				(float)g_gps->ground_speed / 100.0,
+				dcm.yaw_sensor,
+				current_loc.alt / 100.0,
+				climb_rate,
+				(int)g.channel_throttle.servo_out);
         break;
     }
 
-#if HIL_MODE != HIL_MODE_ATTITUDE
-    case MSG_RAW_IMU:
-    {
+	#if HIL_MODE != HIL_MODE_ATTITUDE
+	case MSG_RAW_IMU:
+	{
 		Vector3f accel = imu.get_accel();
 		Vector3f gyro = imu.get_gyro();
 		//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
 		//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
-        mavlink_msg_raw_imu_send(chan,timeStamp,
-            accel.x*1000.0/gravity,accel.y*1000.0/gravity,accel.z*1000.0/gravity,
-			gyro.x*1000.0,gyro.y*1000.0,gyro.z*1000.0,
-            compass.mag_x,compass.mag_y,compass.mag_z);
-        mavlink_msg_raw_pressure_send(chan,timeStamp,
-            adc.Ch(AIRSPEED_CH),barometer.RawPress,0,0);
-        break;
-    }
-#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
+		mavlink_msg_raw_imu_send(
+				chan,
+				timeStamp,
+				accel.x * 1000.0 / gravity,
+				accel.y * 1000.0 / gravity,
+				accel.z * 1000.0 / gravity,
+				gyro.x * 1000.0,
+				gyro.y * 1000.0,
+				gyro.z * 1000.0,
+				compass.mag_x,
+				compass.mag_y,
+				compass.mag_z);
+
+		mavlink_msg_raw_pressure_send(
+				chan,
+				timeStamp,
+				adc.Ch(AIRSPEED_CH),
+				barometer.RawPress,
+				0,
+				0);
+		break;
+	}
+	#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
 
     case MSG_GPS_STATUS:
     {
-        mavlink_msg_gps_status_send(chan,g_gps->num_sats,NULL,NULL,NULL,NULL,NULL);
+        mavlink_msg_gps_status_send(
+        		chan,
+        		g_gps->num_sats,
+        		NULL,
+        		NULL,
+        		NULL,
+        		NULL,
+        		NULL);
         break;
     }
 
     case MSG_CURRENT_WAYPOINT:
     {
-        mavlink_msg_waypoint_current_send(chan,g.waypoint_index);
+        mavlink_msg_waypoint_current_send(
+        		chan,
+        		g.waypoint_index);
         break;
     }
 
@@ -195,7 +273,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 
 void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
 {
-	mavlink_msg_statustext_send(chan,severity,(const int8_t*)str);
+	mavlink_msg_statustext_send(
+			chan,
+			severity,
+			(const int8_t*) str);
 }
 
 void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde
index a0c2a89578..89f7be5afe 100644
--- a/ArduCopterMega/commands.pde
+++ b/ArduCopterMega/commands.pde
@@ -12,12 +12,12 @@ void init_commands()
 void update_auto()
 {
 	if (g.waypoint_index == g.waypoint_total) {
-		return_to_launch();
-		//wp_index 			= 0;
+		do_RTL();
 	}
 }
 
-void reload_commands()
+// this is only used by an air-start
+void reload_commands_airstart()
 {
 	init_commands();
 	g.waypoint_index.load();        // XXX can we assume it's been loaded already by ::load_all?
@@ -31,7 +31,6 @@ struct Location get_wp_with_index(int i)
 	struct Location temp;
 	long mem;
 
-
 	// Find out proper location in memory by using the start_byte position + the index
 	// --------------------------------------------------------------------------------
 	if (i > g.waypoint_total) {
@@ -49,6 +48,11 @@ struct Location get_wp_with_index(int i)
 		mem += 4;
 		temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
 	}
+
+	// Add on home altitude if we are a nav command
+	if(temp.id < 50)
+		temp.alt += home.alt;
+
 	return temp;
 }
 
@@ -57,6 +61,12 @@ struct Location get_wp_with_index(int i)
 void set_wp_with_index(struct Location temp, int i)
 {
 	i = constrain(i, 0, g.waypoint_total.get());
+
+	if(i > 0 && temp.id < 50){
+		// remove home altitude if we are a nav command
+		temp.alt -= home.alt;
+	}
+
 	uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
 
 	eeprom_write_byte((uint8_t *)	mem, temp.id);
@@ -101,69 +111,18 @@ long read_alt_to_hold()
 		return g.RTL_altitude + home.alt;
 }
 
-void
-set_current_loc_here()
-{
-	//struct Location temp;
-	Location l = current_loc;
-	l.alt = get_altitude_above_home();
-	set_next_WP(&l);
-}
-
-void loiter_at_location()
-{
-	next_WP = current_loc;
-}
-
-void set_mode_loiter_home(void)
-{
-	control_mode = LOITER;
-	//crash_timer = 0;
-
-	next_WP = current_loc;
-	// Altitude to hold over home
-	// Set by configuration tool
-	// -------------------------
-	next_WP.alt = read_alt_to_hold();
-
-	// output control mode to the ground station
-	gcs.send_message(MSG_HEARTBEAT);
-
-	if (g.log_bitmask & MASK_LOG_MODE)
-		Log_Write_Mode(control_mode);
-}
 
 //********************************************************************************
 //This function sets the waypoint and modes for Return to Launch
 //********************************************************************************
 
-// add a new command at end of command set to RTL.
-void return_to_launch(void)
-{
-	//so we know where we are navigating from
-	next_WP = current_loc;
-
-	// home is WP 0
-	// ------------
-	g.waypoint_index.set_and_save(0);
-
-	// Loads WP from Memory
-	// --------------------
-	set_next_WP(&home);
-
-	// Altitude to hold over home
-	// Set by configuration tool
-	// -------------------------
-	next_WP.alt = read_alt_to_hold();
-	//send_message(SEVERITY_LOW,"Return To Launch");
-}
 
-struct Location get_LOITER_home_wp()
+Location get_LOITER_home_wp()
 {
 	// read home position
 	struct Location temp 	= get_wp_with_index(0);
 	temp.id 				= MAV_CMD_NAV_LOITER_UNLIM;
-	temp.alt 				= read_alt_to_hold() - home.alt;  // will be incremented up by home.alt in set_next_WP
+	temp.alt 				= read_alt_to_hold();
 	return temp;
 }
 
@@ -186,14 +145,11 @@ void set_next_WP(struct Location *wp)
 	// ---------------------
 	next_WP = *wp;
 
-	// offset the altitude relative to home position
-	// ---------------------------------------------
-	next_WP.alt += home.alt;
-
 	// used to control FBW and limit the rate of climb
 	// -----------------------------------------------
 	target_altitude = current_loc.alt;
 
+	// XXX YUCK!
 	if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
 		offset_altitude = next_WP.alt - prev_WP.alt;
 	else
@@ -210,10 +166,12 @@ void set_next_WP(struct Location *wp)
 	scaleLongUp 		= 1.0f/cos(rads);
 
 	// this is handy for the groundstation
-	wp_totalDistance 	= getDistance(&current_loc, &next_WP);
+	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
+	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing(&current_loc, &next_WP);
 
-	wp_distance 		= wp_totalDistance;
+	// to check if we have missed the WP
+	// ----------------------------
 	old_target_bearing 	= target_bearing;
 
 	// set a new crosstrack bearing
@@ -223,7 +181,6 @@ void set_next_WP(struct Location *wp)
 	gcs.print_current_waypoints();
 }
 
-
 // run this at setup on the ground
 // -------------------------------
 void init_home()
diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde
index dff3455c4a..904d9d0973 100644
--- a/ArduCopterMega/commands_logic.pde
+++ b/ArduCopterMega/commands_logic.pde
@@ -1,157 +1,59 @@
+/********************************************************************************/
+// Command Event Handlers
+/********************************************************************************/
 void
-handle_no_commands()
-{
-	switch (control_mode){
-		case LAND:
-			// don't get a new command
-			break;
-
-		default:
-			next_command = get_LOITER_home_wp();
-			//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
-			//SendDebugln(next_command.id,DEC);
-			break;
-	}
-}
-
-void
-handle_process_must(byte id)
+handle_process_must()
 {
 	// reset navigation integrators
 	// -------------------------
 	reset_I();
 
-	switch(id){
-		case MAV_CMD_NAV_TAKEOFF:	// TAKEOFF!
-			// pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
-			takeoff_altitude 	= (int)next_command.alt;
-			next_WP.lat 		= current_loc.lat + 10;	// so we don't have bad calcs
-			next_WP.lng 		= current_loc.lng + 10;	// so we don't have bad calcs
-			next_WP.alt			= current_loc.alt + takeoff_altitude;
-			takeoff_complete 	= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
-			//set_next_WP(&next_WP);
+	switch(next_command.id){
+
+		case MAV_CMD_NAV_TAKEOFF:
+			do_takeoff();
 			break;
 
 		case MAV_CMD_NAV_WAYPOINT:	// Navigate to Waypoint
+			do_nav_wp();
 			break;
 
-		//case MAV_CMD_NAV_R_WAYPOINT:	// Navigate to Waypoint
-		//	next_command.lat 	+= home.lat;	// offset from home location
-		//	next_command.lng 	+= home.lng;	// offset from home location
-
-			// we've recalculated the WP so we need to set it again
-		//	set_next_WP(&next_command);
-		//	break;
-
 		case MAV_CMD_NAV_LAND:	// LAND to Waypoint
-			velocity_land		= 1000;
-			next_WP.lat 		= current_loc.lat;
-			next_WP.lng 		= current_loc.lng;
-			next_WP.alt 		= home.alt;
-			land_complete 		= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
-			break;
-
-		/*
-		case MAV_CMD_ALTITUDE:	//
-			next_WP.lat 		= current_loc.lat + 10;	// so we don't have bad calcs
-			next_WP.lng 		= current_loc.lng + 10;	// so we don't have bad calcs
-			break;
-		*/
-
-		case MAV_CMD_NAV_LOITER_UNLIM:	// Loiter indefinitely
-			break;
-
-		case MAV_CMD_NAV_LOITER_TURNS:	// Loiter N Times
-			break;
-
-		case MAV_CMD_NAV_LOITER_TIME:
+			do_land();
 			break;
 
 		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-			return_to_launch();
+			do_RTL();
 			break;
 	}
 }
 
 void
-handle_process_may(byte id)
+handle_process_may()
 {
-	switch(id){
+	switch(next_command.id){
 
-		case MAV_CMD_CONDITION_DELAY:	// Navigate to Waypoint
-			delay_start = millis();
-			delay_timeout  = next_command.lat;
+		case MAV_CMD_CONDITION_DELAY:
+			do_delay();
 			break;
 
-		//case MAV_CMD_NAV_LAND_OPTIONS:	// Land this puppy
-		//	break;
+		case MAV_CMD_CONDITION_CHANGE_ALT:
+			do_change_alt();
+			break;
 
 		case MAV_CMD_CONDITION_YAW:
-			// p1:		bearing
-			// alt:		speed
-			// lat:		direction (-1,1),
-			// lng:		rel (1) abs (0)
-
-			// target angle in degrees
-			command_yaw_start		= nav_yaw; // current position
-			command_yaw_start_time 	= millis();
-
-			// which direction to turn
-			// 1 = clockwise, -1 = counterclockwise
-			command_yaw_dir		= next_command.lat;
-
-			// 1 = Relative or 0 = Absolute
-			if (next_command.lng == 1) {
-				// relative
-				command_yaw_dir  = (command_yaw_end > 0) ? 1 : -1;
-				command_yaw_end += nav_yaw;
-				command_yaw_end = wrap_360(command_yaw_end);
-			}else{
-				// absolute
-				command_yaw_end 	= next_command.p1 * 100;
-			}
-
-
-			// if unspecified go 10° a second
-			if(command_yaw_speed == 0)
-				command_yaw_speed = 10;
-
-			// if unspecified go clockwise
-			if(command_yaw_dir == 0)
-				command_yaw_dir = 1;
-
-			// calculate the delta travel
-			if(command_yaw_dir == 1){
-				if(command_yaw_start > command_yaw_end){
-					command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
-				}else{
-					command_yaw_delta = command_yaw_end - command_yaw_start;
-				}
-			}else{
-				if(command_yaw_start > command_yaw_end){
-					command_yaw_delta = command_yaw_start - command_yaw_end;
-				}else{
-					command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
-				}
-			}
-			command_yaw_delta = wrap_360(command_yaw_delta);
-
-			// rate to turn deg per second - default is ten
-			command_yaw_speed 	= next_command.alt;
-			command_yaw_time 	= command_yaw_delta / command_yaw_speed;
-			//9000 turn in 10 seconds
-			//command_yaw_time = 9000/ 10 = 900° per second
-
+			do_yaw();
 			break;
 
 		default:
 			break;
-	}}
+	}
+}
 
 void
-handle_process_now(byte id)
+handle_process_now()
 {
-	switch(id){
+	switch(next_command.id){
 		case MAV_CMD_DO_SET_HOME:
 			init_home();
 			break;
@@ -161,125 +63,327 @@ handle_process_now(byte id)
 			break;
 
 		case MAV_CMD_DO_SET_SERVO:
-			//Serial.print("MAV_CMD_DO_SET_SERVO ");
-			//Serial.print(next_command.p1,DEC);
-			//Serial.print(" PWM: ");
-			//Serial.println(next_command.alt,DEC);
-			APM_RC.OutputCh(next_command.p1, next_command.alt);
+			do_set_servo();
 			break;
 
 		case MAV_CMD_DO_SET_RELAY:
-			if (next_command.p1 == 0) {
-				relay_on();
-			} else if (next_command.p1 == 1) {
-				relay_off();
-			}else{
-				relay_toggle();
-			}
+			do_set_relay();
 			break;
 	}
 }
 
-// Verify commands
-// ---------------
-void verify_must()
+void
+handle_no_commands()
 {
-	switch(command_must_ID) {
+	switch (control_mode){
+		case LAND:
+			// don't get a new command
+			break;
 
-		/*case MAV_CMD_ALTITUDE:
-				if (abs(next_WP.alt - current_loc.alt) < 100){
-					command_must_index 	= 0;
-				}
+		//case GCS_AUTO:
+		//	set_mode(LOITER);
+
+		default:
+			set_mode(RTL);
+			//next_command = get_LOITER_home_wp();
+			//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
+			//SendDebugln(next_command.id,DEC);
 			break;
-		*/
+	}
+}
+
+bool verify_must()
+{
+	switch(command_must_ID) {
 
 		case MAV_CMD_NAV_TAKEOFF:	// Takeoff!
-			if (current_loc.alt > (next_WP.alt -100)){
-				command_must_index 	= 0;
-				takeoff_complete 	= true;
-			}
+			return verify_takeoff();
 			break;
 
 		case MAV_CMD_NAV_LAND:
-			// 10 - 9  = 1
-			velocity_land  = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
-			old_alt = current_loc.alt;
-			if(velocity_land == 0){
-				land_complete 		= true;
-				command_must_index 	= 0;
-			}
-			update_crosstrack();
-
+			return verify_land();
 			break;
 
 		case MAV_CMD_NAV_WAYPOINT:	// reach a waypoint
-			update_crosstrack();
-			if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-				//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
-				//SendDebugln(command_must_index,DEC);
-				char message[50];
-				sprintf(message,"Reached Waypoint #%i",command_must_index);
-				gcs.send_text(SEVERITY_LOW,message);
-
-				// clear the command queue;
-				command_must_index 	= 0;
-			}
-			// add in a more complex case
-			// Doug to do
-			if(loiter_sum > 300){
-				send_message(SEVERITY_MEDIUM,"Missed WP");
-				command_must_index 	= 0;
-			}
+			return verify_nav_wp();
 			break;
 
-		case MAV_CMD_NAV_LOITER_TURNS:	// LOITER N times
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+			return verify_RTL();
 			break;
 
-		case MAV_CMD_NAV_LOITER_TIME:	// loiter N milliseconds
+		default:
+			//gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
+			return false;
 			break;
+	}
+}
 
-		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-			if (wp_distance <= g.waypoint_radius) {
-				gcs.send_text(SEVERITY_LOW,"Reached home");
-				command_must_index 	= 0;
-			}
+bool verify_may()
+{
+	switch(command_may_ID) {
+
+		case MAV_CMD_CONDITION_ANGLE:
+			return verify_yaw();
 			break;
 
-		//case MAV_CMD_NAV_LOITER_UNLIM:	// Just plain LOITER
-		//	break;
+		case MAV_CMD_CONDITION_DELAY:
+			return verify_delay();
+			break;
 
+		case MAV_CMD_CONDITION_CHANGE_ALT:
+			return verify_change_alt();
+			break;
 
 		default:
-			gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
+			//gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current May commands");
+			return false;
 			break;
+
 	}
 }
 
-void verify_may()
+/********************************************************************************/
+// Must command implementations
+/********************************************************************************/
+
+void
+do_takeoff()
 {
-	float power;
-	switch(command_may_ID) {
+	Location temp 		= current_loc;
+	temp.alt			= next_command.alt;
+	takeoff_complete 	= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
 
-		case MAV_CMD_CONDITION_ANGLE:
-			if((millis() - command_yaw_start_time) > command_yaw_time){
-				command_must_index 	= 0;
-				nav_yaw = command_yaw_end;
-			}else{
-				// else we need to be at a certain place
-				// power is a ratio of the time : .5 = half done
-				power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
-				nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
-			}
-			break;
+	set_next_WP(&temp);
+}
 
-		case MAV_CMD_CONDITION_DELAY:
-			if ((millis() - delay_start) > delay_timeout){
-				command_may_index = 0;
-				delay_timeout = 0;
-			}
+bool
+verify_takeoff()
+{
+	if (current_loc.alt > next_WP.alt){
+		takeoff_complete 	= true;
+		return true;
+	}else{
+		return false;
+	}
+}
+
+void
+do_nav_wp()
+{
+	set_next_WP(&next_command);
+}
+
+bool
+verify_nav_wp()
+{
+	update_crosstrack();
+	if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
+		//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
+		//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
+	// Doug to do
+	if(loiter_sum > 300){
+		send_message(SEVERITY_MEDIUM,"Missed WP");
+		return true;
+	}
+	return false;
+}
+
+void
+do_land()
+{
+	land_complete 		= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
+	velocity_land		= 1000;
+
+	Location temp 		= current_loc;
+	temp.alt 			= home.alt;
+
+	set_next_WP(&temp);
+}
+
+bool
+verify_land()
+{
+	update_crosstrack();
+
+	velocity_land  = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
+	old_alt = current_loc.alt;
+
+	if(velocity_land == 0){
+		land_complete 		= true;
+		return true;
+	}
+
+	return false;
+}
+
+// add a new command at end of command set to RTL.
+void
+do_RTL()
+{
+	Location temp 	= home;
+	temp.alt 		= read_alt_to_hold();
+
+	//so we know where we are navigating from
+	next_WP = current_loc;
+
+	// Loads WP from Memory
+	// --------------------
+	set_next_WP(&temp);
+}
+
+bool
+verify_RTL()
+{
+	if (wp_distance <= g.waypoint_radius) {
+		gcs.send_text(SEVERITY_LOW,"Reached home");
+		return true;
+	}else{
+		return false;
+	}
+}
+
+/********************************************************************************/
+// May command implementations
+/********************************************************************************/
+
+void
+do_yaw()
+{
+	// p1:		bearing
+	// alt:		speed
+	// lat:		direction (-1,1),
+	// lng:		rel (1) abs (0)
+
+	// target angle in degrees
+	command_yaw_start		= nav_yaw; // current position
+	command_yaw_start_time 	= millis();
+
+	// which direction to turn
+	// 1 = clockwise, -1 = counterclockwise
+	command_yaw_dir		= next_command.lat;
+
+	// 1 = Relative or 0 = Absolute
+	if (next_command.lng == 1) {
+		// relative
+		command_yaw_dir  = (command_yaw_end > 0) ? 1 : -1;
+		command_yaw_end += nav_yaw;
+		command_yaw_end = wrap_360(command_yaw_end);
+	}else{
+		// absolute
+		command_yaw_end 	= next_command.p1 * 100;
+	}
+
+
+	// if unspecified go 10° a second
+	if(command_yaw_speed == 0)
+		command_yaw_speed = 10;
+
+	// if unspecified go clockwise
+	if(command_yaw_dir == 0)
+		command_yaw_dir = 1;
+
+	// calculate the delta travel
+	if(command_yaw_dir == 1){
+		if(command_yaw_start > command_yaw_end){
+			command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
+		}else{
+			command_yaw_delta = command_yaw_end - command_yaw_start;
+		}
+	}else{
+		if(command_yaw_start > command_yaw_end){
+			command_yaw_delta = command_yaw_start - command_yaw_end;
+		}else{
+			command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
+		}
+	}
+	command_yaw_delta = wrap_360(command_yaw_delta);
+
+	// rate to turn deg per second - default is ten
+	command_yaw_speed 	= next_command.alt;
+	command_yaw_time 	= command_yaw_delta / command_yaw_speed;
+	//9000 turn in 10 seconds
+	//command_yaw_time = 9000/ 10 = 900° per second
+}
+
+bool
+verify_yaw()
+{
+	if((millis() - command_yaw_start_time) > command_yaw_time){
+		nav_yaw = command_yaw_end;
+		return true;
+	}else{
+		// else we need to be at a certain place
+		// power is a ratio of the time : .5 = half done
+		float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
+		nav_yaw 	= command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
+		return false;
+	}
+}
 
-		//case CMD_LAND_OPTIONS:
-		//	break;
+void
+do_delay()
+{
+	delay_start = millis();
+	delay_timeout  = next_command.lat;
+}
+
+bool
+verify_delay()
+{
+	if ((millis() - delay_start) > delay_timeout){
+		delay_timeout = 0;
+		return true;
+	}else{
+		return false;
 	}
 }
 
+void
+do_change_alt()
+{
+	Location temp 	= next_WP;
+	temp.alt 		= next_command.alt + home.alt;
+	set_next_WP(&temp);
+}
+
+bool
+verify_change_alt()
+{
+	if(abs(current_loc.alt - next_WP.alt) < 100){
+		return true;
+	}else{
+		return false;
+	}
+}
+
+/********************************************************************************/
+// Now command implementations
+/********************************************************************************/
+
+void do_hold_position()
+{
+	set_next_WP(&current_loc);
+}
+
+void do_set_servo()
+{
+	APM_RC.OutputCh(next_command.p1, next_command.alt);
+}
+
+void do_set_relay()
+{
+	if (next_command.p1 == 0) {
+		relay_on();
+	} else if (next_command.p1 == 1) {
+		relay_off();
+	}else{
+		relay_toggle();
+	}
+}
diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde
index 1464c1a280..becdb0a1ec 100644
--- a/ArduCopterMega/commands_process.pde
+++ b/ArduCopterMega/commands_process.pde
@@ -5,43 +5,55 @@ void update_commands(void)
 	// This function loads commands into three buffers
 	// when a new command is loaded, it is processed with process_XXX()
 	// ----------------------------------------------------------------
-	if((home_is_set == false) || (control_mode != AUTO)){
+	if(home_is_set == false){
 		return;	// don't do commands
 	}
 
 	if(control_mode == AUTO){
-		load_next_command();
+		load_next_command_from_EEPROM();
 		process_next_command();
+	}else if(control_mode == GCS_AUTO){
+		/*if( there is a command recieved )
+			process_next_command();
+		*/
 	}
-
-	//verify_must();
-	//verify_may();
 }
 
+void verify_commands(void)
+{
+	if(verify_must()){
+		command_must_index 	= NO_COMMAND;
+	}
+
+	if(verify_may()){
+		command_may_index 	= NO_COMMAND;
+	}
+}
 
-void load_next_command()
+void load_next_command_from_EEPROM()
 {
-	// fetch next command if it's empty
-	// --------------------------------
-	if(next_command.id == CMD_BLANK){
+	// fetch next command if the next command queue is empty
+	// -----------------------------------------------------
+	if(next_command.id == NO_COMMAND){
+
 		next_command = get_wp_with_index(g.waypoint_index + 1);
-		if(next_command.id != CMD_BLANK){
+
+		//if(next_command.id != NO_COMMAND){
 			//SendDebug("MSG <load_next_command> fetch found new cmd from list at index ");
 			//SendDebug((g.waypoint_index + 1),DEC);
 			//SendDebug(" with cmd id ");
 			//SendDebugln(next_command.id,DEC);
-		}
+		//}
 	}
 
 	// If the preload failed, return or just Loiter
 	// generate a dynamic command for RTL
 	// --------------------------------------------
-	if(next_command.id == CMD_BLANK){
+	if(next_command.id == NO_COMMAND){
 		// we are out of commands!
 		gcs.send_text(SEVERITY_LOW,"out of commands!");
 		//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
 		//SendDebugln(g.waypoint_index,DEC);
-
 		handle_no_commands();
 	}
 }
@@ -88,11 +100,12 @@ void process_next_command()
 			Log_Write_Cmd(g.waypoint_index, &next_command);
 		process_now();
 	}
-
 }
+
 /*
 These functions implement the waypoint commands.
 */
+
 void process_must()
 {
 	//SendDebug("process must index: ");
@@ -109,12 +122,12 @@ void process_must()
 
 	// loads the waypoint into Next_WP struct
 	// --------------------------------------
-	set_next_WP(&next_command);
+	//set_next_WP(&next_command);
 
 	// invalidate command so a new one is loaded
 	// -----------------------------------------
-	next_command.id = 0;
-	handle_process_must(command_must_ID);
+	handle_process_must();
+	next_command.id = NO_COMMAND;
 }
 
 void process_may()
@@ -123,14 +136,14 @@ void process_may()
 	//Serial.println(g.waypoint_index,DEC);
 	command_may_ID = next_command.id;
 
-	// invalidate command so a new one is loaded
-	// -----------------------------------------
-	next_command.id = 0;
-
 	gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
 	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
 
-	handle_process_may(command_may_ID);
+	handle_process_may();
+
+	// invalidate command so a new one is loaded
+	// -----------------------------------------
+	next_command.id = NO_COMMAND;
 }
 
 void process_now()
@@ -139,14 +152,12 @@ void process_now()
 	//Serial.print("process_now cmd# ");
 	//Serial.println(g.waypoint_index,DEC);
 
-	byte id = next_command.id;
+	gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
+	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
+	handle_process_now();
 
 	// invalidate command so a new one is loaded
 	// -----------------------------------------
-	next_command.id = 0;
-
-	gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
-	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
-	handle_process_now(id);
+	next_command.id = NO_COMMAND;
 }
 
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index c2738eb8c7..8ed5a44d87 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -87,7 +87,7 @@
 #define ALT_HOLD 2			// AUTO control
 #define FBW 3				// AUTO control
 #define AUTO 4				// AUTO control
-#define LOITER 5				// AUTO control
+#define GCS_AUTO 5				// AUTO control
 #define POSITION_HOLD 6		// Hold a single location
 #define RTL 7				// AUTO control
 #define TAKEOFF 8			// controlled decent rate
@@ -95,7 +95,9 @@
 #define NUM_MODES 10
 
 // Commands - Note that APM now uses a subset of the MAVLink protocol commands.  See enum MAV_CMD in the GCS_Mavlink library
-#define CMD_BLANK 0x00 // there is no command stored in the mem location requested
+#define CMD_BLANK 0 // there is no command stored in the mem location requested
+#define NO_COMMAND 0
+
 
 
 //repeating events
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 6637ff8f32..ddeed010c9 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -36,7 +36,6 @@ void
 set_servos_4()
 {
 	static byte num;
-	static byte counteri;
 	int out_min;
 
 	// Quadcopter mix
@@ -292,3 +291,4 @@ set_servos_4()
 		g.rc_4.control_in = ToDeg(dcm.yaw);
 	}
  }
+
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 026e35c6ec..b224626f48 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -18,7 +18,7 @@ void navigate()
 
 	// waypoint distance from plane
 	// ----------------------------
-	wp_distance = getDistance(&current_loc, &next_WP);
+	wp_distance = get_distance(&current_loc, &next_WP);
 
 	if (wp_distance < 0){
 		gcs.send_text(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
@@ -184,7 +184,7 @@ long get_altitude_above_home(void)
 	return current_loc.alt - home.alt;
 }
 
-long getDistance(struct Location *loc1, struct Location *loc2)
+long get_distance(struct Location *loc1, struct Location *loc2)
 {
 	if(loc1->lat == 0 || loc1->lng == 0)
 		return -1;
diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde
index 17e119cf68..32997c6a29 100644
--- a/ArduCopterMega/radio.pde
+++ b/ArduCopterMega/radio.pde
@@ -70,7 +70,13 @@ void read_radio()
 
 	//throttle_failsafe(g.rc_3.radio_in);
 
-	//Serial.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, g.rc_4.control_in);
+	/*
+	Serial.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,
+				g.rc_4.control_in);
+	*/
 }
 
 void throttle_failsafe(uint16_t pwm)
diff --git a/ArduCopterMega/read_me.text b/ArduCopterMega/read_me.text
index 73dcd9b9e1..719fc42792 100644
--- a/ArduCopterMega/read_me.text
+++ b/ArduCopterMega/read_me.text
@@ -15,11 +15,11 @@ CLI interactive setup - You must go through each item and set the values to matc
 
 "setup" menu:
 erase			- run this first! erases bad values from EEPROMS just in case
-reset			- Performs factory reset and initialization of EEPROM values 
+reset			- Performs factory reset and initialization of EEPROM values
 radio			- records the limits of ALL radio channels - very important!
 pid				- restores default PID values
 frame			- sets your frame config: [x, +, tri]
-motors			- interactive setup of your ESC and motors, enter this mode, then plug-in battery, 
+motors			- interactive setup of your ESC and motors, enter this mode, then plug-in battery,
 					point at motors to make them spin
 					throttle will output full range to each motor - this is good for esc calibration
 level			- sets initial value of accelerometers - hold copter level
@@ -55,7 +55,7 @@ Alt_hold   - You need to set your g. value by toggling ch_7 for less than 1 seco
 			 altitude is controlled by the throttle lever using absolute position - from 0 to 40 meters.
 			 this control method will change after testing.
 FBW 	   - Simulates GPS Hold with the sticks being the position offset. Manual Throttle.
-position_hold 
+position_hold
 			- When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden temporarily with the radio.
 RTL 		- Will try and fly back to home at the current altitude.
 Auto 		- Will fly the mission loaded by the Waypoint writer
@@ -64,7 +64,6 @@ Auto 		- Will fly the mission loaded by the Waypoint writer
 what's new to commands for ACM:
 - CMD_ANGLE - will set the desired yaw with control of angle/second and direction.
 - CMD_ALTITUDE - will send the copter up from current position to desired altitude
-- CMD_R_WAYPOINT - is just like a waypoint but relative to home
 - CMD_TRACK_WAYPOINT - coming soon, will point the copter at the waypoint no matter the position
 
 Special note:
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 08dad51f3b..fc18e96a79 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -261,11 +261,11 @@ void set_mode(byte mode)
 			break;
 
 		case STABILIZE:
-			set_current_loc_here();
+			do_hold_position();
 			break;
 
 		case ALT_HOLD:
-			set_current_loc_here();
+			do_hold_position();
 			break;
 
 		case AUTO:
@@ -273,11 +273,11 @@ void set_mode(byte mode)
 			break;
 
 		case POSITION_HOLD:
-			set_current_loc_here();
+			do_hold_position();
 			break;
 
 		case RTL:
-			return_to_launch();
+			do_RTL();
 			break;
 
 		case TAKEOFF:
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 5877bcd035..9decd17c1a 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -136,10 +136,10 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 		g.rc_4.calc_pwm();
 
 		Serial.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),
-							(g.rc_4.control_in),
+							g.rc_1.control_in,
+							g.rc_2.control_in,
+							g.rc_3.control_in,
+							g.rc_4.control_in,
 							g.rc_5.control_in,
 							g.rc_6.control_in,
 							g.rc_7.control_in);