Copter : Modified set_guided_destination() to reject requests for guided waypoints outside the fence.

ArduCopter/Copter.h

@@ -783,8 +783,8 @@ private:
     void guided_vel_control_start();
     void guided_posvel_control_start();
     void guided_angle_control_start();
-    void guided_set_destination(const Vector3f& destination);
     bool guided_set_destination(const Location_Class& dest_loc);
+    bool guided_set_destination(const Vector3f& destination);
     void guided_set_velocity(const Vector3f& velocity);
     void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
     void guided_set_angle(const Quaternion &q, float climb_rate_cms);
@@ -935,6 +935,7 @@ private:
     float pv_alt_above_home(float alt_above_origin_cm);
     float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
     float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
+    float pv_get_home_destination_distance_cm(const Vector3f &destination);
     void default_dead_zones();
     void init_rc_in();
     void init_rc_out();

ArduCopter/GCS_Mavlink.cpp

@@ -1760,7 +1760,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
             copter.guided_set_velocity(vel_vector);
         } else if (!pos_ignore && vel_ignore && acc_ignore) {
-            copter.guided_set_destination(pos_vector);
+            if (!copter.guided_set_destination(pos_vector)) {
+                result = MAV_RESULT_FAILED;
+            }
         } else {
             result = MAV_RESULT_FAILED;
         }
@@ -1832,7 +1834,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
             copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
         } else if (!pos_ignore && vel_ignore && acc_ignore) {
-            copter.guided_set_destination(pos_ned);
+            if (!copter.guided_set_destination(pos_ned)) {
+                result = MAV_RESULT_FAILED;
+            }
         } else {
             result = MAV_RESULT_FAILED;
         }

ArduCopter/control_guided.cpp

@@ -166,31 +166,54 @@ void Copter::guided_angle_control_start()
 }
 
 // guided_set_destination - sets guided mode's target destination
-void Copter::guided_set_destination(const Vector3f& destination)
+// Returns true if the fence is enabled and guided waypoint is within the fence
+// else return false if the waypoint is outside the fence
+bool Copter::guided_set_destination(const Vector3f& destination)
 {
+    bool fence_status = true;
     // ensure we are in position control mode
     if (guided_mode != Guided_WP) {
         guided_pos_control_start();
     }
 
-    // no need to check return status because terrain data is not used
+#if AC_FENCE == ENABLED
+    // ensure waypoint is within the fence
+    fence_status = fence.check_fence_location(destination.z, pv_get_home_destination_distance_cm(destination));
+#endif
+
+    if (!fence_status) {
+        // failure to set destination can only be because waypoint is outside the fence
+        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        // failure is propagated to GCS with NAK
+        return false;
+    }
+
+    // no need to check return status because terrain data is not used and fence is disabled
     wp_nav.set_wp_destination(destination, false);
 
     // log target
     Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
+    return true;
 }
 
 // sets guided mode's target from a Location object
 // returns false if destination could not be set (probably caused by missing terrain data)
+// or if the fence is enabled and guided waypoint is outside the fence
 bool Copter::guided_set_destination(const Location_Class& dest_loc)
 {
+    bool fence_status = true;
     // ensure we are in position control mode
     if (guided_mode != Guided_WP) {
         guided_pos_control_start();
     }
 
-    if (!wp_nav.set_wp_destination(dest_loc)) {
-        // failure to set destination can only be because of missing terrain data
+#if AC_FENCE == ENABLED
+    // ensure waypoint is within the fence
+    fence_status = fence.check_fence_location((float)dest_loc.alt, (float)get_distance_cm(ahrs.get_home(),dest_loc));
+#endif
+
+    if (!fence_status || !wp_nav.set_wp_destination(dest_loc)) {
+        // failure to set destination can only be because of missing terrain data or waypoint is outside the fence
         Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK
         return false;

ArduCopter/position_vector.cpp

@@ -66,3 +66,10 @@ float Copter::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector
 {
     return norm(destination.x-origin.x,destination.y-origin.y);
 }
+
+// pv_get_home_destination_distance_cm - return distance between destination and home in cm
+float Copter::pv_get_home_destination_distance_cm(const Vector3f &destination)
+{
+    Vector3f home = pv_location_to_vector(ahrs.get_home());
+    return pv_get_horizontal_distance_cm(home, destination);
+}