Sub: reject guided mode destinations early

ArduSub/mode_guided.cpp

@@ -165,11 +165,6 @@ void ModeGuided::guided_angle_control_start()
 // else return false if the waypoint is outside the fence
 bool ModeGuided::guided_set_destination(const Vector3f& destination)
 {
-    // ensure we are in position control mode
-    if (sub.guided_mode != Guided_WP) {
-        guided_pos_control_start();
-    }
-
 #if AP_FENCE_ENABLED
     // reject destination if outside the fence
     const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
@@ -180,6 +175,11 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination)
     }
 #endif
 
+    // ensure we are in position control mode
+    if (sub.guided_mode != Guided_WP) {
+        guided_pos_control_start();
+    }
+
     // no need to check return status because terrain data is not used
     sub.wp_nav.set_wp_destination(destination, false);
 
@@ -196,11 +196,6 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination)
 // or if the fence is enabled and guided waypoint is outside the fence
 bool ModeGuided::guided_set_destination(const Location& dest_loc)
 {
-    // ensure we are in position control mode
-    if (sub.guided_mode != Guided_WP) {
-        guided_pos_control_start();
-    }
-
 #if AP_FENCE_ENABLED
     // reject destination outside the fence.
     // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
@@ -211,6 +206,11 @@ bool ModeGuided::guided_set_destination(const Location& dest_loc)
     }
 #endif
 
+    // ensure we are in position control mode
+    if (sub.guided_mode != Guided_WP) {
+        guided_pos_control_start();
+    }
+
     if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
         LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
@@ -231,11 +231,6 @@ bool ModeGuided::guided_set_destination(const Location& dest_loc)
 // else return false if the waypoint is outside the fence
 bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
 {
-    // ensure we are in position control mode
-    if (sub.guided_mode != Guided_WP) {
-        guided_pos_control_start();
-    }
-
 #if AP_FENCE_ENABLED
     // reject destination if outside the fence
     const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
@@ -246,6 +241,11 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_ya
     }
 #endif
 
+    // ensure we are in position control mode
+    if (sub.guided_mode != Guided_WP) {
+        guided_pos_control_start();
+    }
+
     // set yaw state
     guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
@@ -297,11 +297,6 @@ void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, flo
 // set guided mode posvel target
 bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
 {
-    // check we are in velocity control mode
-    if (sub.guided_mode != Guided_PosVel) {
-        guided_posvel_control_start();
-    }
-
 #if AP_FENCE_ENABLED
     // reject destination if outside the fence
     const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
@@ -312,6 +307,11 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
     }
 #endif
 
+    // check we are in posvel control mode
+    if (sub.guided_mode != Guided_PosVel) {
+        guided_posvel_control_start();
+    }
+
     update_time_ms = AP_HAL::millis();
     posvel_pos_target_cm = destination.topostype();
     posvel_vel_target_cms = velocity;
@@ -332,11 +332,6 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
 // set guided mode posvel target
 bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
 {
-    // check we are in velocity control mode
-    if (sub.guided_mode != Guided_PosVel) {
-        guided_posvel_control_start();
-    }
-
     #if AP_FENCE_ENABLED
     // reject destination if outside the fence
     const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
@@ -347,6 +342,11 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
     }
     #endif
 
+    // check we are in posvel control mode
+    if (sub.guided_mode != Guided_PosVel) {
+        guided_posvel_control_start();
+    }
+
     // set yaw state
     guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
@@ -371,7 +371,7 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
 // set guided mode angle target
 void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)
 {
-    // check we are in velocity control mode
+    // check we are in angle control mode
     if (sub.guided_mode != Guided_Angle) {
         guided_angle_control_start();
     }