From cf5e0b3a1baf5f0eb6de1c037f126c4abc6c954b Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Sat, 7 Jan 2012 22:26:56 -0800
Subject: [PATCH] converted auto land to use the mission planner version
 Removed gate that looked for already set control_mode. Wasn't compatible with
 failsafe

---
 ArduCopter/system.pde | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 96d6179b64..a57413b9eb 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -426,10 +426,11 @@ static void startup_ground(void)
 
 static void set_mode(byte mode)
 {
-	if(control_mode == mode){
+	//if(control_mode == mode){
 		// don't switch modes if we are already in the correct mode.
-		return;
-	}
+	//	Serial.print("Double mode\n");
+		//return;
+	//}
 
 	// if we don't have GPS lock
 	if(home_is_set == false){
@@ -458,6 +459,9 @@ static void set_mode(byte mode)
 	// do not auto_land if we are leaving RTL
 	auto_land_timer = 0;
 
+	// if we change modes, we must clear landed flag
+	land_complete 	= false;
+
 	// debug to Serial terminal
 	Serial.println(flight_mode_strings[control_mode]);
 
@@ -531,12 +535,10 @@ static void set_mode(byte mode)
 			break;
 
 		case LAND:
-			land_complete 	= false;
-			yaw_mode 		= YAW_HOLD;
-			roll_pitch_mode = ROLL_PITCH_AUTO;
+			yaw_mode 		= LOITER_YAW;
+			roll_pitch_mode = LOITER_RP;
 			throttle_mode 	= THROTTLE_AUTO;
-			next_WP 		= current_loc;
-			next_WP.alt 	= 0;
+			do_land();
 			break;
 
 		case RTL: