From 248bbcb5207a89602e79b182dc35b21eb89f9a2c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 6 Feb 2016 21:12:02 +0100 Subject: [PATCH] use system specific acceptance radius for multirotor takeoff, ignore mission item --- src/modules/navigator/mission_block.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 64512d3d3d..6a4d381027 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -141,17 +141,9 @@ MissionBlock::is_mission_item_reached() &dist_xy, &dist_z); if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { - /* require only altitude for takeoff for multicopter */ - - /* _mission_item.acceptance_radius is not always set */ - float mission_acceptance_radius = _mission_item.acceptance_radius; - /* if set to zero use the default instead */ - if (mission_acceptance_radius < NAV_EPSILON_POSITION) { - mission_acceptance_radius = _navigator->get_acceptance_radius(); - } - + /* require only altitude for takeoff for multicopter, do not use waypoint acceptance radius */ if (_navigator->get_global_position()->alt > - altitude_amsl - mission_acceptance_radius) { + altitude_amsl - _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {