Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: only descend when close to target while Prec Land is active

This commit is contained in:
Rishabh 2021-08-06 00:35:00 +05:30 committed by Randy Mackay
parent d9534d9526
commit cb0b12f25c
1 changed files with 24 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/mode.cpp
View File

@ -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
}