Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues 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; 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 /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
bool AC_Fence::pre_arm_check(const char* &fail_msg) const 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 // if we got this far everything must be ok
return true; return true;
} }
@ -542,5 +563,11 @@ bool AC_Fence::geofence_failed() const
if (get_breaches() != 0) { if (get_breaches() != 0) {
return true; return true;
} }
if (_enabled_fences & AC_FENCE_TYPE_POLYGON) {
if (!_boundary_valid) {
return true;
}
}
return false; return false;
} }

3
libraries/AC_Fence/AC_Fence.h
View File

@ -130,6 +130,9 @@ private:
/// clear_breach - update breach bitmask, time and count /// clear_breach - update breach bitmask, time and count
void clear_breach(uint8_t fence_type); 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 /// 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); bool load_polygon_from_eeprom(bool force_reload = false);