避障调整，自动、返航、引导模式启用自己的避障，Loiter避障增加距离限制

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

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);
 

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;
 };