避障区分手动和自动

ArduCopter/Copter.h

@@ -1060,6 +1060,7 @@ public:
     uint8_t cacl_volt_pst;
     uint8_t mkdir_step;
     Mode::Number get_before_mode() const { return before_mode; };
+    bool far_from_home;
 };
 
 extern Copter copter;

ArduCopter/Parameters.cpp

@@ -155,8 +155,8 @@ const AP_Param::Info Copter::var_info[] = {
     // @Increment: 1
     // @User: Standard
     GSCALAR(zr_rtl_delay,  "ZR_RTL_DELAY", ZR_RTL_DELAY),
+    GSCALAR(zr_8_circle,  "ZR_8_CICLE", 0), // 是否启用绕8字
 
-    GSCALAR(zr_8_circle,  "ZR_8_circle", 1),    // 是否使用绕8字
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT
     // @DisplayName: RTL Altitude

ArduCopter/UserCode.cpp

@@ -50,26 +50,24 @@ void Copter::userhook_MediumLoop()
 if(current_loc.alt > avoid.get_zr_avd_alt())
     do_avoid_action();
 #else
-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)){
+bool enable_avoid = false;
+static bool last_flag = false;
 
-    if(current_loc.alt < avoid.get_zr_avd_alt()){   // 飞行高度小于避障启用高度
-        if(home_distance() < avoid.get_zr_avd_on_dist()){   // 距离Home点小于启用距离
-            flag_avoid_on = false;   //关闭避障
-        }
-    }
-    if(flag_avoid_on){
-        do_avoid_action();
+if(avoid.get_zr_mode() > 0 && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::CIRCLE)){
+    if(far_from_home){
+        enable_avoid = true;
     }
 }
-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: 避障开启!");
+if(last_flag != enable_avoid){
+    if(enable_avoid){
+        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开启自动避障!");
     }else{
-        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 避障关闭!");
+        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 关闭自动避障!");
     }
+    last_flag = enable_avoid;
+}
+if(enable_avoid){
+    do_avoid_action();
 }
 #endif
 }
@@ -92,6 +90,26 @@ void Copter::userhook_SuperSlowLoop()
     zr_SuperSlowLoop();
     zr_set_uav_state_to_uavcan();
     zr_camera_mkdir();
+    if(current_loc.alt < avoid.get_zr_avd_alt()){   // 飞行高度小于避障启用高度
+        if(home_distance() < abs(avoid.get_zr_avd_on_dist())){   // 距离Home点小于启用距离
+            far_from_home = false;
+        }else{
+            far_from_home = true;
+        }
+    }else{
+     far_from_home = true;    
+    }
+    avoid.set_enable_aviod(far_from_home); // 远离Home点才启用避障
+    /// 手动避障开启提示
+    static bool last_manual_avoid = avoid.get_avoid_action();
+    if(last_manual_avoid != avoid.get_avoid_action()){
+        if(avoid.get_avoid_action()){
+            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开启手动避障!");
+        }else{
+            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 关闭手动避障!");
+        }
+        last_manual_avoid = avoid.get_avoid_action();
+    }
 #if CAM_DEBUG 
     if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){            
         camera.take_picture();

ArduCopter/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.16-AVDv3"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.16-AVDv5"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,16,FIRMWARE_VERSION_TYPE_OFFICIAL

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -91,6 +91,11 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel
 {
     // exit immediately if disabled
     if (_enabled == AC_AVOID_DISABLED) {
+        _avoid_action = false;
+        return;
+    }
+    if(_far_form_home == false){
+        _avoid_action = false;
         return;
     }
     if(_enable_aviod == false){
@@ -111,6 +116,7 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel
     }
 
     if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) {
+        _avoid_action = true;
         adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel_cms, dt);
     }
 }

libraries/AC_Avoidance/AC_Avoid.h

@@ -88,11 +88,11 @@ public:
     int8_t get_zr_avd_min() const { return _zr_min; }
     int16_t get_zr_avd_alt() const { return _zr_alt; }
     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[];
+    void set_enable_aviod(bool flag) { _far_form_home = flag; }
+    bool get_avoid_action() const { return _avoid_action; }
 
+    static const struct AP_Param::GroupInfo var_info[];
 private:
     // behaviour types (see BEHAVE parameter)
     enum BehaviourType {
@@ -162,13 +162,12 @@ private:
     AP_Int8 _zr_min;
     AP_Int16 _zr_alt;
     AP_Int16 _zr_on_dist;
-    
     AP_Int16 _zr_max_climb;
-
     bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
-
     bool _enable_aviod;  // 离开Home点一点高度和距离后有效
 
+    bool _far_form_home;  // 离开Home点一定高度和距离后有效
+    bool _avoid_action;
     static AC_Avoid *_singleton;
 };