From 4044ea1a42aa15f0d9863c1007f1f979fedbf3a0 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Sat, 31 Jul 2021 23:04:01 +0900 Subject: [PATCH] AC_Avoid: confirm USE_PROXIMITY_SENSOR to check if proximity is enabled --- libraries/AC_Avoidance/AC_Avoid.cpp | 2 +- libraries/AC_Avoidance/AC_Avoid.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 63060052aa..b3508ebf06 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -390,7 +390,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // get distance from proximity sensor float proximity_alt_diff; AP_Proximity *proximity = AP::proximity(); - if (proximity && _proximity_enabled && proximity->get_upward_distance(proximity_alt_diff)) { + if (proximity && proximity_avoidance_enabled() && proximity->get_upward_distance(proximity_alt_diff)) { proximity_alt_diff -= _margin; if (!limit_alt || proximity_alt_diff < alt_diff) { alt_diff = proximity_alt_diff; diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index b27c6d7116..2b05fd29ca 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -81,7 +81,7 @@ public: // enable/disable proximity based avoidance void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; } - bool proximity_avoidance_enabled() const { return _proximity_enabled; } + bool proximity_avoidance_enabled() const { return (_proximity_enabled && (_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0); } void proximity_alt_avoidance_enable(bool on_off) { _proximity_alt_enabled = on_off; } // helper functions