Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: factor out check_fence_circle

Browse Source 
NFC
This commit is contained in:
Peter Barker 2017-12-08 11:18:29 +11:00 committed by Randy Mackay
parent d379c7bbd9
commit d04a4dd0f8
2 changed files with 42 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
libraries/AC_Fence/AC_Fence.cpp
View File

@ -192,6 +192,43 @@ bool AC_Fence::check_fence_polygon()
return false; return false;
} }
bool AC_Fence::check_fence_circle()
{
if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) {
// not enabled; no breach
return false;
}
// 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;
}
/// check_fence - returns the fence type that has been breached (if any) /// check_fence - returns the fence type that has been breached (if any)
/// curr_alt is the altitude above home in meters /// curr_alt is the altitude above home in meters
@ -245,32 +282,8 @@ uint8_t AC_Fence::check_fence(float curr_alt)
} }
// circle fence check // circle fence check
if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) != 0 ) { if (check_fence_circle()) {
ret |= AC_FENCE_TYPE_CIRCLE;
// 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 // polygon fence check

3
libraries/AC_Fence/AC_Fence.h
View File

@ -127,6 +127,9 @@ private:
/// check_fence_polygon - true if polygon fence has been newly breached /// check_fence_polygon - true if polygon fence has been newly breached
bool check_fence_polygon(); bool check_fence_polygon();
/// check_fence_circle - true if circle fence has been newly breached
bool check_fence_circle();
/// record_breach - update breach bitmask, time and count /// record_breach - update breach bitmask, time and count
void record_breach(uint8_t fence_type); void record_breach(uint8_t fence_type);