diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index b0fbd05a1e380..d996912bd7876 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/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 {} diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index bb8759b1f8025..74790b06f3b78 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/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) diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index bd25d3f01c452..b77eabcae746f 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/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 ¢er_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; } diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h index 3dbe16bfa47b8..1a4b3ce6decc8 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.h +++ b/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 diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index 45c381f6d4744..233a24445061c 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/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) diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp index 1a216849aca8f..3b7f26109538b 100644 --- a/libraries/AP_Math/polygon.cpp +++ b/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