From cb0b12f25c9f57077cf535a469dada05a0d755a1 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Fri, 6 Aug 2021 00:35:00 +0530 Subject: [PATCH] Copter: only descend when close to target while Prec Land is active --- ArduCopter/mode.cpp | 32 ++++++++++++++++++++++++-------- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 6263a6dd04..4c8ba3738b 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -552,14 +552,30 @@ void Mode::land_run_vertical_control(bool pause_descent) const bool navigating = pos_control->is_active_xy(); bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating; - if (doing_precision_landing && copter.rangefinder_alt_ok() && copter.rangefinder_state.alt_cm > 35.0f && copter.rangefinder_state.alt_cm < 200.0f) { - // compute desired velocity - const float precland_acceptable_error = 15.0f; - const float precland_min_descent_speed = 10.0f; - - float max_descent_speed = abs(g.land_speed)*0.5f; - float land_slowdown = MAX(0.0f, pos_control->get_pos_error_xy_cm()*(max_descent_speed/precland_acceptable_error)); - cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown); + if (doing_precision_landing) { + // prec landing is active + Vector2f target_pos; + float target_error_cm = 0.0f; + if (copter.precland.get_target_position_cm(target_pos)) { + const Vector2f current_pos = inertial_nav.get_position().xy(); + // target is this many cm away from the vehicle + target_error_cm = (target_pos - current_pos).length(); + } + // check if we should descend or not + const float max_horiz_pos_error_cm = copter.precland.get_max_xy_error_before_descending_cm(); + if (target_error_cm > max_horiz_pos_error_cm && !is_zero(max_horiz_pos_error_cm)) { + // doing precland but too far away from the obstacle + // do not descend + cmb_rate = 0.0f; + } else if (copter.rangefinder_alt_ok() && copter.rangefinder_state.alt_cm > 35.0f && copter.rangefinder_state.alt_cm < 200.0f) { + // very close to the ground and doing prec land, lets slow down to make sure we land on target + // compute desired descent velocity + const float precland_acceptable_error_cm = 15.0f; + const float precland_min_descent_speed_cms = 10.0f; + const float max_descent_speed_cms = abs(g.land_speed)*0.5f; + const float land_slowdown = MAX(0.0f, target_error_cm*(max_descent_speed_cms/precland_acceptable_error_cm)); + cmb_rate = MIN(-precland_min_descent_speed_cms, -max_descent_speed_cms+land_slowdown); + } } #endif }