From 9a1a999fba4dec7c6f38ebcf3bcabb66c5878b72 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sun, 28 Jul 2013 10:47:19 +0900
Subject: [PATCH] Copter: landing check changes

---
 ArduCopter/ArduCopter.pde | 30 ++++++++++++++++++++----------
 ArduCopter/Attitude.pde   | 19 +++++++++++++------
 2 files changed, 33 insertions(+), 16 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 70aaa18374..b886e45f93 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1978,17 +1978,27 @@ void update_throttle_mode(void)
             // alt hold plus pilot input of climb rate
             pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
 
-            // clear i term when we're taking off
-            if (pilot_climb_rate > 0 && ap.land_complete) {
-                set_throttle_takeoff();
+            // special handling if we have landed
+            if (ap.land_complete) {
+                if (pilot_climb_rate > 0) {
+                    // indicate we are taking off
+                    set_land_complete(false);
+                    // clear i term when we're taking off
+                    set_throttle_takeoff();
+                }else{
+                    // move throttle to minimum to keep us on the ground
+                    set_throttle_out(g.throttle_min, false);
+                }
             }
-
-            if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
-                // if sonar is ok, use surface tracking
-                get_throttle_surface_tracking(pilot_climb_rate);    // this function calls set_target_alt_for_reporting for us
-            }else{
-                // if no sonar fall back stabilize rate controller
-                get_throttle_rate_stabilized(pilot_climb_rate);     // this function calls set_target_alt_for_reporting for us
+            // check land_complete flag again in case it was changed above
+            if (!ap.land_complete) {
+                if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
+                    // if sonar is ok, use surface tracking
+                    get_throttle_surface_tracking(pilot_climb_rate);    // this function calls set_target_alt_for_reporting for us
+                }else{
+                    // if no sonar fall back stabilize rate controller
+                    get_throttle_rate_stabilized(pilot_climb_rate);     // this function calls set_target_alt_for_reporting for us
+                }
             }
         }else{
             // pilot's throttle must be at zero so keep motors off
diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index b15f1c8302..16dc17c269 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -863,8 +863,14 @@ void throttle_accel_deactivate()
 static void
 set_throttle_takeoff()
 {
-    if (g.pid_throttle_accel.get_integrator() < 0)
+    // set alt target
+    if (controller_desired_alt < current_loc.alt) {
+        controller_desired_alt = current_loc.alt + 20;
+    }
+    // clear i term from acceleration controller
+    if (g.pid_throttle_accel.get_integrator() < 0) {
         g.pid_throttle_accel.reset_I();
+    }
 }
 
 // get_throttle_accel - accelerometer based throttle controller
@@ -1178,12 +1184,13 @@ static bool update_land_detector()
 {
     // detect whether we have landed by watching for low climb rate and minimum throttle
     if (abs(climb_rate) < 20 && motors.limit.throttle_lower) {
-        // run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target)
-        if( land_detector < LAND_DETECTOR_TRIGGER ) {
-            land_detector++;
-        }else{
-            if(!ap.land_complete) {
+        if (!ap.land_complete) {
+            // run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target)
+            if( land_detector < LAND_DETECTOR_TRIGGER) {
+                land_detector++;
+            }else{
                 set_land_complete(true);
+                land_detector = 0;
             }
         }
     }else{