aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Fence: Add breach calculation variant to use LLA

Browse Source 
Adds a parameter to make selection between vector pos_cm from origin and lat/lng.
This commit is contained in:
James O'Shannessy 2020-10-12 14:24:43 +11:00 committed by Peter Barker
parent e041b647d7
commit f6231db618
2 changed files with 95 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -180,39 +180,30 @@ bool AC_PolyFence_loader::load_point_from_eeprom(uint16_t i, Vector2l& point) co
bool AC_PolyFence_loader::breached() const
{
// 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
struct Location loc;
if (!AP::ahrs().get_position(loc)) {
return false;
}
position = position * 100.0f; // m to cm
return breached(position);
return breached(loc);
}
bool AC_PolyFence_loader::breached(const Location& loc) const
{
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);
}
// check if a position (expressed as offsets in cm from the EKF origin) is within the boundary
// 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 Vector2f& pos_cm) const
bool AC_PolyFence_loader::breached(const Location& loc) const
{
if (!loaded()) {
return false;
}
Vector2l pos;
pos.x = loc.lat;
pos.y = loc.lng;
// 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];
if (Polygon_outside(pos_cm, boundary.points, boundary.count)) {
if (Polygon_outside(pos, boundary.points_lla, boundary.count)) {
return true;
}
}
@ -220,27 +211,29 @@ bool AC_PolyFence_loader::breached(const Vector2f& pos_cm) const
// 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];
if (!Polygon_outside(pos_cm, boundary.points, boundary.count)) {
if (!Polygon_outside(pos, boundary.points_lla, boundary.count)) {
return true;
}
}
// check circular excludes
for (uint8_t i=0; i<_num_loaded_circle_exclusion_boundaries; i++) {
const ExclusionCircle &circle = _loaded_circle_exclusion_boundary[i];
const Vector2f diff_cm = pos_cm - circle.pos_cm;
const float diff_cm_squared = diff_cm.length_squared();
if (diff_cm_squared < sq(circle.radius*100.0f)) {
Location circle_center;
circle_center.lat = circle.point.x;
circle_center.lng = circle.point.y;
const float diff_cm = loc.get_distance(circle_center)*100.0f;
if (diff_cm < circle.radius * 100.0f) {
return true;
}
}
// check circular includes
for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
const Vector2f diff_cm = pos_cm - circle.pos_cm;
const float diff_cm_squared = diff_cm.length_squared();
if (diff_cm_squared > sq(circle.radius*100.0f)) {
Location circle_center;
circle_center.lat = circle.point.x;
circle_center.lng = circle.point.y;
const float diff_cm = loc.get_distance(circle_center)*100.0f;
if (diff_cm > circle.radius * 100.0f) {
return true;
}
}
@ -339,24 +332,28 @@ out:
return ret;
}
bool AC_PolyFence_loader::read_scaled_latlon_from_storage(const Location &origin, uint16_t &read_offset, Vector2f &pos_cm)
bool AC_PolyFence_loader::scale_latlon_from_origin(const Location &origin, const Vector2l &point, Vector2f &pos_cm)
{
Location tmp_loc;
tmp_loc.lat = fence_storage.read_uint32(read_offset);
read_offset += 4;
tmp_loc.lng = fence_storage.read_uint32(read_offset);
read_offset += 4;
tmp_loc.lat = point.x;
tmp_loc.lng = point.y;
pos_cm = origin.get_distance_NE(tmp_loc) * 100.0f;
return true;
}
bool AC_PolyFence_loader::read_polygon_from_storage(const Location &origin, uint16_t &read_offset, const uint8_t vertex_count, Vector2f *&next_storage_point)
bool AC_PolyFence_loader::read_polygon_from_storage(const Location &origin, uint16_t &read_offset, const uint8_t vertex_count, Vector2f *&next_storage_point, Vector2l *&next_storage_point_lla)
{
for (uint8_t i=0; i<vertex_count; i++) {
// read and convert to lat/lon
if (!read_scaled_latlon_from_storage(origin, read_offset, *next_storage_point)) {
// read from storage to lat/lon
if (!read_latlon_from_storage(read_offset, *next_storage_point_lla)) {
return false;
}
// convert lat/lon to position in cm from origin
if (!scale_latlon_from_origin(origin, *next_storage_point_lla, *next_storage_point)) {
return false;
}
next_storage_point_lla++;
next_storage_point++;
}
return true;
@ -544,6 +541,9 @@ void AC_PolyFence_loader::unload()
delete[] _loaded_offsets_from_origin;
_loaded_offsets_from_origin = nullptr;
delete[] _loaded_points_lla;
_loaded_points_lla = nullptr;
delete[] _loaded_inclusion_boundary;
_loaded_inclusion_boundary = nullptr;
_num_loaded_inclusion_boundaries = 0;
@ -561,6 +561,7 @@ void AC_PolyFence_loader::unload()
_num_loaded_circle_exclusion_boundaries = 0;
_loaded_return_point = nullptr;
_loaded_return_point_lla = nullptr;
_load_time_ms = 0;
}
@ -637,7 +638,8 @@ bool AC_PolyFence_loader::load_from_eeprom()
Debug("Fence: Allocating %u bytes for points",
(unsigned)(count * sizeof(Vector2f)));
_loaded_offsets_from_origin = new Vector2f[count];
if (_loaded_offsets_from_origin == nullptr) {
_loaded_points_lla = new Vector2l[count];
if (_loaded_offsets_from_origin == nullptr || _loaded_points_lla == nullptr) {
unload();
get_loaded_fence_semaphore().give();
return false;
@ -695,6 +697,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
}
Vector2f *next_storage_point = _loaded_offsets_from_origin;
Vector2l *next_storage_point_lla = _loaded_points_lla;
// use index to load fences from eeprom
bool storage_valid = true;
@ -716,6 +719,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
// FIXME: consider factoring this with the EXCLUSION case
InclusionBoundary &boundary = _loaded_inclusion_boundary[_num_loaded_inclusion_boundaries];
boundary.points = next_storage_point;
boundary.points_lla = next_storage_point_lla;
boundary.count = index.count;
if (index.count < 3) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count");
@ -723,7 +727,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
break;
}
storage_offset += 1; // skip vertex count
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point)) {
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
storage_valid = false;
break;
@ -734,6 +738,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
case AC_PolyFenceType::POLYGON_EXCLUSION: {
ExclusionBoundary &boundary = _loaded_exclusion_boundary[_num_loaded_exclusion_boundaries];
boundary.points = next_storage_point;
boundary.points_lla = next_storage_point_lla;
boundary.count = index.count;
if (index.count < 3) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count");
@ -741,7 +746,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
break;
}
storage_offset += 1; // skip vertex count
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point)) {
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
storage_valid = false;
break;
@ -751,7 +756,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
}
case AC_PolyFenceType::CIRCLE_EXCLUSION: {
ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries];
if (!read_scaled_latlon_from_storage(ekf_origin, storage_offset, circle.pos_cm)) {
if (!read_latlon_from_storage(storage_offset, circle.point)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
@ -768,7 +778,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
}
case AC_PolyFenceType::CIRCLE_INCLUSION: {
InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries];
if (!read_scaled_latlon_from_storage(ekf_origin, storage_offset, circle.pos_cm)) {
if (!read_latlon_from_storage(storage_offset, circle.point)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)){
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
@ -790,12 +805,25 @@ bool AC_PolyFence_loader::load_from_eeprom()
break;
}
_loaded_return_point = next_storage_point;
if (!read_scaled_latlon_from_storage(ekf_origin, storage_offset, *next_storage_point)) {
if (_loaded_return_point_lla != nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
storage_valid = false;
break;
}
_loaded_return_point_lla = next_storage_point_lla;
// Read the point from storage
if (!read_latlon_from_storage(storage_offset, *next_storage_point_lla)) {
storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break;
}
if (!scale_latlon_from_origin(ekf_origin, *next_storage_point_lla, *next_storage_point)) {
storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break;
}
next_storage_point++;
next_storage_point_lla++;
break;
}
}

