dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1352 from PX4/loiteracceptance_master 

navigator: loiter mission items: better reached check for FW
This commit is contained in:
Lorenz Meier 2014-09-05 22:37:12 +02:00
parent ee2eb98167 81837dcdde
commit e9bfa9fd1c
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached()
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
* 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 <= _mission_item.loiter_radius * 1.2f) {
_waypoint_position_reached = true;
}
} else {
/* for normal mission items used their acceptance radius */
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {