aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: tidy land_run_vertical_control for precision-landing case

Browse Source
This commit is contained in:
Peter Barker 2019-12-10 14:12:07 +11:00 committed by Randy Mackay
parent 9ed37e4486
commit 10bbcf98be
1 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/mode.cpp
View File

@ -519,17 +519,6 @@ int32_t Mode::get_alt_above_ground_cm(void)
void Mode::land_run_vertical_control(bool pause_descent)
{
#if PRECISION_LANDING == ENABLED
const bool navigating = pos_control->is_active_xy();
bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating;
#else
bool doing_precision_landing = false;
#endif
// compute desired velocity
const float precland_acceptable_error = 15.0f;
const float precland_min_descent_speed = 10.0f;
float cmb_rate = 0;
if (!pause_descent) {
float max_land_descent_velocity;
@ -548,11 +537,20 @@ void Mode::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 PRECISION_LANDING == ENABLED
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_horizontal_error()*(max_descent_speed/precland_acceptable_error));
cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown);
}
#endif
}
// update altitude target and call position controller