Plane: allow exiting loiter_to_alt in cases where you get stuck

- check for scenarios where updrafts can keep you from loitering down indefinitely.

ArduPlane/Plane.h

@@ -641,9 +641,20 @@ private:
         // Direction for loiter. 1 for clockwise, -1 for counter-clockwise
         int8_t direction;
 
+        // when loitering and an altitude is involved, this flag is true when it has been reached at least once
+        bool reached_target_alt;
+
+        // check for scenarios where updrafts can keep you from loitering down indefinitely.
+        bool unable_to_acheive_target_alt;
+
         // start time of the loiter.  Milliseconds.
         uint32_t start_time_ms;
 
+        // altitude at start of loiter loop lap. Used to detect delta alt of each lap.
+        // only valid when sum_cd > 36000
+        int32_t start_lap_alt_cm;
+        int32_t next_sum_lap_cd;
+
         // The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
         uint32_t time_max_ms;
     } loiter;

ArduPlane/commands_logic.cpp

@@ -499,12 +499,8 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
     set_next_WP(loc);
     loiter_set_direction_wp(cmd);
 
-    // used to signify primary turns goal not yet met when non-zero
+    // init to 0, set to 1 when altitude is reached
-    condition_value = next_WP_loc.alt;
+    condition_value = 0;
-    if (condition_value == 0) {
-        // the value of 0 is used to signify it has been reached. Lets bump alt to 1 which is 10cm. Close enough!
-        condition_value = 1;
-    }
 }
 
 /********************************************************************************/
@@ -674,7 +670,7 @@ bool Plane::verify_loiter_time()
     }
 
     if (result) {
-        gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER time complete");
+        gcs_send_text(MAV_SEVERITY_INFO,"Loiter time complete");
         auto_state.vtol_loiter = false;
     }
     return result;
@@ -702,7 +698,7 @@ bool Plane::verify_loiter_turns()
     }
 
     if (result) {
-        gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
+        gcs_send_text(MAV_SEVERITY_INFO,"Loiter orbits complete");
     }
     return result;
 }
@@ -717,12 +713,16 @@ bool Plane::verify_loiter_to_alt()
     bool result = false;
     update_loiter(mission.get_current_nav_cmd().p1);
 
-    //has target altitude been reached?
+    // condition_value == 0 means alt has never been reached
-    if (condition_value != 0) {
+    if (condition_value == 0) {
-        // primary goal, loiter alt
+        // primary goal, loiter to alt
-        if (labs(condition_value - current_loc.alt) < 500 && loiter.sum_cd > 1) {
+        if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_acheive_target_alt)) {
             // primary goal completed, initialize secondary heading goal
-            condition_value = 0;
+            if (loiter.unable_to_acheive_target_alt) {
+                gcs_send_text_fmt(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", current_loc.alt/100);
+            }
+
+            condition_value = 1;
             result = verify_loiter_heading(true);
         }
     } else {
@@ -731,7 +731,7 @@ bool Plane::verify_loiter_to_alt()
     }
 
     if (result) {
-        gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER alt complete");
+        gcs_send_text(MAV_SEVERITY_INFO,"Loiter to alt complete");
     }
     return result;
 }

ArduPlane/navigation.cpp

@@ -22,6 +22,8 @@ void Plane::loiter_angle_reset(void)
 {
     loiter.sum_cd = 0;
     loiter.total_cd = 0;
+    loiter.reached_target_alt = false;
+    loiter.unable_to_acheive_target_alt = false;
 }
 
 /*
@@ -30,21 +32,38 @@ void Plane::loiter_angle_reset(void)
  */
 void Plane::loiter_angle_update(void)
 {
-    int32_t target_bearing_cd = nav_controller->target_bearing_cd();
+    static const int32_t lap_check_interval_cd = 3*36000;
+
+    const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
     int32_t loiter_delta_cd;
+
     if (loiter.sum_cd == 0 && !reached_loiter_target()) {
         // we don't start summing until we are doing the real loiter
         loiter_delta_cd = 0;
     } else if (loiter.sum_cd == 0) {
         // use 1 cd for initial delta
         loiter_delta_cd = 1;
+        loiter.start_lap_alt_cm = current_loc.alt;
+        loiter.next_sum_lap_cd = lap_check_interval_cd;
     } else {
         loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
     }
+
     loiter.old_target_bearing_cd = target_bearing_cd;
     loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
-
     loiter.sum_cd += loiter_delta_cd * loiter.direction;
+
+    if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
+        loiter.reached_target_alt = true;
+        loiter.unable_to_acheive_target_alt = false;
+        loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;
+
+    } else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) {
+        // check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long
+        loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500;
+        loiter.start_lap_alt_cm = current_loc.alt;
+        loiter.next_sum_lap_cd += lap_check_interval_cd;
+    }
 }
 
 //****************************************************************