Plane: remove persistent guided_WP_loc state

So instead of updating plane.guided_WP_loc and then calling
set_guided_WP(void) to copy that state into plane.next_WP_loc we pass
the new location in the call to set_guided_WP(const Location &loc).

avoidance was the only place which was not entirely over-writing
plane.guided_WP_loc.  However, plane.next_WP_loc was updated to be the
current location when we entered guided mode.  If we update the
horizontal/vertical avoidance now it is relative to the current
location, not the guided wp location, which could be quite important.

ArduPlane/ArduPlane.cpp

@@ -713,19 +713,20 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
 
 #if AP_SCRIPTING_ENABLED
 // set target location (for use by scripting)
-bool Plane::set_target_location(const Location& target_loc)
+bool Plane::set_target_location(const Location &target_loc)
 {
+    Location loc{target_loc};
+
     if (plane.control_mode != &plane.mode_guided) {
         // only accept position updates when in GUIDED mode
         return false;
     }
-    plane.guided_WP_loc = target_loc;
     // add home alt if needed
-    if (plane.guided_WP_loc.relative_alt) {
-        plane.guided_WP_loc.alt += plane.home.alt;
-        plane.guided_WP_loc.relative_alt = 0;
+    if (loc.relative_alt) {
+        loc.alt += plane.home.alt;
+        loc.relative_alt = 0;
     }
-    plane.set_guided_WP();
+    plane.set_guided_WP(loc);
     return true;
 }
 

ArduPlane/GCS_Mavlink.cpp

@@ -714,15 +714,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
     if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
         (plane.control_mode == &plane.mode_guided)) {
         plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
-        plane.guided_WP_loc = requested_position;
 
         // add home alt if needed
-        if (plane.guided_WP_loc.relative_alt) {
-            plane.guided_WP_loc.alt += plane.home.alt;
-            plane.guided_WP_loc.relative_alt = 0;
+        if (requested_position.relative_alt) {
+            requested_position.alt += plane.home.alt;
+            requested_position.relative_alt = 0;
         }
 
-        plane.set_guided_WP();
+        plane.set_guided_WP(requested_position);
 
         return MAV_RESULT_ACCEPTED;
     }

ArduPlane/Plane.h

@@ -721,9 +721,6 @@ private:
     // The location of the current/active waypoint.  Used for altitude ramp, track following and loiter calculations.
     Location next_WP_loc {};
 
-    // The location of the active waypoint in Guided mode.
-    struct Location guided_WP_loc {};
-
     // Altitude control
     struct {
         // target altitude above sea level in cm. Used for barometric
@@ -947,7 +944,7 @@ private:
 #endif
 
     // commands.cpp
-    void set_guided_WP(void);
+    void set_guided_WP(const Location &loc);
     void update_home();
     // set home location and store it persistently:
     bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;

ArduPlane/avoidance_adsb.cpp

@@ -61,31 +61,34 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
             }
             break;
 
-        case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND:
+        case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: {
             // climb or descend to avoid obstacle
-            if (handle_avoidance_vertical(obstacle, failsafe_state_change)) {
-                plane.set_guided_WP();
+            Location loc = plane.next_WP_loc;
+            if (handle_avoidance_vertical(obstacle, failsafe_state_change, loc)) {
+                plane.set_guided_WP(loc);
             } else {
                 actual_action = MAV_COLLISION_ACTION_NONE;
             }
             break;
-
-        case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY:
+        }
+        case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: {
             // move horizontally to avoid obstacle
-            if (handle_avoidance_horizontal(obstacle, failsafe_state_change)) {
-                plane.set_guided_WP();
+            Location loc = plane.next_WP_loc;
+            if (handle_avoidance_horizontal(obstacle, failsafe_state_change, loc)) {
+                plane.set_guided_WP(loc);
             } else {
                 actual_action = MAV_COLLISION_ACTION_NONE;
             }
             break;
-
+        }
         case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR:
         {
             // move horizontally and vertically to avoid obstacle
-            const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change);
-            const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change);
+            Location loc = plane.next_WP_loc;
+            const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change, loc);
+            const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change, loc);
             if (success_vert || success_hor) {
-                plane.set_guided_WP();
+                plane.set_guided_WP(loc);
             } else {
                 actual_action = MAV_COLLISION_ACTION_NONE;
             }
@@ -157,7 +160,7 @@ bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change)
     return (plane.control_mode == &plane.mode_avoidADSB);
 }
 
-bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
+bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc)
 {
     // ensure copter is in avoid_adsb mode
      if (!check_flightmode(allow_mode_change)) {
@@ -167,20 +170,20 @@ bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle
      // get best vector away from obstacle
      if (plane.current_loc.alt > obstacle->_location.alt) {
          // should climb
-         plane.guided_WP_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX
+         new_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX
          return true;
 
      } else if (plane.current_loc.alt > plane.g.RTL_altitude_cm) {
          // should descend while above RTL alt
          // TODO: consider using a lower altitude than RTL_altitude_cm since it's default (100m) is quite high
-         plane.guided_WP_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX
+         new_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX
          return true;
      }
 
      return false;
 }
 
-bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
+bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc)
 {
     // ensure plane is in avoid_adsb mode
     if (!check_flightmode(allow_mode_change)) {
@@ -205,7 +208,7 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl
         velocity_neu *= 10000;
 
         // set target
-        plane.guided_WP_loc.offset(velocity_neu.x, velocity_neu.y);
+        new_loc.offset(velocity_neu.x, velocity_neu.y);
         return true;
     }
 

ArduPlane/avoidance_adsb.h

@@ -26,10 +26,10 @@ protected:
     bool check_flightmode(bool allow_mode_change);
 
     // vertical avoidance handler
-    bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
+    bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc);
 
     // horizontal avoidance handler
