diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-05 10:04:42 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-05 10:15:31 +0200 |
commit | 4c7d6707936718760639ee60b3c27698f0ca119c (patch) | |
tree | 4ab9ea1370c5ccff68bd1dc6ba58a0820782283d | |
parent | ee2eb98167c282aea88714e1a9beea9e523f2e55 (diff) | |
download | px4-firmware-4c7d6707936718760639ee60b3c27698f0ca119c.tar.gz px4-firmware-4c7d6707936718760639ee60b3c27698f0ca119c.tar.bz2 px4-firmware-4c7d6707936718760639ee60b3c27698f0ca119c.zip |
loiter mission items: better reached check for FW
-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..157a1cf71 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 + * 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) { |