// check_fence_polygon - returns true if the polygon fence is freshly breached
bool AC_Fence::check_fence_polygon()
{
if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) {
// not enabled; no breach
return false;
}
// check consistency of number of points
if (_boundary_num_points != _total) {
// Fence is currently not completely loaded. Can't breach it?!
_boundary_loaded = false;
load_polygon_from_eeprom();
return false;
}
if (!_boundary_valid) {
// fence isn't valid - can't breach it?!
return false;
}
// check if vehicle is outside the polygon fence
Vector2f position;
if (!_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
if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary, true)) {
// check if this is a new breach
if (_breached_fences & AC_FENCE_TYPE_POLYGON) {
// not a new breach
return false;
}
// record that we have breached the polygon
record_breach(AC_FENCE_TYPE_POLYGON);
return true;
}
// inside boundary; clear breach if present
if (_breached_fences & AC_FENCE_TYPE_POLYGON) {
clear_breach(AC_FENCE_TYPE_POLYGON);
}
return false;
}
/// check_fence - returns the fence type that has been breached (if any)
/// curr_alt is the altitude above home in meters
uint8_t AC_Fence::check_fence(float curr_alt)
{
uint8_t ret = AC_FENCE_TYPE_NONE;
// return immediately if disabled
if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) {
return AC_FENCE_TYPE_NONE;
}
// check if pilot is attempting to recover manually
if (_manual_recovery_start_ms != 0) {
// we ignore any fence breaches during the manual recovery period which is about 10 seconds
if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) {
return AC_FENCE_TYPE_NONE;
} else {
// recovery period has passed so reset manual recovery time and continue with fence breach checks
_manual_recovery_start_ms = 0;
}
}
// altitude fence check
if ((_enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
// check if we are over the altitude fence
if( curr_alt >= _alt_max ) {
// record distance above breach
_alt_max_breach_distance = curr_alt - _alt_max;
// check for a new breach or a breach of the backup fence
if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) == 0 || (!is_zero(_alt_max_backup) && curr_alt >= _alt_max_backup)) {
// record that we have breached the upper limit
record_breach(AC_FENCE_TYPE_ALT_MAX);
ret |= AC_FENCE_TYPE_ALT_MAX;
// create a backup fence 20m higher up
_alt_max_backup = curr_alt + AC_FENCE_ALT_MAX_BACKUP_DISTANCE;
}
}else{
// clear alt breach if present
if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
clear_breach(AC_FENCE_TYPE_ALT_MAX);
_alt_max_backup = 0.0f;
_alt_max_breach_distance = 0.0f;
}
}
}
// circle fence check
if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) != 0 ) {
// check if we are outside the fence
if (_home_distance >= _circle_radius) {
// record distance outside the fence
_circle_breach_distance = _home_distance - _circle_radius;
// check for a new breach or a breach of the backup fence
if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) == 0 || (!is_zero(_circle_radius_backup) && _home_distance >= _circle_radius_backup)) {
// record that we have breached the circular distance limit
record_breach(AC_FENCE_TYPE_CIRCLE);
ret |= AC_FENCE_TYPE_CIRCLE;
// create a backup fence 20m further out
_circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE;
}
}else{
// clear circle breach if present
if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) != 0) {
clear_breach(AC_FENCE_TYPE_CIRCLE);
_circle_radius_backup = 0.0f;
_circle_breach_distance = 0.0f;
}
}
}
// polygon fence check
if ((_enabled_fences & AC_FENCE_TYPE_POLYGON) != 0 ) {
// check consistency of number of points
if (_boundary_num_points != _total) {
_boundary_loaded = false;
}
// load fence if necessary
if (!_boundary_loaded) {
load_polygon_from_eeprom();
} else if (_boundary_valid) {
// check if vehicle is outside the polygon fence
const Vector3f& position = _inav.get_position();
if (_poly_loader.boundary_breached(Vector2f(position.x, position.y), _boundary_num_points, _boundary, true)) {
// check if this is a new breach
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) == 0) {
// record that we have breached the polygon
record_breach(AC_FENCE_TYPE_POLYGON);
ret |= AC_FENCE_TYPE_POLYGON;
}
} else {
// clear breach if present
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) {
clear_breach(AC_FENCE_TYPE_POLYGON);
}
}
}
}
// return any new breaches that have occurred
return ret;
}