32
libraries/AC_Fence/AC_PolyFence_loader.h
View File

@ -162,13 +162,9 @@ public:
void update();
private:
// multi-thread access support
HAL_Semaphore _loaded_fence_sem;
// breached(Vector2f&) - returns true of pos_cm (an offset in cm from the EKF origin) breaches any fence
bool breached(const Vector2f& pos_cm) const WARN_IF_UNUSED;
/*
* Fence storage Index related functions
*/
@ -251,20 +247,28 @@ private:
// can be found:
Vector2f *_loaded_return_point;
// pointer into _loaded_points_lla where the return point
// can be found:
Vector2l *_loaded_return_point_lla;
class InclusionBoundary {
public:
Vector2f *points; // pointer into the _loaded_offsets_from_origin array
Vector2l *points_lla; // pointer into the _loaded_points_lla array
uint8_t count; // count of points in the boundary
};
InclusionBoundary *_loaded_inclusion_boundary;
uint8_t _num_loaded_inclusion_boundaries;
class ExclusionBoundary {
public:
Vector2f *points; // pointer into the _loaded_offsets_from_origin array
Vector2l *points_lla; // pointer into the _loaded_points_lla_lla array
uint8_t count; // count of points in the boundary
};
ExclusionBoundary *_loaded_exclusion_boundary;
uint8_t _num_loaded_exclusion_boundaries;
// _loaded_offsets_from_origin - stores x/y offset-from-origin
@ -272,21 +276,26 @@ private:
// allocation - the polygon boundaries and the return point, for
// example.
Vector2f *_loaded_offsets_from_origin;
Vector2l *_loaded_points_lla;
class ExclusionCircle {
public:
Vector2f pos_cm;
Vector2f pos_cm; // vector offset from home in cm
Vector2l point; // lat/lng of zone
float radius;
};
ExclusionCircle *_loaded_circle_exclusion_boundary;
uint8_t _num_loaded_circle_exclusion_boundaries;
class InclusionCircle {
public:
Vector2f pos_cm;
Vector2f pos_cm; // vector offset from home in cm
Vector2l point; // lat/lng of zone
float radius;
};
InclusionCircle *_loaded_circle_inclusion_boundary;
uint8_t _num_loaded_circle_inclusion_boundaries;
// _load_attempted - true if we have attempted to load the fences
@ -298,6 +307,13 @@ private:
// succeeded. Will be zero if fences are not loaded
uint32_t _load_time_ms;
// scale_latlon_from_origin - given a latitude/longitude
// transforms the point to an offset-from-origin and deposits
// the result into pos_cm.
bool scale_latlon_from_origin(const Location &origin,
const Vector2l &point,
Vector2f &pos_cm) WARN_IF_UNUSED;
// read_scaled_latlon_from_storage - reads a latitude/longitude
// from offset in permanent storage, transforms them into an
// offset-from-origin and deposits the result into pos_cm.
@ -313,7 +329,8 @@ private:
bool read_polygon_from_storage(const Location &origin,
uint16_t &read_offset,
const uint8_t vertex_count,
Vector2f *&next_storage_point) WARN_IF_UNUSED;
Vector2f *&next_storage_point,
Vector2l *&next_storage_point_lla) WARN_IF_UNUSED;
/*
* Upgrade functions - attempt to keep user's fences when
@ -368,6 +385,7 @@ private:
AP_Int8 &_total;
uint8_t _old_total;
// scan_eeprom - a method that traverses the fence storage area,
// calling the supplied callback for each fence found. If the
// scan fails (for example, the storage is corrupt), then this