Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: use LowPassFilterVector3f for land detector accel filter

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-16 12:47:55 -07:00 committed by Randy Mackay
parent eafbd94d51
commit 9a025024b5
3 changed files with 6 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/ArduCopter.pde
View File

@ -538,8 +538,7 @@ static uint8_t sonar_alt_health; // true if we can trust the altitude from th
static float target_sonar_alt; // desired altitude in cm above the ground static float target_sonar_alt; // desired altitude in cm above the ground
static int32_t baro_alt; // barometer altitude in cm above home static int32_t baro_alt; // barometer altitude in cm above home
static float baro_climbrate; // barometer climbrate in cm/s static float baro_climbrate; // barometer climbrate in cm/s
Vector3f land_filtered_accel_ef; // accelerations for land detector test static LowPassFilterVector3f land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF); // accelerations for land detector test
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// 3D Location vectors // 3D Location vectors

3
ArduCopter/config.h
View File

@ -379,6 +379,9 @@
#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC #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) # 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_ACCEL_LPF_CUTOFF
# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter
#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
#endif #endif

10
ArduCopter/land_detector.pde
View File

@ -27,16 +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
// TODO write and use LowPassFilterVector3f accel_ef = land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
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);
// check that the airframe is not accelerating (not falling or breaking after fast forward flight) // check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (land_filtered_accel_ef.length() < 1.0f); bool accel_stationary = (accel_ef.length() < 1.0f);
if ( motor_at_lower_limit && accel_stationary) { if ( motor_at_lower_limit && accel_stationary) {
if (!ap.land_complete) { if (!ap.land_complete) {