From eafbd94d5102dc0f244157b4b7bf71026042cc0d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 16 Apr 2015 11:35:17 -0700 Subject: [PATCH] Copter: run land detector at 400hz --- ArduCopter/ArduCopter.pde | 6 +++--- ArduCopter/config.h | 8 ++++---- ArduCopter/land_detector.pde | 17 +++++++++-------- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e46d5e9d34..3eeda6e3d0 100644 --- a/ArduCopter/ArduCopter.pde +++ b/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(); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 07bf0db422..5e913a3fca 100644 --- a/ArduCopter/config.h +++ b/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 diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde index ac16082ba2..f46bd610e8 100644 --- a/ArduCopter/land_detector.pde +++ b/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