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