From ab23a44b99cc4f9ba234a9a463d534dca445a482 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 May 2021 10:53:03 +0200 Subject: [PATCH] navigator: fix acceptance radius for multicopter This fixes a regression introduced in https://github.com/PX4/PX4-Autopilot/pull/16646 which meant that the acceptance radius was no longer used at all for multicopter, and instead only the NAV_ACC_RAD param was used. With this change we use the acceptance radius of the mission item again if it is actually set (and not NAN) which we did not do before, and we only do that for multicopter. --- src/modules/navigator/mission_block.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 01910affba..8f651d6a54 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -277,9 +277,15 @@ MissionBlock::is_mission_item_reached() _time_wp_reached = now; } else { - /*normal mission items */ - float mission_acceptance_radius = _navigator->get_acceptance_radius(); + float acceptance_radius = _navigator->get_acceptance_radius(); + + // We use the acceptance radius of the mission item if it has been set (not NAN) + // but only for multicopter. + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && PX4_ISFINITE(_mission_item.acceptance_radius)) { + acceptance_radius = _mission_item.acceptance_radius; + } /* for vtol back transition calculate acceptance radius based on time and ground speed */ if (_mission_item.vtol_back_transition @@ -292,13 +298,13 @@ MissionBlock::is_mission_item_reached() const float reverse_delay = _navigator->get_vtol_reverse_delay(); if (back_trans_dec > FLT_EPSILON && velocity > FLT_EPSILON) { - mission_acceptance_radius = ((velocity / back_trans_dec / 2) * velocity) + reverse_delay * velocity; + acceptance_radius = ((velocity / back_trans_dec / 2) * velocity) + reverse_delay * velocity; } } - if (dist_xy >= 0.0f && dist_xy <= mission_acceptance_radius + if (dist_xy >= 0.0f && dist_xy <= acceptance_radius && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; }