Copter: run land detector at 400hz

ArduCopter/ArduCopter.pde

@@ -967,6 +967,9 @@ static void fast_loop()
 
     // update home from EKF if necessary
     update_home_from_EKF();
+
+    // check if we've landed
+    update_land_detector();
 }
 
 // rc_loops - reads user input from transmitter/receiver
@@ -986,9 +989,6 @@ static void throttle_loop()
     // get altitude and climb rate from inertial lib
     read_inertial_altitude();
 
-    // check if we've landed
-    update_land_detector();
-
     // update throttle_low_comp value (controls priority of throttle vs attitude control)
     update_throttle_low_comp();
 

ArduCopter/config.h

@@ -373,11 +373,11 @@
 #ifndef LAND_START_ALT
  # define LAND_START_ALT 1000         // altitude in cm where land controller switches to slow rate of descent
 #endif
-#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.
+#ifndef LAND_DETECTOR_TRIGGER_SEC
+ # define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing
 #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)
+#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC
+ # define LAND_DETECTOR_MAYBE_TRIGGER_SEC   0.2f  // 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

ArduCopter/land_detector.pde

@@ -1,7 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 // counter to verify landings
-static uint16_t land_detector = LAND_DETECTOR_TRIGGER;  // we assume we are landed
+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)
 static bool land_complete_maybe()
@@ -10,7 +10,7 @@ static bool land_complete_maybe()
 }
 
 // update_land_detector - checks if we have landed and updates the ap.land_complete flag
-// called at 50hz
+// called at MAIN_LOOP_RATE
 static void update_land_detector()
 {
     // land detector can not use the following sensors because they are unreliable during landing
@@ -27,9 +27,10 @@ static void update_land_detector()
     accel_ef.z += GRAVITY_MSS;
 
     // lowpass filter on accel
-    float freq_cut = 1.0f;
-    float dt = 0.02f; //This should be set from somewhere
-    float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f);
+    // TODO write and use LowPassFilterVector3f
+    const static float freq_cut = 1.0f;
+    const static float dt = 1.0f/MAIN_LOOP_RATE;
+    const static float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI_F*freq_cut)),0.0f,1.0f);
     land_filtered_accel_ef.x += alpha * (accel_ef.x - land_filtered_accel_ef.x);
     land_filtered_accel_ef.y += alpha * (accel_ef.y - land_filtered_accel_ef.y);
     land_filtered_accel_ef.z += alpha * (accel_ef.z - land_filtered_accel_ef.z);
@@ -40,11 +41,11 @@ static void update_land_detector()
     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 < LAND_DETECTOR_TRIGGER) {
+            if( land_detector < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
                 land_detector++;
             }else{
                 set_land_complete(true);
-                land_detector = LAND_DETECTOR_TRIGGER;
+                land_detector = ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE;
             }
         }
     } else {
@@ -57,7 +58,7 @@ static void update_land_detector()
     }
 
     // set land maybe flag
-    set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
+    set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE);
 }
 
 // update_throttle_low_comp - sets motors throttle_low_comp value depending upon vehicle state