Added auto_land to RTL

ArduCopter/ArduCopter.pde

@@ -787,6 +787,8 @@ static uint32_t         nav_loopTimer;
 static float 			dTnav;
 // Counters for branching from 4 minute control loop used to save Compass offsets
 static int16_t			superslow_loopCounter;
+// RTL Autoland Timer
+static uint32_t 		auto_land_timer;
 
 
 // Tracks if GPS is enabled based on statup routine
@@ -1606,10 +1608,8 @@ static void update_navigation()
 
 		case RTL:
 			if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
-				//lets just jump to Loiter Mode after RTL
-				//if(land after RTL)
-				//set_mode(LAND);
-				//else
+				// if this value > 0, we are set to trigger auto_land after 30 seconds
+				auto_land_timer = millis();
 				set_mode(LOITER);
 
 			}else{
@@ -1642,6 +1642,12 @@ static void update_navigation()
 				wp_control = LOITER_MODE;
 			}
 
+			if(auto_land_timer != 0 && (millis() - auto_land_timer) > 30000){
+				// just to make sure we clear the timer
+				auto_land_timer = 0;
+				set_mode(land);
+			}
+
 			// calculates the desired Roll and Pitch
 			update_nav_wp();
 			break;