From 5464f0dcd26ea41fd352b7953d1b6a1ff12ad7b2 Mon Sep 17 00:00:00 2001
From: Michael Day <michael.day@gtri.gatech.edu>
Date: Tue, 20 Dec 2016 20:27:28 -0500
Subject: [PATCH] Plane: Fixed bug where NAV_LOITER_TO_ALT was not verifying
heading.
NAV_LOITER_TO_ALT now verifying that the plane was heading
towards the next waypoint before breaking out of loiter IF the
first parameter of the mavlink message is not 0.
---
ArduPlane/commands_logic.cpp | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 4184137cc3..dfcdae4ef2 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -690,6 +690,11 @@ bool Plane::verify_loiter_to_alt()
if (labs(condition_value - current_loc.alt) < 500 && loiter.sum_cd > 1) {
// primary goal completed, initialize secondary heading goal
condition_value = 0;
+ if (mission.get_current_nav_cmd().content.location.flags.loiter_hdg_chk != 0) {
+ loiter.sum_cd = 3; //don't immediately stop loitering;
+ //wait for heading toward next wp or 3 complete loiters,
+ // whichever comes first.
+ }
result = verify_loiter_heading(true);
}
} else {
--
GitLab