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

AC_Fence: extend pre-arm checks

Browse Source 
Fix comments
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-08-24 11:48:17 +02:00 committed by Peter Barker
parent a3efc9ad98
commit 22bdc3e54c
2 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AC_Fence/AC_Fence.cpp
View File

@ -276,6 +276,22 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
return false;
}
// validate FENCE_MARGIN parameter range
if (_margin < 0.0f) {
fail_msg = "Invalid FENCE_MARGIN value";
return false;
}
if (_alt_max < _alt_min) {
fail_msg = "FENCE_ALT_MAX < FENCE_ALT_MIN";
return false;
}
if (_alt_max - _alt_min <= 2.0f * _margin) {
fail_msg = "FENCE_MARGIN too big";
return false;
}
// if we got this far everything must be ok
return true;
}
@ -343,14 +359,14 @@ bool AC_Fence::check_fence_alt_min()
_curr_alt = -_curr_alt; // translate Down to Up
// check if we are under the altitude fence
if (_curr_alt < _alt_min) {
if (_curr_alt <= _alt_min) {
// record distance below breach
_alt_min_breach_distance = _alt_min - _curr_alt;
// check for a new breach or a breach of the backup fence
if (!(_breached_fences & AC_FENCE_TYPE_ALT_MIN) ||
(!is_zero(_alt_min_backup) && _curr_alt < _alt_min_backup)) {
(!is_zero(_alt_min_backup) && _curr_alt <= _alt_min_backup)) {
// new breach
record_breach(AC_FENCE_TYPE_ALT_MIN);

2
libraries/AC_Fence/AC_Fence.h
View File

@ -120,7 +120,7 @@ public:
/// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin)
float get_safe_alt_max() const { return _alt_max - _margin; }
/// get_safe_alt_min - returns the minimum safe altitude (i.e. alt_min - margin)
/// get_safe_alt_min - returns the minimum safe altitude (i.e. alt_min + margin)
float get_safe_alt_min() const { return _alt_min + _margin; }
/// get_radius - returns the fence radius in meters