mirror of https://github.com/ArduPilot/ardupilot
AC_Fence: Change logic to fence enable and sys_status reporting
This commit is contained in:
parent
4f7ffc5eb1
commit
783a4b7965
|
@ -18,7 +18,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
|
|||
// @Description: Allows you to enable (1) or disable (0) the fence functionality
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0),
|
||||
//AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0),
|
||||
|
||||
// @Param: TYPE
|
||||
// @DisplayName: Fence Type
|
||||
|
@ -159,11 +159,18 @@ void AC_Fence::disable_floor()
|
|||
|
||||
bool AC_Fence::present() const
|
||||
{
|
||||
// A fence is present if it is enabled and there are points present
|
||||
if (_poly_loader.total_fence_count() == 0) {
|
||||
return false;
|
||||
// A fence is present if any of the conditions are true.
|
||||
// * tin can (circle) is enabled
|
||||
// * min or max alt is enabled
|
||||
// * polygon fences are enabled and any fence has been uploaded
|
||||
if (_enabled_fences.get() & AC_FENCE_TYPE_CIRCLE ||
|
||||
_enabled_fences.get() & AC_FENCE_TYPE_ALT_MIN ||
|
||||
_enabled_fences.get() & AC_FENCE_TYPE_ALT_MAX ||
|
||||
((_enabled_fences.get() & AC_FENCE_TYPE_POLYGON) && _poly_loader.total_fence_count() > 0)) {
|
||||
return true;
|
||||
}
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/// get_enabled_fences - returns bitmask of enabled fences
|
||||
|
@ -598,7 +605,8 @@ bool AC_Fence::sys_status_enabled() const
|
|||
if (_action == AC_FENCE_ACTION_REPORT_ONLY) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
// Fence is only enabled when the flag is enabled
|
||||
return _enabled;
|
||||
}
|
||||
|
||||
bool AC_Fence::sys_status_failed() const
|
||||
|
|
|
@ -177,7 +177,6 @@ private:
|
|||
bool pre_arm_check_alt(const char* &fail_msg) const;
|
||||
|
||||
// parameters
|
||||
AP_Int8 _enabled; // fence enable/disable control
|
||||
AP_Int8 _auto_enabled; // top level flag for auto enabling fence
|
||||
AP_Int8 _enabled_fences; // bit mask holding which fences are enabled
|
||||
AP_Int8 _action; // recovery action specified by user
|
||||
|
@ -200,6 +199,7 @@ private:
|
|||
float _circle_breach_distance; // distance beyond the circular fence
|
||||
|
||||
// other internal variables
|
||||
bool _enabled; // fence enable/disable control
|
||||
bool _floor_enabled; // fence floor is enabled
|
||||
float _home_distance; // distance from home in meters (provided by main code)
|
||||
float _curr_alt;
|
||||
|
|
Loading…
Reference in New Issue