void AC_Fence::enable(bool value)
{
_enabled = value;
if (!value) {
clear_breach(AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON);
}
}
// 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;
}
bool AC_Fence::check_fence_alt_max()
{
// altitude fence check
if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MAX)) {
// not enabled; no breach
return false;
}
_ahrs.get_relative_position_D_home(_curr_alt);
_curr_alt = -_curr_alt; // translate Down to Up
// 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) ||
(!is_zero(_alt_max_backup) && _curr_alt >= _alt_max_backup)) {
// new breach
record_breach(AC_FENCE_TYPE_ALT_MAX);
// create a backup fence 20m higher up
_alt_max_backup = _curr_alt + AC_FENCE_ALT_MAX_BACKUP_DISTANCE;
// new breach:
return true;
}
// old breach:
return false;
}
// not breached
// 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;
}
return false;
}
bool AC_Fence::check_fence_circle()
{
if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) {
// not enabled; no breach
return false;
}
Vector2f home;
if (_ahrs.get_relative_position_NE_home(home)) {
// we (may) remain breached if we can't update home
_home_distance = home.length();
}
// 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) ||
(!is_zero(_circle_radius_backup) && _home_distance >= _circle_radius_backup)) {
// new breach
// create a backup fence 20m further out
record_breach(AC_FENCE_TYPE_CIRCLE);
_circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE;
return true;
}
return false;
}
// not currently breached
// clear circle breach if present
if (_breached_fences & AC_FENCE_TYPE_CIRCLE) {
clear_breach(AC_FENCE_TYPE_CIRCLE);
_circle_radius_backup = 0.0f;
_circle_breach_distance = 0.0f;
}
return false;
}
bool AC_Fence::check_fence_alt_max(const float curr_alt)
{
// altitude fence check
if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MAX)) {
// not enabled; no breach
return false;
}
// 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) ||
(!is_zero(_alt_max_backup) && curr_alt >= _alt_max_backup)) {
// new breach
record_breach(AC_FENCE_TYPE_ALT_MAX);
// create a backup fence 20m higher up
_alt_max_backup = curr_alt + AC_FENCE_ALT_MAX_BACKUP_DISTANCE;
// new breach:
return true;
}
// old breach:
return false;
}
// not breached
// 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;
}
return false;
}
/// check_fence - returns the fence type that has been breached (if any)
uint8_t AC_Fence::check_fence()
{
uint8_t ret = AC_FENCE_TYPE_NONE;
// return immediately if disabled
if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) {
return AC_FENCE_TYPE_NONE;
}
// get current altitude in meters
float curr_alt = _inav->get_position().z * 0.01f;
// 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 || (_alt_max_backup != 0 && curr_alt >= _alt_max_backup)) {
// record that we have breached the upper limit
record_breach(AC_FENCE_TYPE_ALT_MAX);
ret = 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;
_alt_max_breach_distance = 0;
}
}
}
// 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 || (_circle_radius_backup != 0 && _home_distance >= _circle_radius_backup)) {
// record that we have breached the circular distance limit
record_breach(AC_FENCE_TYPE_CIRCLE);
ret = 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;
_circle_breach_distance = 0;
}
}
}
// return any new breaches that have occurred
return ret;
// To-Do: add min alt and polygon check
//outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
}
/// check_fence - returns the fence type that has been breached (if any)
uint8_t AC_Fence::check_fence()
{
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 ((hal.scheduler->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;
}
}
// get current altitude in meters
float curr_alt = _inav->get_altitude() * 0.01f;
// 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 || (_alt_max_backup != 0.0f && curr_alt >= _alt_max_backup)) {
// record that we have breached the upper limit
record_breach(AC_FENCE_TYPE_ALT_MAX);
ret = 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 || (_circle_radius_backup != 0.0f && _home_distance >= _circle_radius_backup)) {
// record that we have breached the circular distance limit
record_breach(AC_FENCE_TYPE_CIRCLE);
ret = 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;
}
}
}
// return any new breaches that have occurred
return ret;
// To-Do: add min alt and polygon check
//outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
}
/// 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;
}
