From fffb479edf0c611aacc3e4c5dd6b588f8ffff2ac Mon Sep 17 00:00:00 2001 From: Martina Date: Tue, 7 Aug 2018 11:35:58 +0200 Subject: [PATCH] navigator_main: use the altitude acceptance radius from the pos control if it is greater than the one in the mission item --- src/modules/navigator/navigator_main.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e3808d7a5a..510ff57e98 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -847,7 +847,16 @@ Navigator::get_default_altitude_acceptance_radius() return _param_fw_alt_acceptance_radius.get(); } else { - return _param_mc_alt_acceptance_radius.get(); + float alt_acceptance_radius = _param_mc_alt_acceptance_radius.get(); + + const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + + if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) + && pos_ctrl_status.altitude_acceptance_radius > alt_acceptance_radius) { + alt_acceptance_radius = pos_ctrl_status.altitude_acceptance_radius; + } + + return alt_acceptance_radius; } }