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

AC_Fence: add a pre-arm check for polygon fence validity

Browse Source 
Also change SYS_STATUS bit to be unhealthy if boundary invalid
This commit is contained in:
Peter Barker 2017-12-14 13:54:03 +11:00 committed by Francisco Ferreira
parent aa7389b455
commit 155407956c
2 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AC_Fence/AC_Fence.cpp
View File

@ -104,6 +104,23 @@ uint8_t AC_Fence::get_enabled_fences() const
return _enabled_fences;
}
// additional checks for the polygon fence:
bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
{
if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) {
// not enabled; all good
return true;
}
if (!_boundary_valid) {
fail_msg = "Polygon boundary invalid";
return false;
}
return true;
}
/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
bool AC_Fence::pre_arm_check(const char* &fail_msg) const
{
@ -131,6 +148,10 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
}
}
if (!pre_arm_check_polygon(fail_msg)) {
return false;
}
// if we got this far everything must be ok
return true;
}
@ -542,5 +563,11 @@ bool AC_Fence::geofence_failed() const
if (get_breaches() != 0) {
return true;
}
if (_enabled_fences & AC_FENCE_TYPE_POLYGON) {
if (!_boundary_valid) {
return true;
}
}
return false;
}

3
libraries/AC_Fence/AC_Fence.h
View File

@ -130,6 +130,9 @@ private:
/// clear_breach - update breach bitmask, time and count
void clear_breach(uint8_t fence_type);
// additional checks for the polygon fence:
bool pre_arm_check_polygon(const char* &fail_msg) const;
/// load polygon points stored in eeprom into boundary array and perform validation. returns true if load successfully completed
bool load_polygon_from_eeprom(bool force_reload = false);