fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: fix acceptance radius for multicopter

Browse Source 
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.
This commit is contained in:
Julian Oes 2021-05-27 10:53:03 +02:00 committed by Lorenz Meier
parent 5f775b508f
commit ab23a44b99
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/navigator/mission_block.cpp
View File

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