-    bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
+    bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc);
 
     // control mode before avoidance began
     enum Mode::Number prev_control_mode_number = Mode::Number::RTL;

ArduPlane/commands.cpp

@@ -61,9 +61,9 @@ void Plane::set_next_WP(const struct Location &loc)
     setup_turn_angle();
 }
 
-void Plane::set_guided_WP(void)
+void Plane::set_guided_WP(const Location &loc)
 {
-    if (aparm.loiter_radius < 0 || guided_WP_loc.loiter_ccw) {
+    if (aparm.loiter_radius < 0 || loc.loiter_ccw) {
         loiter.direction = -1;
     } else {
         loiter.direction = 1;
@@ -75,7 +75,7 @@ void Plane::set_guided_WP(void)
 
     // Load the next_WP slot
     // ---------------------
-    next_WP_loc = guided_WP_loc;
+    next_WP_loc = loc;
 
     // used to control FBW and limit the rate of climb
     // -----------------------------------------------

ArduPlane/fence.cpp

@@ -70,39 +70,39 @@ void Plane::fence_check()
                 set_mode(mode_guided, ModeReason::FENCE_BREACHED);
             }
 
+            Location loc;
             if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) {
-                guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm());
+                loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm());
             } else {
                 //return to fence return point, not a rally point
-                guided_WP_loc = {};
                 if (fence.get_return_altitude() > 0) {
                     // fly to the return point using _retalt
-                    guided_WP_loc.alt = home.alt + 100.0f * fence.get_return_altitude();
+                    loc.alt = home.alt + 100.0f * fence.get_return_altitude();
                 } else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) {
                     // invalid min/max, use RTL_altitude
-                    guided_WP_loc.alt = home.alt + g.RTL_altitude_cm;
+                    loc.alt = home.alt + g.RTL_altitude_cm;
                 } else {
                     // fly to the return point, with an altitude half way between
                     // min and max
-                    guided_WP_loc.alt = home.alt + 100.0f * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2;
+                    loc.alt = home.alt + 100.0f * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2;
                 }
 
                 Vector2l return_point;
                 if(fence.polyfence().get_return_point(return_point)) {
-                    guided_WP_loc.lat = return_point[0];
-                    guided_WP_loc.lng = return_point[1];
+                    loc.lat = return_point[0];
+                    loc.lng = return_point[1];
                 } else {
                     // When no fence return point is found (ie. no inclusion fence uploaded, but exclusion is)
                     // we fail to obtain a valid fence return point. In this case, home is considered a safe
                     // return point.
-                    guided_WP_loc.lat = home.lat;
-                    guided_WP_loc.lng = home.lng;
+                    loc.lat = home.lat;
+                    loc.lng = home.lng;
                 }
             }
 
             if (fence.get_action() != AC_FENCE_ACTION_RTL_AND_LAND) {
-                setup_terrain_target_alt(guided_WP_loc);
-                set_guided_WP();
+                setup_terrain_target_alt(loc);
+                set_guided_WP(loc);
             }
 
             if (fence.get_action() == AC_FENCE_ACTION_GUIDED_THROTTLE_PASS) {

ArduPlane/mode_LoiterAltQLand.cpp

@@ -44,20 +44,18 @@ void ModeLoiterAltQLand::switch_qland()
 
 bool ModeLoiterAltQLand::handle_guided_request(Location target_loc)
 {
-    plane.guided_WP_loc = target_loc;
-
     // setup altitude
 #if AP_TERRAIN_AVAILABLE
     if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
-        plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
+        target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
     } else {
-        plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
+        target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
     }
 #else
-    plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
+    target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
 #endif
 
-    plane.set_guided_WP();
+    plane.set_guided_WP(target_loc);
 
     return true;
 }

ArduPlane/mode_guided.cpp

@@ -8,19 +8,19 @@ bool ModeGuided::_enter()
       when entering guided mode we set the target as the current
       location. This matches the behaviour of the copter code
     */
-    plane.guided_WP_loc = plane.current_loc;
+    Location loc{plane.current_loc};
 
 #if HAL_QUADPLANE_ENABLED
     if (plane.quadplane.guided_mode_enabled()) {
         /*
           if using Q_GUIDED_MODE then project forward by the stopping distance
         */
-        plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()),
-                                           plane.quadplane.stopping_distance());
+        loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()),
+                           plane.quadplane.stopping_distance());
     }
 #endif
 
-    plane.set_guided_WP();
+    plane.set_guided_WP(loc);
     return true;
 }
 
@@ -45,15 +45,13 @@ void ModeGuided::navigate()
 
 bool ModeGuided::handle_guided_request(Location target_loc)
 {
-    plane.guided_WP_loc = target_loc;
-
     // add home alt if needed
-    if (plane.guided_WP_loc.relative_alt) {
-        plane.guided_WP_loc.alt += plane.home.alt;
-        plane.guided_WP_loc.relative_alt = 0;
+    if (target_loc.relative_alt) {
+        target_loc.alt += plane.home.alt;
+        target_loc.relative_alt = 0;
     }
 
-    plane.set_guided_WP();
+    plane.set_guided_WP(target_loc);
 
     return true;
 }