From 4f5fa50efb82e0b3f9c73c7a79affc03c4141c50 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 21 Oct 2016 17:23:52 +0200 Subject: [PATCH] navigator: fix takeoff jump edge case In the normal sitl `commander takeoff` case, the takeoff jump was never actually carried out because the default altitude radius is set to 3m and the takeoff altitude to ~2m which means that the takeoff waypoint is "reached" immediately. This works around this edge case by using the altitude between the home altitude and takeoff altitude divided by 2 as a acceptance radius. --- src/modules/navigator/mission_block.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index d7282400c5..a1a24beea0 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -171,9 +171,25 @@ MissionBlock::is_mission_item_reached() if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->is_rotary_wing) { + + /* We want to avoid the edge case where the acceptance radius is bigger or equal than + * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow + * take-off procedures like leaving the landing gear down. */ + + float takeoff_alt = _mission_item.altitude_is_relative ? + _mission_item.altitude : + (_mission_item.altitude - _navigator->get_home_position()->alt); + + float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); + + /* It should be safe to juse use half of the takoeff_alt as an acceptance radius. */ + if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { + altitude_acceptance_radius = takeoff_alt / 2.0f; + } + /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > - altitude_amsl - _navigator->get_altitude_acceptance_radius()) { + altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {