From 633fd0f6c97ff3c2641dfbec989f003128f78fdb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 20 Sep 2019 11:07:26 +0900 Subject: [PATCH] AC_Avoidance: add enabled and margin accessors --- libraries/AC_Avoidance/AC_Avoid.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 6d91c23f9e..dbf554b22d 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -35,6 +35,9 @@ public: return _singleton; } + // return true if any avoidance feature is enabled + bool enabled() const { return _enabled != AC_AVOID_DISABLED; } + /* * Adjusts the desired velocity so that the vehicle can stop * before the fence/object. @@ -76,6 +79,9 @@ public: // kP should be non-zero for Copter which has a non-linear response float get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const; + // return margin (in meters) that the vehicle should stay from objects + float get_margin() const { return _margin; } + static const struct AP_Param::GroupInfo var_info[]; private: