Plane: Soaring, don't wait for heading if too low, and add timeout otherwise.

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

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