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

use NAV_ACC_RAD for vertical waypoint acceptance (#4978)

Browse Source
This commit is contained in:
Daniel Agar
2016-07-04 21:41:16 +02:00
committed by Lorenz Meier
parent a9a3050682
commit 7f89994785
3 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+6 -3
View File
@@ -170,7 +170,8 @@ MissionBlock::is_mission_item_reached()
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
&& dist_z <= _navigator->get_default_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
@@ -183,7 +184,8 @@ MissionBlock::is_mission_item_reached()
* Therefore the item is marked as reached once the system reaches the loiter
* radius (+ some margin). Time inside and turn count is handled elsewhere.
*/
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {
@@ -195,7 +197,8 @@ MissionBlock::is_mission_item_reached()
mission_acceptance_radius = _navigator->get_acceptance_radius();
}
if (dist >= 0.0f && dist <= mission_acceptance_radius) {
if (dist >= 0.0f && dist <= mission_acceptance_radius
&& dist_z <= _navigator->get_default_acceptance_radius()) {
_waypoint_position_reached = true;
}
}