From 2c814e9de2a1f384f1c913365caee4889877c82c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 26 Feb 2019 14:07:55 +1100 Subject: [PATCH] Copter: get_alt_above_ground to get_alt_above_ground_cm --- ArduCopter/landing_gear.cpp | 2 +- ArduCopter/mode.cpp | 8 +++----- ArduCopter/mode.h | 2 +- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index 010fdcbc50..4ceaf81324 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -30,7 +30,7 @@ void Copter::landinggear_update() last_deploy_status = landinggear.deployed(); // support height based triggering using rangefinder or altitude above ground - int32_t height_cm = flightmode->get_alt_above_ground(); + int32_t height_cm = flightmode->get_alt_above_ground_cm(); // use rangefinder if available switch (rangefinder.status_orient(ROTATION_PITCH_270)) { diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 09898008d0..850291f99e 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -393,7 +393,7 @@ void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up) /* get a height above ground estimate for landing */ -int32_t Copter::Mode::get_alt_above_ground(void) +int32_t Copter::Mode::get_alt_above_ground_cm(void) { int32_t alt_above_ground; if (copter.rangefinder_alt_ok()) { @@ -419,7 +419,6 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent) // compute desired velocity const float precland_acceptable_error = 15.0f; const float precland_min_descent_speed = 10.0f; - const int32_t alt_above_ground = get_alt_above_ground(); float cmb_rate = 0; if (!pause_descent) { @@ -434,7 +433,7 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent) max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. - cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); + cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); @@ -526,8 +525,7 @@ void Copter::Mode::land_run_horizontal_control() // there is any position estimate drift after touchdown. We // limit attitude to 7 degrees below this limit and linearly // interpolate for 1m above that - const int alt_above_ground = get_alt_above_ground(); - float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, alt_above_ground, + float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); float total_angle_cd = norm(nav_roll, nav_pitch); if (total_angle_cd > attitude_limit_cd) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 18502ea122..fa88cc4e67 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -116,7 +116,7 @@ protected: // functions to control landing // in modes that support landing - int32_t get_alt_above_ground(void); + int32_t get_alt_above_ground_cm(void); void land_run_horizontal_control(); void land_run_vertical_control(bool pause_descent = false);