Plane: unify loiter mission items to require heading to next wp

- except loiter_unlimited because it never exits

ArduPlane/Plane.h

@@ -620,8 +620,6 @@ private:
     // For example in a change altitude command, it is the altitude to change to.
     int32_t condition_value;
 
-    // Sometimes there is a second condition required:
-    int32_t condition_value2;
     // A starting value used to check the status of a conditional command.
     // For example in a delay command the condition_start records that start time for the delay
     uint32_t condition_start;
@@ -824,6 +822,7 @@ private:
     bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
     void do_loiter_at_location();
     void do_take_picture();
+    bool verify_loiter_heading(bool init);
     void log_picture();
     void exit_mission_callback();
     void update_commands(void);

ArduPlane/commands_logic.cpp

@@ -417,14 +417,16 @@ void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
     set_next_WP(cmd.content.location);
     loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
     loiter_set_direction_wp(cmd);
+    condition_value = 1; // used to signify primary turns goal not yet met
 }
 
 void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
 {
     set_next_WP(cmd.content.location);
     // we set start_time_ms when we reach the waypoint
-    loiter.time_max_ms = cmd.p1 * (uint32_t)1000;     // units are seconds
+    loiter.time_max_ms = cmd.p1 * (uint32_t)1000;     // convert sec to ms
     loiter_set_direction_wp(cmd);
+    condition_value = 1; // used to signify primary time goal not yet met
 }
 
 void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
@@ -471,9 +473,12 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
         next_WP_loc.alt += home.alt;
     }
 
-    //I know I'm storing this twice -- I'm doing that on purpose -- 
-    //see verify_loiter_alt() function
+    // used to signify primary turns goal not yet met when non-zero
     condition_value = next_WP_loc.alt;
+    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;
+    }
     
     //are lat and lon 0?  if so, don't change the current wp lat/lon
     if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
@@ -481,9 +486,6 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
     }
     //set loiter direction
     loiter_set_direction_wp(cmd);
-
-    //must the plane be heading towards the next waypoint before breaking?
-    condition_value2 = LOWBYTE(cmd.p1);
 }
 
 /********************************************************************************/
@@ -614,32 +616,55 @@ bool Plane::verify_loiter_unlim()
 
 bool Plane::verify_loiter_time()
 {
+    bool result = false;
     // mission radius is always g.loiter_radius
     update_loiter(0);
+
     if (loiter.start_time_ms == 0) {
         if (nav_controller->reached_loiter_target()) {
             // we've reached the target, start the timer
             loiter.start_time_ms = millis();
         }
-    } else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
+    } else if (condition_value != 0) {
+        // primary goal, loiter time
+        if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
+            // primary goal completed, initialize secondary heading goal
+            condition_value = 0;
+            result = verify_loiter_heading(true);
+        }
+    } else {
+        // secondary goal, loiter to heading
+        result = verify_loiter_heading(false);
+    }
+
+    if (result) {
         gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER time complete");
-        return true;
     }
