From 298088268ab13eb797f59960998ece0be69a9462 Mon Sep 17 00:00:00 2001
From: Samuel Tabor <samuelctabor@gmail.com>
Date: Mon, 10 Jun 2019 18:07:24 +0100
Subject: [PATCH] Plane: Soaring, don't wait for heading if too low, and add
 timeout otherwise.

---
 ArduPlane/Plane.h     |  3 +++
 ArduPlane/soaring.cpp | 10 +++++++---
 2 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 4efe051ab7..47a8332220 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -757,6 +757,9 @@ private:
     // rudder mixing gain for differential thrust (0 - 1)
     float rudder_dt;
 
+    // soaring mode-change timer
+    uint32_t soaring_mode_timer;
+
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);
     void send_fence_status(mavlink_channel_t chan);
diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp
index 23a39e6ddc..06fb595166 100644
--- a/ArduPlane/soaring.cpp
+++ b/ArduPlane/soaring.cpp
@@ -70,9 +70,13 @@ void Plane::update_soaring() {
             // Reset loiter angle, so that the loiter exit heading criteria
             // only starts expanding when we're ready to exit.
             plane.loiter.sum_cd = 0;
+            plane.soaring_mode_timer = AP_HAL::millis();
             break;
         }
 
+        // Some other loiter status, we need to think about exiting loiter.
+        uint32_t timer = AP_HAL::millis() - plane.soaring_mode_timer;
+
         const char* strTooHigh = "Soaring: Too high, restoring ";
         const char* strTooLow =  "Soaring: Too low, restoring ";
         const char* strTooWeak = "Soaring: Thermal ended, restoring ";
@@ -83,7 +87,7 @@ void Plane::update_soaring() {
         case Mode::Number::FLY_BY_WIRE_B: {
             const bool homeIsSet = AP::ahrs().home_is_set();
             const bool headingLinedupToHome = homeIsSet && plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home());
-            if (homeIsSet && !headingLinedupToHome) {
+            if (homeIsSet && !headingLinedupToHome && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
                 break;
             }
             switch (loiterStatus) {
@@ -110,7 +114,7 @@ void Plane::update_soaring() {
 
         case Mode::Number::CRUISE: {
             const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp(cruise_state.locked_heading_cd);
-            if (cruise_state.locked_heading && !headingLinedupToCruise) {
+            if (cruise_state.locked_heading && !headingLinedupToCruise && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
                 break;
             }
             // return to cruise with old ground course
@@ -143,7 +147,7 @@ void Plane::update_soaring() {
             //Get the lat/lon of next Nav waypoint after this one:
             AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();;
             const bool headingLinedupToWP = plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
-            if (!headingLinedupToWP) {
+            if (!headingLinedupToWP && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
                 break;
             }
             switch (loiterStatus) {