Copter: rework land detector logic

ArduCopter/AP_State.cpp

@@ -94,21 +94,6 @@ void Copter::set_failsafe_gcs(bool b)
     failsafe.gcs = b;
 }
 
-// ---------------------------------------------
-void Copter::set_land_complete(bool b)
-{
-    // if no change, exit immediately
-    if( ap.land_complete == b )
-        return;
-
-    if(b){
-        Log_Write_Event(DATA_LAND_COMPLETE);
-    }else{
-        Log_Write_Event(DATA_NOT_LANDED);
-    }
-    ap.land_complete = b;
-}
-
 // ---------------------------------------------
 
 // set land complete maybe flag

ArduCopter/control_auto.cpp

@@ -294,7 +294,7 @@ void Copter::auto_land_run()
     }
 
     // relax loiter targets if we might be landed
-    if (land_complete_maybe()) {
+    if (ap.land_complete_maybe) {
         wp_nav.loiter_soften_for_landing();
     }
 

ArduCopter/control_brake.cpp

@@ -46,7 +46,7 @@ void Copter::brake_run()
     }
 
     // relax stop target if we might be landed
-    if (land_complete_maybe()) {
+    if (ap.land_complete_maybe) {
         wp_nav.loiter_soften_for_landing();
     }
 

ArduCopter/control_land.cpp

@@ -72,7 +72,7 @@ void Copter::land_gps_run()
     }
 
     // relax loiter target if we might be landed
-    if (land_complete_maybe()) {
+    if (ap.land_complete_maybe) {
         wp_nav.loiter_soften_for_landing();
     }
 

ArduCopter/control_loiter.cpp

@@ -78,7 +78,7 @@ void Copter::loiter_run()
     }
 
     // relax loiter target if we might be landed
-    if (land_complete_maybe()) {
+    if (ap.land_complete_maybe) {
         wp_nav.loiter_soften_for_landing();
     }
 

ArduCopter/control_poshold.cpp

@@ -172,7 +172,7 @@ void Copter::poshold_run()
     }
 
     // relax loiter target if we might be landed
-    if (land_complete_maybe()) {
+    if (ap.land_complete_maybe) {
         wp_nav.loiter_soften_for_landing();
     }
 

ArduCopter/control_rtl.cpp

@@ -345,7 +345,7 @@ void Copter::rtl_land_run()
     }
 
     // relax loiter target if we might be landed
-    if (land_complete_maybe()) {
+    if (ap.land_complete_maybe) {
         wp_nav.loiter_soften_for_landing();
     }
 

ArduCopter/land_detector.cpp

@@ -4,13 +4,7 @@
 
 
 // counter to verify landings
-static uint32_t land_detector = ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE; // we assume we are landed
-
-// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
-bool Copter::land_complete_maybe()
-{
-    return (ap.land_complete || ap.land_complete_maybe);
-}
+static uint32_t land_detector_count = 0;
 
 // update_land_detector - checks if we have landed and updates the ap.land_complete flag
 // called at MAIN_LOOP_RATE
@@ -24,7 +18,22 @@ void Copter::update_land_detector()
     // range finder :                       tend to be problematic at very short distances
     // input throttle :                     in slow land the input throttle may be only slightly less than hover
 
-   
+    Vector3f accel_ef = ahrs.get_accel_ef_blended();
+    accel_ef.z += GRAVITY_MSS;
+
+    // lowpass filter on accel
+    accel_ef = land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
+
+    if (!motors.armed()) {
+        // if disarmed, always landed.
+        set_land_complete(true);
+    } else if (ap.land_complete) {
+        // if throttle output is high then clear landing flag
+        if (motors.get_throttle() > get_non_takeoff_throttle()) {
+            set_land_complete(false);
+        }
+    } else {
+
 #if FRAME_CONFIG == HELI_FRAME
     // check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
     bool motor_at_lower_limit = motors.limit.throttle_lower;
@@ -32,36 +41,40 @@ void Copter::update_land_detector()
     // check that the average throttle output is near minimum (less than 12.5% hover throttle)
     bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min();
 #endif
-    Vector3f accel_ef = ahrs.get_accel_ef_blended();
-    accel_ef.z += GRAVITY_MSS;
-
-    // lowpass filter on accel
-    accel_ef = land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
 
     // check that the airframe is not accelerating (not falling or breaking after fast forward flight)
     bool accel_stationary = (accel_ef.length() < 1.0f);
 
-    if ( motor_at_lower_limit && accel_stationary) {
-        if (!ap.land_complete) {
-            // increase counter until we hit the trigger then set land complete flag
-            if( land_detector < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
-                land_detector++;
-            }else{
+        if (motor_at_lower_limit && accel_stationary) {
+            // landed criteria met - increment the counter and check if we've triggered
+            if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
+                land_detector_count++;
+            } else {
                 set_land_complete(true);
-                land_detector = ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE;
             }
-        }
     } else {
         // we've sensed movement up or down so reset land_detector
-        land_detector = 0;
-        // if throttle output is high then clear landing flag
-        if (motors.get_throttle() > get_non_takeoff_throttle()) {
-            set_land_complete(false);
+            land_detector_count = 0;
         }
     }
 
-    // set land maybe flag
-    set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE);
+    set_land_complete_maybe(ap.land_complete || (land_detector_count > LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE));
+}
+
+void Copter::set_land_complete(bool b)
+{
+    // if no change, exit immediately
+    if( ap.land_complete == b )
+        return;
+
+    land_detector_count = 0;
+
+    if(b){
+        Log_Write_Event(DATA_LAND_COMPLETE);
+    } else {
+        Log_Write_Event(DATA_NOT_LANDED);
+    }
+    ap.land_complete = b;
 }
 
 // update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state