From 2a120734e933a05d77c0252062896419ecac5c35 Mon Sep 17 00:00:00 2001
From: zbr <zbr3550@sina.com>
Date: Fri, 17 Sep 2021 10:15:57 +0800
Subject: [PATCH] =?UTF-8?q?=E9=81=BF=E9=9A=9C=E8=B0=83=E6=95=B4=EF=BC=8C?=
 =?UTF-8?q?=E8=87=AA=E5=8A=A8=E3=80=81=E8=BF=94=E8=88=AA=E3=80=81=E5=BC=95?=
 =?UTF-8?q?=E5=AF=BC=E6=A8=A1=E5=BC=8F=E5=90=AF=E7=94=A8=E8=87=AA=E5=B7=B1?=
 =?UTF-8?q?=E7=9A=84=E9=81=BF=E9=9A=9C=EF=BC=8CLoiter=E9=81=BF=E9=9A=9C?=
 =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=B7=9D=E7=A6=BB=E9=99=90=E5=88=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 ArduCopter/UserCode.cpp             | 23 ++++++++++++++++-------
 libraries/AC_Avoidance/AC_Avoid.cpp |  4 +++-
 libraries/AC_Avoidance/AC_Avoid.h   |  4 +++-
 3 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp
index 96eea4858d..8c5321643c 100644
--- a/ArduCopter/UserCode.cpp
+++ b/ArduCopter/UserCode.cpp
@@ -50,18 +50,27 @@ void Copter::userhook_MediumLoop()
 if(current_loc.alt > avoid.get_zr_avd_alt())
     do_avoid_action();
 #else
-
-if(avoid.get_zr_mode()>0 && in_debug_mode > 0){
-    bool just_alarm = false;
-    if(current_loc.alt < avoid.get_zr_avd_alt()){
-        if(home_distance() < avoid.get_zr_avd_on_dist()){
-            just_alarm = true;
+bool flag_avoid_on = true;
+static bool last_avoid_flag = true;
+if(avoid.get_zr_mode() > 0 && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::GUIDED)){
+    
+    if(current_loc.alt < avoid.get_zr_avd_alt()){   // 飞行高度小于避障启用高度
+        if(home_distance() < avoid.get_zr_avd_on_dist()){   // 距离Home点小于启用距离
+            flag_avoid_on = false;   //关闭避障
         }
     }
-    if(!just_alarm){
+    if(flag_avoid_on){
         do_avoid_action();
     }
 }
+if(last_avoid_flag != flag_avoid_on){
+    avoid.set_enable_aviod(flag_avoid_on);
+    if(flag_avoid_on){
+        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 避障开启!");
+    }else{
+        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 避障关闭!");
+    }
+}
 #endif
 }
 #endif
diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp
index 22fe7edf86..5e3ef28c7e 100644
--- a/libraries/AC_Avoidance/AC_Avoid.cpp
+++ b/libraries/AC_Avoidance/AC_Avoid.cpp
@@ -93,7 +93,9 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel
     if (_enabled == AC_AVOID_DISABLED) {
         return;
     }
-
+    if(_enable_aviod == false){
+        return;
+    }
     // limit acceleration
     const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
 
diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h
index 8f1e4650a9..9fba2de812 100644
--- a/libraries/AC_Avoidance/AC_Avoid.h
+++ b/libraries/AC_Avoidance/AC_Avoid.h
@@ -90,7 +90,7 @@ public:
     int16_t get_zr_avd_on_dist() const { return _zr_on_dist; }
 
     int16_t get_zr_avd_max_climb() const { return _zr_max_climb; }
-
+    void set_enable_aviod(bool flag) {_enable_aviod = flag;}
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -167,6 +167,8 @@ private:
 
     bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
 
+    bool _enable_aviod;  // 离开Home点一点高度和距离后有效
+
     static AC_Avoid *_singleton;
 };