Copter: soften loiter target when maybe landed

Applies to auto's land, land, loiter, pos hold and rtl flight modes

ArduCopter/config.h

@@ -459,6 +459,9 @@
 #ifndef LAND_DETECTOR_TRIGGER
  # define LAND_DETECTOR_TRIGGER 50    // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete.
 #endif
+#ifndef LAND_DETECTOR_MAYBE_TRIGGER
+ # define LAND_DETECTOR_MAYBE_TRIGGER   10  // number of 50hz iterations with near zero climb rate and low throttle that means we might be landed (used to reset horizontal position targets to prevent tipping over)
+#endif
 #ifndef LAND_DETECTOR_CLIMBRATE_MAX
 # define LAND_DETECTOR_CLIMBRATE_MAX    30  // vehicle climb rate must be between -30 and +30 cm/s
 #endif

ArduCopter/control_auto.pde

@@ -287,6 +287,11 @@ static void auto_land_run()
         return;
     }
 
+    // init loiter targets when maybe landed
+    if(land_maybe_complete()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // process pilot's input
     if (!failsafe.radio) {
         if (g.land_repositioning) {

ArduCopter/control_land.pde

@@ -73,6 +73,11 @@ static void land_gps_run()
         return;
     }
 
+    // relax loiter target if we might be landed
+    if(land_maybe_complete()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // process pilot inputs
     if (!failsafe.radio) {
         if (g.land_repositioning) {
@@ -187,6 +192,11 @@ static float get_throttle_land()
     }
 }
 
+static bool land_maybe_complete()
+{
+    return (ap.land_complete || land_detector > LAND_DETECTOR_MAYBE_TRIGGER);
+}
+
 // update_land_detector - checks if we have landed and updates the ap.land_complete flag
 // called at 50hz
 static void update_land_detector()

ArduCopter/control_loiter.pde

@@ -68,6 +68,11 @@ static void loiter_run()
         wp_nav.clear_pilot_desired_acceleration();
     }
 
+    // relax loiter target if we might be landed
+    if (land_maybe_complete()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // when landed reset targets and output zero throttle
     if (ap.land_complete) {
         wp_nav.init_loiter_target();

ArduCopter/control_poshold.pde

@@ -183,6 +183,11 @@ static void poshold_run()
         }
     }
 
+    // relax loiter target if we might be landed
+    if(land_maybe_complete()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // if landed initialise loiter targets, set throttle to zero and exit
     if (ap.land_complete) {
         wp_nav.init_loiter_target();

ArduCopter/control_rtl.pde

@@ -349,6 +349,11 @@ static void rtl_land_run()
         return;
     }
 
+    // relax loiter target if we might be landed
+    if(land_maybe_complete()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // process pilot's input
     if (!failsafe.radio) {
         if (g.land_repositioning) {