Fence margin direction

libraries/AC_Fence/AC_Fence.cpp

@@ -550,6 +550,21 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c
     return true;
 }
 
+// retrieve the current NED position relative to home
+// always retrieves altitude but returns false if position could not be determnined
+bool AC_Fence::get_current_position_NED(Vector3f& currpos) const
+{
+    AP::ahrs().get_relative_position_D_home(currpos.z);
+
+    Vector2f pos;
+    if (AP::ahrs().get_relative_position_NE_home(pos)) {
+        currpos.x = pos.x;
+        currpos.y = pos.y;
+        return true;
+    }
+    return false;
+}
+
 /// returns true if we have freshly breached the maximum altitude
 /// fence; also may set up a fallback fence which, if breached, will
 /// cause the altitude fence to be freshly breached
@@ -561,9 +576,8 @@ bool AC_Fence::check_fence_alt_max()
         return false;
     }
 
-    float alt;
-    AP::ahrs().get_relative_position_D_home(alt);
-    const float _curr_alt = -alt; // translate Down to Up
+    get_current_position_NED(_last_fence_check_pos);
+    const float _curr_alt = -_last_fence_check_pos.z; // translate Down to Up
 
     // record distance above/below breach
     _alt_max_breach_distance = _curr_alt - _alt_max;
@@ -612,9 +626,9 @@ bool AC_Fence::check_fence_alt_min()
         return false;
     }
 
-    float alt;
-    AP::ahrs().get_relative_position_D_home(alt);
-    const float _curr_alt = -alt; // translate Down to Up
+    get_current_position_NED(_last_fence_check_pos);
+
+    const float _curr_alt = -_last_fence_check_pos.z; // translate Down to Up
 
     // record distance above/below breach
     _alt_min_breach_distance = _alt_min - _curr_alt;
@@ -698,7 +712,9 @@ bool AC_Fence::check_fence_polygon()
     Location loc;
     const bool have_location = AP::ahrs().get_location(loc);
 
-    if (have_location && _poly_loader.breached(loc, _polygon_breach_distance)) {
+    get_current_position_NED(_last_fence_check_pos);
+
+    if (have_location && _poly_loader.breached(loc, _polygon_breach_distance, _polygon_nearest_point)) {
         if (!was_breached) {
             record_breach(AC_FENCE_TYPE_POLYGON);
             return true;
@@ -727,10 +743,17 @@ bool AC_Fence::check_fence_circle()
         return false;
     }
 
-    Vector2f home;
-    if (AP::ahrs().get_relative_position_NE_home(home)) {
+    if (get_current_position_NED(_last_fence_check_pos)) {
         // we (may) remain breached if we can't update home
-        _home_distance = home.length();
+        const Vector2f& pos = _last_fence_check_pos.xy();
+        _home_distance = pos.length();
+
+        if (is_zero(pos.length_squared())) {
+            _circle_breach_direction.x = _circle_radius;
+            _circle_breach_direction.y = 0.0f;
+        } else {
+            _circle_breach_direction = pos.normalized() * _circle_radius.get()  - pos;
+        }
     }
 
     // record distance outside/inside the fence
@@ -1019,6 +1042,35 @@ float AC_Fence::get_breach_distance(uint8_t fence_type) const
     return max;
 }
 
+/// get_breach_direction - returns direction to the closest fence breach in NED frame.
+// fence_check_pos is the home relative position in NED frame when the check was made
+//  fence_type is a bitmask here.
+Vector3f AC_Fence::get_breach_direction_NED(uint8_t fence_type, Vector3f& fence_check_pos) const
+{
+    fence_type = fence_type & present();
+    Vector3f direction_to_closest;
+    fence_check_pos = _last_fence_check_pos;
+    float closest = FLT_MAX;
+
+    if (fence_type & AC_FENCE_TYPE_ALT_MAX && fabsf(_alt_max_breach_distance) < closest) {
+        direction_to_closest = Vector3f{0, 0, _alt_max_breach_distance};
+        closest = fabsf(_alt_max_breach_distance);
+    }
+    if (fence_type & AC_FENCE_TYPE_ALT_MIN && fabsf(_alt_min_breach_distance) < closest) {
+        direction_to_closest = Vector3f{0, 0, -_alt_min_breach_distance};
+        closest = fabsf(_alt_min_breach_distance);
+    }
+    if (fence_type & AC_FENCE_TYPE_CIRCLE && fabsf(_circle_breach_distance) < closest) {
+        direction_to_closest = Vector3f{_circle_breach_direction.x, _circle_breach_direction.y, 0};
+        closest = fabsf(_circle_breach_distance);
+    }
+    if (fence_type & AC_FENCE_TYPE_POLYGON && fabsf(_polygon_breach_distance) < closest) {
+        direction_to_closest = Vector3f{_polygon_nearest_point.x, _polygon_nearest_point.y, 0};
+        closest = fabsf(_polygon_breach_distance);
+    }
+    return direction_to_closest;
+}
+
 /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds
 ///     has no effect if no breaches have occurred
 void AC_Fence::manual_recovery_start()
@@ -1098,6 +1150,7 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c
 uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; }
 bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; }
 float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; }
