mirror of https://github.com/ArduPilot/ardupilot
Plane: fixed a potential numerical error close to waypoints
This commit is contained in:
parent
e07b70de4e
commit
12012c9530
|
@ -134,7 +134,7 @@ static void calc_altitude_error()
|
||||||
control_mode == CRUISE) {
|
control_mode == CRUISE) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (nav_controller->reached_loiter_target()) {
|
if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
|
||||||
// once we reach a loiter target then lock to the final
|
// once we reach a loiter target then lock to the final
|
||||||
// altitude target
|
// altitude target
|
||||||
target_altitude_cm = next_WP_loc.alt;
|
target_altitude_cm = next_WP_loc.alt;
|
||||||
|
|
Loading…
Reference in New Issue