loiter mission items: better reached check for FW

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) {