diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-09-05 22:37:34 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-09-05 22:37:34 +0200 |
commit | 2fe50a65244f80591b1fe389029471f804ce6d06 (patch) | |
tree | a1d284842e8213543e6a16e9ad993e35885dec30 | |
parent | 9b2a40068a44a2f274de6a31b424599e58c9a4d2 (diff) | |
parent | 0a6267e010a1401ba111d5477c26a0b8ff17ffea (diff) | |
download | px4-firmware-2fe50a65244f80591b1fe389029471f804ce6d06.tar.gz px4-firmware-2fe50a65244f80591b1fe389029471f804ce6d06.tar.bz2 px4-firmware-2fe50a65244f80591b1fe389029471f804ce6d06.zip |
Merge pull request #10 from swissfang/loiteracceptance
navigator: loiter mission items: better reached check for FW (swissfang)
-rw-r--r-- | src/modules/navigator/mission_block.cpp | 13 |
1 files changed, 13 insertions, 0 deletions
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4adf77dce..723caec7c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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) { |