From 588b0c17c832f47d8863df310090617a4dc4914c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 15 Dec 2017 21:06:56 +1100 Subject: [PATCH] AC_Fence: remove AC_FENCE_TYPE_NONE define 0 is the appropriate value for a bitmask which is empty --- libraries/AC_Fence/AC_Fence.cpp | 38 ++++++++++++--------------------- libraries/AC_Fence/AC_Fence.h | 1 - 2 files changed, 14 insertions(+), 25 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 2b929230d7..98550dfa8f 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -74,16 +74,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { /// Default constructor. AC_Fence::AC_Fence(const AP_AHRS_NavEKF& ahrs) : - _ahrs(ahrs), - _alt_max_backup(0), - _circle_radius_backup(0), - _alt_max_breach_distance(0), - _circle_breach_distance(0), - _home_distance(0), - _breached_fences(AC_FENCE_TYPE_NONE), - _breach_time(0), - _breach_count(0), - _manual_recovery_start_ms(0) + _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); @@ -108,10 +99,9 @@ void AC_Fence::enable(bool value) uint8_t AC_Fence::get_enabled_fences() const { if (!_enabled) { - return AC_FENCE_TYPE_NONE; - }else{ - return _enabled_fences; + return 0; } + return _enabled_fences; } /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully @@ -120,12 +110,12 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const fail_msg = nullptr; // if not enabled or not fence set-up always return true - if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { + if (!_enabled || !_enabled_fences) { return true; } // check no limits are currently breached - if (_breached_fences != AC_FENCE_TYPE_NONE) { + if (_breached_fences) { fail_msg = "vehicle outside fence"; return false; } @@ -284,22 +274,22 @@ bool AC_Fence::check_fence_circle() /// check - returns bitmask of fence types breached (if any) uint8_t AC_Fence::check() { - uint8_t ret = AC_FENCE_TYPE_NONE; + uint8_t ret = 0; // return immediately if disabled - if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { - return AC_FENCE_TYPE_NONE; + if (!_enabled || !_enabled_fences) { + return 0; } // check if pilot is attempting to recover manually if (_manual_recovery_start_ms != 0) { // we ignore any fence breaches during the manual recovery period which is about 10 seconds if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { - return AC_FENCE_TYPE_NONE; - } else { - // recovery period has passed so reset manual recovery time and continue with fence breach checks - _manual_recovery_start_ms = 0; + return 0; } + // recovery period has passed so reset manual recovery time + // and continue with fence breach checks + _manual_recovery_start_ms = 0; } // maximum altitude fence check @@ -359,7 +349,7 @@ bool AC_Fence::check_destination_within_fence(const Location_Class& loc) void AC_Fence::record_breach(uint8_t fence_type) { // if we haven't already breached a limit, update the breach time - if (_breached_fences == AC_FENCE_TYPE_NONE) { + if (!_breached_fences) { _breach_time = AP_HAL::millis(); } @@ -407,7 +397,7 @@ float AC_Fence::get_breach_distance(uint8_t fence_type) const void AC_Fence::manual_recovery_start() { // return immediate if we haven't breached a fence - if (_breached_fences == AC_FENCE_TYPE_NONE) { + if (!_breached_fences) { return; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index b45671f1b2..f8133826c4 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -10,7 +10,6 @@ #include // bit masks for enabled fence types. Used for TYPE parameter -#define AC_FENCE_TYPE_NONE 0 // fence disabled #define AC_FENCE_TYPE_ALT_MAX 1 // high alt fence which usually initiates an RTL #define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL) #define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence