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

AC_Fence: factor out check_fence_alt_max

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

72
libraries/AC_Fence/AC_Fence.cpp
View File

@ -145,6 +145,48 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
return true; return true;
} }
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_polygon - returns true if the polygon fence is freshly breached // check_fence_polygon - returns true if the polygon fence is freshly breached
bool AC_Fence::check_fence_polygon() bool AC_Fence::check_fence_polygon()
{ {
@ -252,33 +294,9 @@ uint8_t AC_Fence::check_fence(float curr_alt)
} }
} }
// altitude fence check // maximum altitude fence check
if ((_enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) { if (check_fence_alt_max(curr_alt)) {
ret |= AC_FENCE_TYPE_ALT_MAX;
// 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 // circle fence check

3
libraries/AC_Fence/AC_Fence.h
View File

@ -124,6 +124,9 @@ public:
private: private:
AC_Fence(const AP_AHRS_NavEKF &ahrs); AC_Fence(const AP_AHRS_NavEKF &ahrs);
/// check_fence_alt_max - true if alt fence has been newly breached
bool check_fence_alt_max(float curr_alt);
/// 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();