Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: run land detector at 400hz

This commit is contained in:
Jonathan Challinger 2015-04-16 11:35:17 -07:00 committed by Randy Mackay
parent f93ff2d3ec
commit eafbd94d51
3 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/ArduCopter.pde
View File

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

8
ArduCopter/config.h
View File

@ -373,11 +373,11 @@
#ifndef LAND_START_ALT #ifndef LAND_START_ALT
# define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent # define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent
#endif #endif
#ifndef LAND_DETECTOR_TRIGGER #ifndef LAND_DETECTOR_TRIGGER_SEC
# define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete. # define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing
#endif #endif
#ifndef LAND_DETECTOR_MAYBE_TRIGGER #ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC
# 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) # 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 #endif
#ifndef LAND_DETECTOR_CLIMBRATE_MAX #ifndef LAND_DETECTOR_CLIMBRATE_MAX
# define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s # define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s

17
ArduCopter/land_detector.pde
View File

@ -1,7 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings // 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) // land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static bool land_complete_maybe() 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 // 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() static void update_land_detector()
{ {
// land detector can not use the following sensors because they are unreliable during landing // 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; accel_ef.z += GRAVITY_MSS;
// lowpass filter on accel // lowpass filter on accel
float freq_cut = 1.0f; // TODO write and use LowPassFilterVector3f
float dt = 0.02f; //This should be set from somewhere const static float freq_cut = 1.0f;
float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,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.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.y += alpha * (accel_ef.y - land_filtered_accel_ef.y);
land_filtered_accel_ef.z += alpha * (accel_ef.z - land_filtered_accel_ef.z); 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 ( motor_at_lower_limit && accel_stationary) {
if (!ap.land_complete) { if (!ap.land_complete) {
// increase counter until we hit the trigger then set land complete flag // 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++; land_detector++;
}else{ }else{
set_land_complete(true); set_land_complete(true);
land_detector = LAND_DETECTOR_TRIGGER; land_detector = ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE;
} }
} }
} else { } else {
@ -57,7 +58,7 @@ static void update_land_detector()
} }
// set land maybe flag // 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 // update_throttle_low_comp - sets motors throttle_low_comp value depending upon vehicle state