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

AP_Beacon: update_boundary_points calculates outer perimeter around beacons

Browse Source
This commit is contained in:
Randy Mackay 2017-06-08 15:58:15 +09:00
parent c9475c2747
commit cded78022c
2 changed files with 146 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
libraries/AP_Beacon/AP_Beacon.cpp
View File

@ -119,6 +119,9 @@ void AP_Beacon::update(void)
return;
}
_driver->update();
// update boundary for fence
update_boundary_points();
}
// return origin of position estimate system
@ -225,6 +228,135 @@ uint32_t AP_Beacon::beacon_last_update_ms(uint8_t beacon_instance) const
return beacon_state[beacon_instance].distance_update_ms;
}
// create fence boundary points
void AP_Beacon::update_boundary_points()
{
// we need three beacons at least to create boundary fence.
// update boundary fence if number of beacons changes
if (!device_ready() || num_beacons < AP_BEACON_MINIMUM_FENCE_BEACONS || boundary_num_beacons == num_beacons) {
return;
}
// record number of beacons so we do not repeat calculations
boundary_num_beacons = num_beacons;
// accumulate beacon points
Vector2f beacon_points[AP_BEACON_MAX_BEACONS];
for (uint8_t index = 0; index < num_beacons; index++) {
const Vector3f& point_3d = beacon_position(index);
beacon_points[index].x = point_3d.x;
beacon_points[index].y = point_3d.y;
}
// create polygon around boundary points using the following algorithm
// set the "current point" as the first boundary point
// loop through all the boundary points looking for the point which creates a vector (from the current point to this new point) with the lowest angle
// check if point is already in boundary
// - no: add to boundary, move current point to this new point and repeat the above
// - yes: we've completed the bounding box, delete any boundary points found earlier than the duplicate
Vector2f boundary_points[AP_BEACON_MAX_BEACONS+1]; // array of boundary points
uint8_t curr_boundary_idx = 0; // index into boundary_sorted index. always points to the highest filled in element of the array
uint8_t curr_beacon_idx = 0; // index into beacon_point array. point indexed is same point as curr_boundary_idx's
// initialise first point of boundary_sorted with first beacon's position (this point may be removed later if it is found to not be on the outer boundary)
boundary_points[curr_boundary_idx] = beacon_points[curr_beacon_idx];
bool boundary_success = false; // true once the boundary has been successfully found
bool boundary_failure = false; // true if we fail to build the boundary
float start_angle = 0.0f; // starting angle used when searching for next boundary point, on each iteration this climbs but never climbs past PI * 2
while (!boundary_success && !boundary_failure) {
// look for next outer point
uint8_t next_idx;
float next_angle;
if (get_next_boundary_point(beacon_points, num_beacons, curr_beacon_idx, start_angle, next_idx, next_angle)) {
// add boundary point to boundary_sorted array
curr_boundary_idx++;
boundary_points[curr_boundary_idx] = beacon_points[next_idx];
curr_beacon_idx = next_idx;
start_angle = next_angle;
// check if we have a complete boundary by looking for duplicate points within the boundary_sorted
uint8_t dup_idx = 0;
bool dup_found = false;
while (dup_idx < curr_boundary_idx && !dup_found) {
dup_found = (boundary_points[dup_idx] == boundary_points[curr_boundary_idx]);
if (!dup_found) {
dup_idx++;
}
}
// if duplicate is found, remove all boundary points before the duplicate because they are inner points
if (dup_found) {
uint8_t num_pts = curr_boundary_idx - dup_idx + 1;
if (num_pts > AP_BEACON_MINIMUM_FENCE_BEACONS) {
// success, copy boundary points to boundary array and convert meters to cm
for (uint8_t j = 0; j < num_pts; j++) {
boundary[j] = boundary_points[j+dup_idx] * 100.0f;
}
boundary_num_points = num_pts;
boundary_success = true;
} else {
// boundary has too few points
boundary_failure = true;
}
}
} else {
// failed to create boundary - give up!
boundary_failure = true;
}
}
// clear boundary on failure
if (boundary_failure) {
boundary_num_points = 0;
}
}
// find next boundary point from an array of boundary points given the current index into that array
// returns true if a next point can be found
// current_index should be an index into the boundary_pts array
// start_angle is an angle (in radians), the search will sweep clockwise from this angle
// the index of the next point is returned in the next_index argument
// the angle to the next point is returned in the next_angle argument
bool AP_Beacon::get_next_boundary_point(const Vector2f* boundary_pts, uint8_t num_points, uint8_t current_index, float start_angle, uint8_t& next_index, float& next_angle)
{
// sanity check
if (boundary_pts == nullptr || current_index >= num_points) {
return false;
}
// get current point
Vector2f curr_point = boundary_pts[current_index];
// search through all points for next boundary point in a clockwise direction
float lowest_angle = M_PI_2;
float lowest_angle_relative = M_PI_2;
bool lowest_found = false;
uint8_t lowest_index;
for (uint8_t i=0; i < num_points; i++) {
if (i != current_index) {
Vector2f vec = boundary_pts[i] - curr_point;
if (!vec.is_zero()) {
float angle = wrap_2PI(atan2f(vec.y, vec.x));
float angle_relative = wrap_2PI(angle - start_angle);
if ((angle_relative < lowest_angle_relative) || !lowest_found) {
lowest_angle = angle;
lowest_angle_relative = angle_relative;
lowest_index = i;
lowest_found = true;
}
}
}
}
// return results
if (lowest_found) {
next_index = lowest_index;
next_angle = lowest_angle;
}
return lowest_found;
}
// return fence boundary array
const Vector2f* AP_Beacon::get_boundary_points(uint16_t& num_points) const
{

16
libraries/AP_Beacon/AP_Beacon.h
View File

@ -91,6 +91,9 @@ public:
// return last update time from beacon in milliseconds
uint32_t beacon_last_update_ms(uint8_t beacon_instance) const;
// update fence boundary array
void update_boundary_points();
// return fence boundary array
const Vector2f* get_boundary_points(uint16_t& num_points) const;
@ -101,6 +104,14 @@ private:
// check if device is ready
bool device_ready(void) const;
// find next boundary point from an array of boundary points given the current index into that array
// returns true if a next point can be found
// current_index should be an index into the boundary_pts array
// start_angle is an angle (in radians), the search will sweep clockwise from this angle
// the index of the next point is returned in the next_index argument
// the angle to the next point is returned in the next_angle argument
static bool get_next_boundary_point(const Vector2f* boundary, uint8_t num_points, uint8_t current_index, float start_angle, uint8_t& next_index, float& next_angle);
// parameters
AP_Int8 _type;
AP_Float origin_lat;
@ -122,6 +133,7 @@ private:
BeaconState beacon_state[AP_BEACON_MAX_BEACONS];
// fence boundary
Vector2f *boundary;
uint8_t boundary_num_beacons;
Vector2f boundary[AP_BEACON_MAX_BEACONS+1]; // array of boundary points (used for fence)
uint8_t boundary_num_points; // number of points in boundary
uint8_t boundary_num_beacons; // total number of beacon points consumed while building boundary
};