+Vector3f AC_Fence::get_breach_direction_NED(uint8_t fence_type, Vector3f& fence_check_pos) const { return Vector3f(); }
 void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { }
 void AC_Fence::print_fence_message(const char* msg, uint8_t fences) const {}
 

libraries/AC_Fence/AC_Fence.h

@@ -143,6 +143,11 @@ class AC_Fence
     /// of the given fences.  fence_type is a bitmask here.
     float get_breach_distance(uint8_t fence_type) const;
 
+    /// get_breach_direction_NED - returns direction in meters outside/inside
+    /// of the given fences.  fence_check_pos is the home relative position in NED frame when the check was made.
+    // fence_type is a bitmask.
+    Vector3f get_breach_direction_NED(uint8_t fence_type, Vector3f& fence_check_pos) const;
+
     /// get_action - getter for user requested action on limit breach
     Action get_action() const { return _action; }
 
@@ -233,6 +238,9 @@ class AC_Fence
     /// clear_margin_breach - update margin breach bitmask
     void clear_margin_breach(uint8_t fence_type);
 
+    /// retrieve the current NED position relative to home
+    bool get_current_position_NED(Vector3f& currpos) const;
+
     // additional checks for the different fence types:
     bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const;
     bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const;
@@ -269,6 +277,9 @@ class AC_Fence
     float           _alt_min_breach_distance;   // distance below the altitude min
     float           _circle_breach_distance;    // distance beyond the circular fence
     float           _polygon_breach_distance;   // distance beyond the polygon fence
+    Vector2f        _polygon_nearest_point;  // direction towards the polygon breach
+    Vector2f        _circle_breach_direction;   // direction towards the circle breach
+    Vector3f        _last_fence_check_pos;  // position used in the last fence check
 
     // other internal variables
     float           _home_distance;         // distance from home in meters (provided by main code)

libraries/AC_Fence/AC_PolyFence_loader.cpp

@@ -229,7 +229,7 @@ bool AC_PolyFence_loader::breached() const
 
 // check if a position (expressed as lat/lng) is within the boundary
 //   returns true if location is outside the boundary
-bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence) const
+bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const
 {
     if (!loaded() || total_fence_count() == 0) {
         return false;
@@ -248,9 +248,8 @@ bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_
     // check we are inside each inclusion zone:
     for (uint8_t i=0; i<_num_loaded_inclusion_boundaries; i++) {
         const InclusionBoundary &boundary = _loaded_inclusion_boundary[i];
-        float distance;
-        bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, distance);
-        distance *= 0.01f; // convert back to meters
+        bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, fence_direction);
+        float distance = fence_direction.length() * 0.01f; // convert back to meters
         if (Polygon_outside(pos, boundary.points_lla, boundary.count)) {
             num_inclusion_outside++;
             if (valid_distance) {
@@ -268,9 +267,8 @@ bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_
     // check we are outside each exclusion zone:
     for (uint8_t i=0; i<_num_loaded_exclusion_boundaries; i++) {
         const ExclusionBoundary &boundary = _loaded_exclusion_boundary[i];
-        float distance;
-        bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, distance);
-        distance *= 0.01f; // convert back to meters
+        bool valid_distance = Polygon_closest_distance_point(boundary.points, boundary.count, scaled_pos, fence_direction);
+        float distance = fence_direction.length() * 0.01f; // convert back to meters
         if (!Polygon_outside(pos, boundary.points_lla, boundary.count)) {
             if (valid_distance) {
                 distance_outside_fence = distance;
@@ -1705,7 +1703,7 @@ bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f &center_p
 void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg) {};
 
 bool AC_PolyFence_loader::breached() const { return false; }
-bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence) const { return false; }
+bool AC_PolyFence_loader::breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const { return false; }
 
 uint16_t AC_PolyFence_loader::max_items() const { return 0; }
 

libraries/AC_Fence/AC_PolyFence_loader.h

@@ -146,12 +146,13 @@ class AC_PolyFence_loader
     //  breached() - returns true if the vehicle has breached any fence
     bool breached() const WARN_IF_UNUSED;
     //  returns true if location is outside the boundary also returns the minimum distance to the fence
-    bool breached(const Location& loc, float& distance_outside_fence) const WARN_IF_UNUSED;
+    bool breached(const Location& loc, float& distance_outside_fence, Vector2f& fence_direction) const WARN_IF_UNUSED;
     //  breached(Location&) - returns true if location is outside the boundary
     bool breached(const Location& loc) const WARN_IF_UNUSED
     {
         float distance_outside_fence;
-        return breached(loc, distance_outside_fence);
+        Vector2f breach_direction;
+        return breached(loc, distance_outside_fence, breach_direction);
     }
 
     // returns true if a polygonal include fence could be returned

libraries/AP_Common/tests/test_location.cpp

@@ -400,11 +400,12 @@ TEST(Location, OffsetError)
 #define TEST_POLYGON_DISTANCE_POINTS(POLYGON, TEST_POINTS)                       \
     do {                                                                \
         for (uint32_t i = 0; i < ARRAY_SIZE(TEST_POINTS); i++) {        \
-            float distance; \
+            Vector2f direction; \
             Polygon_closest_distance_point(POLYGON,     \
                                             ARRAY_SIZE(POLYGON),\
-                                            TEST_POINTS[i].point, distance);\
-            EXPECT_TRUE(fabs(TEST_POINTS[i].distance - distance) <= 1.0f); \
+                                            TEST_POINTS[i].point, direction);\
+            EXPECT_TRUE(fabs(TEST_POINTS[i].distance - direction.length()) <= 1.0f); \
+            EXPECT_TRUE(fabs(degrees(TEST_POINTS[i].direction.angle()) - degrees(direction.angle())) <= 5.0f); \
         }                                                               \
     } while(0)
 
@@ -452,12 +453,13 @@ static const Vector2f London_boundary[] {
 static const struct {
     Vector2f point;
     float distance;
+    Vector2f direction;
 } London_test_points[] = {
-    { CARTESIAN(CENTER_LAT, CENTER_LON), 75.0f, },
-    { CARTESIAN(CENTER_NORTH_LAT, CENTER_LON), 37.5f, },
-    { CARTESIAN(CENTER_LAT, CENTER_EAST_LON), 37.5f, },
-    { CARTESIAN(CENTER_SOUTH_LAT, CENTER_LON), 37.5f, },
-    { CARTESIAN(CENTER_LAT, CENTER_WEST_LON), 37.5f, },
+    { CARTESIAN(CENTER_LAT, CENTER_LON), 75.0f, { 0.0f, 75.0f }},
+    { CARTESIAN(CENTER_NORTH_LAT, CENTER_LON), 37.5f, { 37.5f, 0.0f }},
+    { CARTESIAN(CENTER_LAT, CENTER_EAST_LON), 37.5f, { 0.0f, 37.5f }},
+    { CARTESIAN(CENTER_SOUTH_LAT, CENTER_LON), 37.5f, { -37.5f, 0.0f }},
+    { CARTESIAN(CENTER_LAT, CENTER_WEST_LON), 37.5f, { 0.0f, -37.5f }},
 };
 
 TEST(Location, London_distance)

libraries/AP_Math/polygon.cpp

@@ -200,31 +200,38 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
   return the closest distance that point p comes to an edge of closed
   polygon V, defined by N points
  */
-bool Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p, float& closest)
+bool Polygon_closest_distance_point(const Vector2f *V, unsigned N,
+                                    const Vector2f &p, Vector2f &closest_vec)
 {
     float closest_sq = FLT_MAX;
-    const bool complete = Polygon_complete(V, N);
-    if (complete) {
-        // the last point is the same as the first point; treat as if
-        // the last point wasn't passed in
-        N--;
+    if (Polygon_complete(V, N) && N > 0) {
+        N--;   // not a polygon
     }
-    if (N < 3) {    // not a polygon
+    if (N < 3) {
         return false;
     }
-    for (uint8_t i=0; i<N; i++) {
-        const Vector2f &v1 = V[i];
-        const Vector2f &v2 = V[(i+1) % N];
 
-        float dist_sq = Vector2f::closest_distance_between_line_and_point_squared(v1, v2, p);
-        if (dist_sq < closest_sq) {
-            closest_sq = dist_sq;
+    Vector2f best_v(0.0f, 0.0f);
+
+    for (uint32_t i = 0; i < N; i++) {
+        const Vector2f &a = V[i];
+        const Vector2f &b = V[(i + 1) % N];
+
+        // Built-in: closest point on segment ab to p (handles v==w).
+        const Vector2f q = Vector2f::closest_point(p, a, b);
+        const Vector2f v = q - p;                  // vector from p to boundary
+        const float vsq = v.length_squared();       // squared distance
+
+        if (vsq < closest_sq) {
+            closest_sq = vsq;
+            best_v  = v;
         }
     }
+
     if (is_equal(closest_sq, FLT_MAX)) {
-        closest = 0.0f;
         return false;
     }
-    closest = sqrtf(closest_sq);
+
+    closest_vec = best_v;                          // already correct direction & length
     return true;
 }

libraries/AP_Math/polygon.h

@@ -43,5 +43,5 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
   return the closest distance that a point p in cartesian comes to an edge of
   closed polygon V, defined by N points of cartesian. Returns true if successful, false otherwise
  */
- bool Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p, float& closest);
+ bool Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p, Vector2f& closest_segment);
 

libraries/AP_Scripting/docs/docs.lua

@@ -4035,6 +4035,16 @@ function fence:get_margin_breaches() end
 ---@return number -- distance
 function fence:get_breach_distance(fence_type) end
 
+-- Returns the direction and distance in meters to the nearest fence in NED frame given by the type bitmask
+---@param fence_type integer
+---| 1 # Maximim altitude
+---| 2 # Circle
+---| 4 # Polygon
+---| 8 # Minimum altitude
+---@return Vector3f_ud -- direction and distance to breach in NED frame
+---@return Vector3f_ud -- home relative position at the time of the breach
+function fence:get_breach_direction_NED(fence_type) end
+
 -- Rally library
 rally = {}
 -- Returns a specfic rally by index as a Location 

libraries/AP_Scripting/generator/description/bindings.desc

@@ -1051,6 +1051,7 @@ singleton AC_Fence method get_breach_time uint32_t
 singleton AC_Fence method get_margin_breaches uint8_t
 singleton AC_Fence method get_margin_breach_time uint32_t
 singleton AC_Fence method get_breach_distance float uint8_t'skip_check
+singleton AC_Fence method get_breach_direction_NED Vector3f uint8_t'skip_check Vector3f'Ref
 
 include AP_Rally/AP_Rally.h depends HAL_RALLY_ENABLED