From ab044e274d4ed966d6600b231bb1d89e73f42c63 Mon Sep 17 00:00:00 2001 From: Philipp Oettershagen Date: Mon, 16 Jul 2018 05:54:08 +0200 Subject: [PATCH] Navigator: Fix fixed-wing first order altitude hold (#9850) i.e. the altitude reference oscillations caused by it in LOITER mode --- src/modules/navigator/mission.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fa7b2ea186..90ff162b39 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1297,8 +1297,15 @@ Mission::altitude_sp_foh_update() return; } + // Calculate acceptance radius, i.e. the radius within which we do not perform a first order hold anymore + float acc_rad = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); + + if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + acc_rad = _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f); + } + /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ - if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < FLT_EPSILON) { + if (_distance_current_previous - acc_rad < FLT_EPSILON) { return; } @@ -1327,7 +1334,7 @@ Mission::altitude_sp_foh_update() /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ - if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { + if (_min_current_sp_distance_xy < acc_rad) { pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item); } else { @@ -1337,8 +1344,7 @@ Mission::altitude_sp_foh_update() * radius around the current waypoint **/ float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt); - float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius( - _mission_item.acceptance_radius)); + float grad = -delta_alt / (_distance_current_previous - acc_rad); float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; }