Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: move polygon points into AC_Fence_Polygon

This commit is contained in:
Peter Barker 2019-05-29 23:02:04 +10:00 committed by Randy Mackay
parent 189ef5f1e7
commit ff37590776
4 changed files with 114 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AC_Fence/AC_Fence.cpp
View File

@ -116,7 +116,7 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
return true;
}
if (!_boundary_valid) {
if (!_poly_loader.valid()) {
fail_msg = "Polygon boundary invalid";
return false;
}
@ -258,27 +258,9 @@ bool AC_Fence::polygon_fence_is_breached()
return false;
}
// check consistency of number of points
if (_boundary_num_points != _total) {
// Fence is currently not completely loaded. Can't breach it?!
load_polygon_from_eeprom();
return false;
}
if (!_boundary_valid) {
// fence isn't valid - can't breach it?!
return false;
return _poly_loader.breached();
}
// check if vehicle is outside the polygon fence
Vector2f position;
if (!AP::ahrs().get_relative_position_NE_origin(position)) {
// we have no idea where we are; can't breach the fence
return false;
}
position = position * 100.0f; // m to cm
return _poly_loader.boundary_breached(position, _boundary_num_points, _boundary);
}
bool AC_Fence::check_fence_circle()
{
@ -385,15 +367,11 @@ bool AC_Fence::check_destination_within_fence(const Location& loc)
}
// polygon fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON) && _boundary_num_points > 0) {
// check ekf has a good location
Vector2f posNE;
if (loc.get_vector_xy_from_origin_NE(posNE)) {
if (_poly_loader.boundary_breached(posNE, _boundary_num_points, _boundary)) {
if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) {
if (_poly_loader.breached(loc)) {
return false;
}
}
}
return true;
}
@ -449,17 +427,17 @@ void AC_Fence::manual_recovery_start()
}
/// returns pointer to array of polygon points and num_points is filled in with the total number
Vector2f* AC_Fence::get_boundary_points(uint16_t& num_points) const
Vector2f* AC_PolyFence_loader::get_boundary_points(uint16_t& num_points) const
{
// return array minus the first point which holds the return location
if (_boundary == nullptr) {
return nullptr;
}
if (!_boundary_valid) {
if (!valid()) {
return nullptr;
}
// minus one for return point, minus one for closing point
// (_boundary_valid is not true unless we have a closing point AND
// (polygon().valid() is not true unless we have a closing point AND
// we have a minumum number of points)
if (_boundary_num_points < 2) {
return nullptr;
@ -468,14 +446,8 @@ Vector2f* AC_Fence::get_boundary_points(uint16_t& num_points) const
return &_boundary[1];
}
/// 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
{
return _poly_loader.boundary_breached(location, num_points, points);
}
/// handler for polygon fence messages with GCS
void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg)
{
switch (msg.msgid) {
// receive a fence point from GCS and store in EEPROM
@ -488,7 +460,7 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
Vector2l point;
point.x = packet.lat*1.0e7f;
point.y = packet.lng*1.0e7f;
if (!_poly_loader.save_point_to_eeprom(packet.idx, point)) {
if (!save_point_to_eeprom(packet.idx, point)) {
link.send_text(MAV_SEVERITY_WARNING, "Failed to save polygon point, too many points?");
} else {
// trigger reload of points
@ -504,7 +476,7 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
mavlink_msg_fence_fetch_point_decode(&msg, &packet);
// attempt to retrieve from eeprom
Vector2l point;
if (_poly_loader.load_point_from_eeprom(packet.idx, point)) {
if (load_point_from_eeprom(packet.idx, point)) {
mavlink_msg_fence_point_send(link.get_chan(), msg.sysid, msg.compid, packet.idx, _total, point.x*1.0e-7f, point.y*1.0e-7f);
} else {
link.send_text(MAV_SEVERITY_WARNING, "Bad fence point");
@ -519,12 +491,12 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
}
/// load polygon points stored in eeprom into boundary array and perform validation
bool AC_Fence::load_polygon_from_eeprom()
bool AC_PolyFence_loader::load_from_eeprom()
{
// check if we need to create array
if (!_boundary_create_attempted) {
_boundary = (Vector2f *)_poly_loader.create_point_array(sizeof(Vector2f));
_boundary_create_attempted = true;
if (!_create_attempted) {
_boundary = (Vector2f *)create_point_array(sizeof(Vector2f));
_create_attempted = true;
}
// exit if we could not allocate RAM for the boundary
@ -543,13 +515,13 @@ bool AC_Fence::load_polygon_from_eeprom()
}
// sanity check total
_total = constrain_int16(_total, 0, _poly_loader.max_points());
_total = constrain_int16(_total, 0, max_points());
// load each point from eeprom
Vector2l temp_latlon;
for (uint16_t index=0; index<_total; index++) {
// load boundary point as lat/lon point
if (!_poly_loader.load_point_from_eeprom(index, temp_latlon)) {
if (!load_point_from_eeprom(index, temp_latlon)) {
return false;
}
// move into location structure and convert to offset from ekf origin
@ -558,10 +530,10 @@ bool AC_Fence::load_polygon_from_eeprom()
_boundary[index] = ekf_origin.get_distance_NE(temp_loc) * 100.0f;
}
_boundary_num_points = _total;
_boundary_update_ms = AP_HAL::millis();
_update_ms = AP_HAL::millis();
// update validity of polygon
_boundary_valid = _poly_loader.boundary_valid(_boundary_num_points, _boundary);
_valid = calculate_boundary_valid();
return true;
}
@ -593,7 +565,7 @@ bool AC_Fence::sys_status_failed() const
return true;
}
if (_enabled_fences & AC_FENCE_TYPE_POLYGON) {
if (!_boundary_valid) {
if (!_poly_loader.valid()) {
return true;
}
}

35
libraries/AC_Fence/AC_Fence.h
View File

@ -96,26 +96,6 @@ public:
/// has no effect if no breaches have occurred
void manual_recovery_start();
///
/// polygon related methods
///
/// returns true if polygon fence is valid (i.e. has at least 3 sides)
bool is_polygon_valid() const { return _boundary_valid; }
/// returns pointer to array of polygon points and num_points is filled in with the total number
/// points are offsets from EKF origin in NE frame
Vector2f* get_boundary_points(uint16_t& num_points) const;
/// returns true if we've breached the polygon boundary. simple passthrough to underlying _poly_loader object
bool boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points) const;
/// handler for polygon fence messages with GCS
void handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg);
/// return system time of last update to the boundary (allows external detection of boundary changes)
uint32_t get_boundary_update_ms() const { return _boundary_update_ms; }
static const struct AP_Param::GroupInfo var_info[];
// methods for mavlink SYS_STATUS message (send_sys_status)
@ -126,6 +106,9 @@ public:
// get singleton instance
static AC_Fence *get_singleton() { return _singleton; }
AC_PolyFence_loader &polyfence() { return _poly_loader; }
const AC_PolyFence_loader &polyfence_const() const { return _poly_loader; }
private:
static AC_Fence *_singleton;
@ -149,9 +132,6 @@ private:
bool pre_arm_check_circle(const char* &fail_msg) const;
bool pre_arm_check_alt(const char* &fail_msg) const;
/// load polygon points stored in eeprom into boundary array and perform validation. returns true if load successfully completed
bool load_polygon_from_eeprom();
// returns true if we have breached the fence:
bool polygon_fence_is_breached();
@ -185,13 +165,8 @@ private:
uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control
// polygon fence variables
AC_PolyFence_loader _poly_loader; // helper for loading/saving polygon points
Vector2f *_boundary = nullptr; // array of boundary points. Note: point 0 is the return point
uint8_t _boundary_num_points = 0; // number of points in the boundary array (should equal _total parameter after load has completed)
bool _boundary_create_attempted = false; // true if we have attempted to create the boundary array
bool _boundary_valid = false; // true if boundary forms a closed polygon
uint32_t _boundary_update_ms; // system time of last update to the boundary
AC_PolyFence_loader _poly_loader{_total}; // polygon fence
};
namespace AP {

64
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -53,13 +53,12 @@ bool AC_PolyFence_loader::save_point_to_eeprom(uint16_t i, const Vector2l& point
return true;
}
// validate array of boundary points (expressed as either floats or long ints)
// validate array of boundary points
// returns true if boundary is valid
template <typename T>
bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>* points) const
bool AC_PolyFence_loader::calculate_boundary_valid() const
{
// exit immediate if no points
if (points == nullptr) {
if (_boundary == nullptr) {
return false;
}
@ -67,30 +66,61 @@ bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>*
uint8_t start_num = 1;
// a boundary requires at least 4 point (a triangle and last point equals first)
if (num_points < start_num + 4) {
if (_boundary_num_points < start_num + 4) {
return false;
}
// point 1 and last point must be the same. Note: 0th point is reserved as the return point
if (!Polygon_complete(&points[start_num], num_points-start_num)) {
if (!Polygon_complete(&_boundary[start_num], _boundary_num_points-start_num)) {
return false;
}
// check return point is within the fence
if (Polygon_outside(points[0], &points[1], num_points-start_num)) {
if (Polygon_outside(_boundary[0], &_boundary[1], _boundary_num_points-start_num)) {
return false;
}
return true;
}
// check if a location (expressed as either floats or long ints) is within the boundary
// returns true if location is outside the boundary
template <typename T>
bool AC_PolyFence_loader::boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points) const
bool AC_PolyFence_loader::breached()
{
// check if vehicle is outside the polygon fence
Vector2f position;
if (!AP::ahrs().get_relative_position_NE_origin(position)) {
// we have no idea where we are; can't breach the fence
return false;
}
position = position * 100.0f; // m to cm
return breached(position);
}
bool AC_PolyFence_loader::breached(const Location& loc)
{
Vector2f posNE;
if (!loc.get_vector_xy_from_origin_NE(posNE)) {
// not breached if we don't now where we are
return false;
}
return breached(posNE);
}
// returns true if location is outside the boundary
bool AC_PolyFence_loader::breached(const Vector2f& location)
{
// check consistency of number of points
if (_boundary_num_points != _total) {
// Fence is currently not completely loaded. Can't breach it?!
load_from_eeprom();
return false;
}
// exit immediate if no points
if (points == nullptr) {
if (_boundary == nullptr) {
return false;
}
if (!_valid) {
// fence isn't valid - can't breach it?!
return false;
}
@ -98,13 +128,5 @@ bool AC_PolyFence_loader::boundary_breached(const Vector2<T>& location, uint16_t
uint8_t start_num = 1;
// check location is within the fence
return Polygon_outside(location, &points[start_num], num_points-start_num);
return Polygon_outside(location, &_boundary[start_num], _boundary_num_points-start_num);
}
// declare type specific methods
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) const;
template bool AC_PolyFence_loader::boundary_breached<int32_t>(const Vector2l& location, uint16_t num_points,
const Vector2l* points) const;
template bool AC_PolyFence_loader::boundary_breached<float>(const Vector2f& location, uint16_t num_points,
const Vector2f* points) const;

54
libraries/AC_Fence/AC_PolyFence_loader.h
View File

@ -2,12 +2,49 @@
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
class AC_PolyFence_loader
{
public:
AC_PolyFence_loader(AP_Int8 &total) :
_total(total) {}
AC_PolyFence_loader(const AC_PolyFence_loader &other) = delete;
AC_PolyFence_loader &operator=(const AC_PolyFence_loader&) = delete;
/// returns pointer to array of polygon points and num_points is
/// filled in with the total number. This does not include the
/// return point or the closing point.
Vector2f* get_boundary_points(uint16_t& num_points) const;
/// handler for polygon fence messages with GCS
void handle_msg(GCS_MAVLINK &link, mavlink_message_t* msg);
bool breached();
// returns true if location is outside the boundary
bool breached(const Location& loc);
// returns true if boundary is valid
bool valid() const {
return _valid;
}
// returns the system in in millis when the boundary was last updated
uint32_t update_ms() const {
return _update_ms;
}
private:
bool breached(const Vector2f& location);
/// load polygon points stored in eeprom into boundary array and
/// perform validation. returns true if load successfully
/// completed
bool load_from_eeprom();
// maximum number of fence points we can store in eeprom
uint8_t max_points() const;
@ -21,16 +58,15 @@ public:
// save a fence point to eeprom, returns true on successful save
bool save_point_to_eeprom(uint16_t i, const Vector2l& point);
// update the validity flag:
bool calculate_boundary_valid() const;
// validate array of boundary points (expressed as either floats or long ints)
// returns true if boundary is valid
template <typename T>
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
// returns true if location is outside the boundary
template <typename T>
bool boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points) const;
Vector2f *_boundary; // array of boundary points. Note: point 0 is the return point
uint8_t _boundary_num_points; // number of points in the boundary array (should equal _total parameter after load has completed)
bool _create_attempted; // true if we have attempted to create the boundary array
bool _valid; // true if boundary forms a closed polygon
uint32_t _update_ms; // system time of last update to the boundary
AP_Int8 &_total;
};