From f6ff1742d5f3313ee84c8d174004e183a288c059 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 Oct 2014 16:07:10 +0900 Subject: [PATCH] Copter: landing detector checks baro climb rate Barometer climb rate must be -150cm/s ~ +150cm/s This threshold is generous because we already use the inertial navigation climb rate so this is just to catch cases where inertial nav is very incorrect in it's climbrate estimates --- ArduCopter/ArduCopter.pde | 6 +++--- ArduCopter/config.h | 3 +++ ArduCopter/control_land.pde | 4 +++- ArduCopter/sensors.pde | 8 +++----- ArduCopter/test.pde | 4 ++-- 5 files changed, 14 insertions(+), 11 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index bec7aa94db..2bcc235deb 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -581,8 +581,8 @@ static int16_t climb_rate; static int16_t sonar_alt; static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar static float target_sonar_alt; // desired altitude in cm above the ground -// The altitude as reported by Baro in cm - Values can be quite high -static int32_t baro_alt; +static int32_t baro_alt; // barometer altitude in cm above home +static float baro_climbrate; // barometer climbrate in cm/s //////////////////////////////////////////////////////////////////////////////// @@ -1373,7 +1373,7 @@ static void read_AHRS(void) static void update_altitude() { // read in baro altitude - baro_alt = read_barometer(); + read_barometer(); // read in sonar altitude sonar_alt = read_sonar(); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2b9d7f3450..624112e45d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -498,6 +498,9 @@ #ifndef LAND_DETECTOR_CLIMBRATE_MAX # define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s #endif +#ifndef LAND_DETECTOR_BARO_CLIMBRATE_MAX +# define LAND_DETECTOR_BARO_CLIMBRATE_MAX 150 // barometer climb rate must be between -150cm/s ~ +150cm/s +#endif #ifndef LAND_DETECTOR_ROTATION_MAX # define LAND_DETECTOR_ROTATION_MAX 0.50f // vehicle rotation must be below 0.5 rad/sec (=30deg/sec for) vehicle to consider itself landed #endif diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 8131b3ce22..62dc557690 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -203,7 +203,9 @@ static bool land_complete_maybe() static void update_land_detector() { // detect whether we have landed by watching for low climb rate, motors hitting their lower limit, overall low throttle and low rotation rate - if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && motors.limit.throttle_lower && + if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && + (abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX) && + motors.limit.throttle_lower && #if FRAME_CONFIG != HELI_FRAME (motors.get_throttle_out() < get_non_takeoff_throttle()) && #endif diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index a836ff8881..38ab52a235 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -19,13 +19,14 @@ static void init_barometer(bool full_calibration) } // return barometric altitude in centimeters -static int32_t read_barometer(void) +static void read_barometer(void) { barometer.read(); if (should_log(MASK_LOG_IMU)) { Log_Write_Baro(); } - int32_t balt = barometer.get_altitude() * 100.0f; + baro_alt = barometer.get_altitude() * 100.0f; + baro_climbrate = barometer.get_climb_rate() * 100.0f; // run glitch protection and update AP_Notify if home has been initialised baro_glitch.check_alt(); @@ -38,9 +39,6 @@ static int32_t read_barometer(void) } AP_Notify::flags.baro_glitching = report_baro_glitch; } - - // return altitude - return balt; } // return sonar altitude in centimeters diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 15be4b828c..477f632a3b 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -58,13 +58,13 @@ test_baro(uint8_t argc, const Menu::arg *argv) while(1) { delay(100); - alt = read_barometer(); + read_barometer(); if (!barometer.healthy()) { cliSerial->println_P(PSTR("not healthy")); } else { cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"), - alt / 100.0, + baro_alt / 100.0, barometer.get_pressure(), barometer.get_temperature()); }