mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 23:33:57 -04:00
Copter: descend normally from 35cm
This commit is contained in:
parent
f2ef8eec8c
commit
7cb9ad6e2b
@ -196,7 +196,7 @@ void Copter::land_run_vertical_control(bool pause_descent)
|
||||
// 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));
|
||||
|
||||
if (doing_precision_landing && alt_above_ground < 200.0f) {
|
||||
if (doing_precision_landing && rangefinder_alt_ok() && rangefinder_state.alt_cm > 35.0f && rangefinder_state.alt_cm < 200.0f) {
|
||||
float max_descent_speed = abs(g.land_speed)/2.0f;
|
||||
float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(max_descent_speed/precland_acceptable_error));
|
||||
cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown);
|
||||
|
Loading…
Reference in New Issue
Block a user