-    return false;
+    return result;
 }
 
 bool Plane::verify_loiter_turns()
 {
+    bool result = false;
     uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
     update_loiter(radius);
 
-    if (loiter.sum_cd > loiter.total_cd) {
-        loiter.total_cd = 0;
+    if (condition_value != 0) {
+        // primary goal, loiter time
+        if (loiter.sum_cd > loiter.total_cd) {
+            // primary goal completed, initialize secondary heading goal
+            condition_value = 0;
+            result = verify_loiter_heading(true);
+        }
+    } else {
+        // secondary goal, loiter to heading
+        result = verify_loiter_heading(false);
+    }
+
+    if (result) {
         gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
-        // clear the command queue;
-        return true;
     }
-    return false;
+    return result;
 }
 
 /*
@@ -649,70 +674,26 @@ bool Plane::verify_loiter_turns()
  */
 bool Plane::verify_loiter_to_alt() 
 {
-    uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
-    update_loiter(radius);
+    bool result = false;
+    update_loiter(mission.get_current_nav_cmd().p1);
 
     //has target altitude been reached?
     if (condition_value != 0) {
+        // primary goal, loiter alt
         if (labs(condition_value - current_loc.alt) < 500) {
-            //Only have to reach the altitude once -- that's why I need
-            //this global condition variable.
-            //
-            //This is in case of altitude oscillation when still trying
-            //to reach the target heading.
+            // primary goal completed, initialize secondary heading goal
             condition_value = 0;
-        } else {
-            return false;
+            result = verify_loiter_heading(true);
         }
+    } else {
+        // secondary goal, loiter to heading
+        result = verify_loiter_heading(false);
     }
-    
-    //has target heading been reached?
-    if (condition_value2 != 0) {
-        //Get the lat/lon of next Nav waypoint after this one:
-        AP_Mission::Mission_Command next_nav_cmd;
-        if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
-                                       next_nav_cmd)) {
-            //no next waypoint to shoot for -- go ahead and break out of loiter
-            return true;        
-        }
 
-        if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(g.loiter_radius)) {
-            /* Whenever next waypoint is within the loiter radius, 
-               maintaining loiter would prevent us from ever pointing toward the next waypoint.
-               Hence break out of loiter immediately
-             */
-            return true;
-        }
-
-        // Bearing in radians
-        int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);
-
-        // get current heading. We should probably be using the ground
-        // course instead to improve the accuracy in wind
-        int32_t heading_cd = ahrs.yaw_sensor;
-
-        int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
- 
-        /*
-          Check to see if the the plane is heading toward the land
-          waypoint
-          We use 10 degrees of slop so that we can handle 100
-          degrees/second of yaw
-        */
-        if (labs(heading_err_cd) <= 1000) {
-            //Want to head in a straight line from _here_ to the next waypoint.
-            //DON'T want to head in a line from the center of the loiter to 
-            //the next waypoint.
-            //Therefore: mark the "last" (next_wp_loc is about to be updated)
-            //wp lat/lon as the current location.
-            next_WP_loc = current_loc;
-            return true;
-        } else {
-            return false;
-        }
-    } 
-
-    return true;
+    if (result) {
+        gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER alt complete");
+    }
+    return result;
 }
 
 bool Plane::verify_RTL()
@@ -1032,3 +1013,51 @@ void Plane::exit_mission_callback()
         start_command(auto_rtl_command);
     }
 }
+
+bool Plane::verify_loiter_heading(bool init)
+{
+    //Get the lat/lon of next Nav waypoint after this one:
+    AP_Mission::Mission_Command next_nav_cmd;
+    if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
+                                   next_nav_cmd)) {
+        //no next waypoint to shoot for -- go ahead and break out of loiter
+        return true;
+    }
+
+    if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(g.loiter_radius)) {
+        /* Whenever next waypoint is within the loiter radius,
+           maintaining loiter would prevent us from ever pointing toward the next waypoint.
+           Hence break out of loiter immediately
+         */
+        return true;
+    }
+
+    // Bearing in degrees
+    int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);
+
+    // get current heading.
+    int32_t heading_cd = gps.ground_course_cd();
+
+    int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
+
+    if (init) {
+        loiter.total_cd =  wrap_360_cd(bearing_cd - heading_cd);
+        loiter.sum_cd = 0;
+    }
+
+    /*
+      Check to see if the the plane is heading toward the land
+      waypoint. We use 20 degrees (+/-10 deg) of margin so that
+      we can handle 200 degrees/second of yaw. Allow turn count
+      to stop it too to ensure we don't loop around forever in
+      case high winds are forcing us beyond 200 deg/sec at this
+      particular moment.
+    */
+    if (labs(heading_err_cd) <= 1000  ||
+            loiter.sum_cd > loiter.total_cd) {
+        // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
+        next_WP_loc = current_loc;
+        return true;
+    }
+    return false;
+}