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:04:42 +0200 |
| commit | d7f93f4ece19a4244cacf0227d0818f418a0f214 (patch) | |
| tree | 7164dfb0c8eff47dd142ff3db343483a1ba4f637 /src | |
| parent | 9b2a40068a44a2f274de6a31b424599e58c9a4d2 (diff) | |
| download | px4-firmware-d7f93f4ece19a4244cacf0227d0818f418a0f214.tar.gz px4-firmware-d7f93f4ece19a4244cacf0227d0818f418a0f214.tar.bz2 px4-firmware-d7f93f4ece19a4244cacf0227d0818f418a0f214.zip | |
loiter mission items: better reached check for FW
Diffstat (limited to 'src')
| -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) { |