Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: remove always-true contains_return_point parameter

Browse Source
This commit is contained in:
Peter Barker 2019-02-04 17:18:26 +11:00 committed by Randy Mackay
parent a72477590f
commit 5448cfda80
3 changed files with 17 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_Fence/AC_Fence.cpp
View File

@ -261,7 +261,7 @@ bool AC_Fence::check_fence_polygon()
} }
position = position * 100.0f; // m to cm position = position * 100.0f; // m to cm
if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary, true)) { if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary)) {
// check if this is a new breach // check if this is a new breach
if (_breached_fences & AC_FENCE_TYPE_POLYGON) { if (_breached_fences & AC_FENCE_TYPE_POLYGON) {
// not a new breach // not a new breach
@ -389,7 +389,7 @@ bool AC_Fence::check_destination_within_fence(const Location& loc)
// check ekf has a good location // check ekf has a good location
Vector2f posNE; Vector2f posNE;
if (loc.get_vector_xy_from_origin_NE(posNE)) { if (loc.get_vector_xy_from_origin_NE(posNE)) {
if (_poly_loader.boundary_breached(posNE, _boundary_num_points, _boundary, true)) { if (_poly_loader.boundary_breached(posNE, _boundary_num_points, _boundary)) {
return false; return false;
} }
} }
@ -472,7 +472,7 @@ Vector2f* AC_Fence::get_polygon_points(uint16_t& num_points) const
/// returns true if we've breached the polygon boundary. simple passthrough to underlying _poly_loader object /// returns true if we've breached the polygon boundary. simple passthrough to underlying _poly_loader object
bool AC_Fence::boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points) const bool AC_Fence::boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points) const
{ {
return _poly_loader.boundary_breached(location, num_points, points, true); return _poly_loader.boundary_breached(location, num_points, points);
} }
/// handler for polygon fence messages with GCS /// handler for polygon fence messages with GCS
@ -568,7 +568,7 @@ bool AC_Fence::load_polygon_from_eeprom(bool force_reload)
_boundary_loaded = true; _boundary_loaded = true;
// update validity of polygon // update validity of polygon
_boundary_valid = _poly_loader.boundary_valid(_boundary_num_points, _boundary, true); _boundary_valid = _poly_loader.boundary_valid(_boundary_num_points, _boundary);
return true; return true;
} }

25
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -54,18 +54,17 @@ bool AC_PolyFence_loader::save_point_to_eeprom(uint16_t i, const Vector2l& point
} }
// validate array of boundary points (expressed as either floats or long ints) // validate array of boundary points (expressed as either floats or long ints)
// contains_return_point should be true for plane which stores the return point as the first point in the array
// returns true if boundary is valid // returns true if boundary is valid
template <typename T> template <typename T>
bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>* points, bool contains_return_point) const bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>* points) const
{ {
// exit immediate if no points // exit immediate if no points
if (points == nullptr) { if (points == nullptr) {
return false; return false;
} }
// start from 2nd point if boundary contains return point (as first point) // start from 2nd point as boundary contains return point (as first point)
uint8_t start_num = contains_return_point ? 1 : 0; uint8_t start_num = 1;
// a boundary requires at least 4 point (a triangle and last point equals first) // a boundary requires at least 4 point (a triangle and last point equals first)
if (num_points < start_num + 4) { if (num_points < start_num + 4) {
@ -78,7 +77,7 @@ bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>*
} }
// check return point is within the fence // check return point is within the fence
if (contains_return_point && Polygon_outside(points[0], &points[1], num_points-start_num)) { if (Polygon_outside(points[0], &points[1], num_points-start_num)) {
return false; return false;
} }
@ -86,28 +85,26 @@ bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>*
} }
// check if a location (expressed as either floats or long ints) is within the boundary // check if a location (expressed as either floats or long ints) is within the boundary
// contains_return_point should be true for plane which stores the return point as the first point in the array
// returns true if location is outside the boundary // returns true if location is outside the boundary
template <typename T> template <typename T>
bool AC_PolyFence_loader::boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points, bool AC_PolyFence_loader::boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points) const
bool contains_return_point) const
{ {
// exit immediate if no points // exit immediate if no points
if (points == nullptr) { if (points == nullptr) {
return false; return false;
} }
// start from 2nd point if boundary contains return point (as first point) // start from 2nd point as boundary contains return point (as first point)
uint8_t start_num = contains_return_point ? 1 : 0; uint8_t start_num = 1;
// check location is within the fence // check location is within the fence
return Polygon_outside(location, &points[start_num], num_points-start_num); return Polygon_outside(location, &points[start_num], num_points-start_num);
} }
// declare type specific methods // declare type specific methods
template bool AC_PolyFence_loader::boundary_valid<int32_t>(uint16_t num_points, const Vector2l* points, bool contains_return_point) const; template bool AC_PolyFence_loader::boundary_valid<int32_t>(uint16_t num_points, const Vector2l* points) const;
template bool AC_PolyFence_loader::boundary_valid<float>(uint16_t num_points, const Vector2f* points, bool contains_return_point) const; template bool AC_PolyFence_loader::boundary_valid<float>(uint16_t num_points, const Vector2f* points) const;
template bool AC_PolyFence_loader::boundary_breached<int32_t>(const Vector2l& location, uint16_t num_points, template bool AC_PolyFence_loader::boundary_breached<int32_t>(const Vector2l& location, uint16_t num_points,
const Vector2l* points, bool contains_return_point) const; const Vector2l* points) const;
template bool AC_PolyFence_loader::boundary_breached<float>(const Vector2f& location, uint16_t num_points, template bool AC_PolyFence_loader::boundary_breached<float>(const Vector2f& location, uint16_t num_points,
const Vector2f* points, bool contains_return_point) const; const Vector2f* points) const;

6
libraries/AC_Fence/AC_PolyFence_loader.h
View File

@ -23,16 +23,14 @@ public:
// validate array of boundary points (expressed as either floats or long ints) // validate array of boundary points (expressed as either floats or long ints)
// contains_return_point should be true for plane which stores the return point as the first point in the array
// returns true if boundary is valid // returns true if boundary is valid
template <typename T> template <typename T>
bool boundary_valid(uint16_t num_points, const Vector2<T>* points, bool contains_return_point) const; bool boundary_valid(uint16_t num_points, const Vector2<T>* points) const;
// check if a location (expressed as either floats or long ints) is within the boundary // check if a location (expressed as either floats or long ints) is within the boundary
// contains_return_point should be true for plane which stores the return point as the first point in the array
// returns true if location is outside the boundary // returns true if location is outside the boundary
template <typename T> template <typename T>
bool boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points, bool contains_return_point) const; bool boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points) const;
}; };