避障是佛启用条件增加Home点距离，增加避不开后执行动作返航

ArduCopter/Copter.h

@@ -1027,7 +1027,8 @@ private:
         PROXY_DELAY,
         PROXY_CLIMB,
         PROXY_NEED_CTL,
-        PROXY_RTL
+        PROXY_RTL,
+        PROXY_RTL_ERR
     };
     Proxy_State proxy_state;
     Proxy_Area proxy_area;

ArduCopter/UserCode.cpp

@@ -50,8 +50,18 @@ void Copter::userhook_MediumLoop()
 if(current_loc.alt > avoid.get_zr_avd_alt())
     do_avoid_action();
 #else
-if(current_loc.alt > avoid.get_zr_avd_alt() && avoid.get_zr_mode()>0 && in_debug_mode > 0)
-    do_avoid_action();
+
+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;
+        }
+    }
+    if(!just_alarm){
+        do_avoid_action();
+    }
+}
 #endif
 }
 #endif

ArduCopter/zr_flight.cpp

@@ -144,19 +144,12 @@ void Copter::do_avoid_action(){
     uint16_t rc10_in = RC_Channels::rc_channel(CH_6)->get_radio_in();
     proxy_dist = (rc10_in - 1100)/900.0*50.0;
 #else
-    if(avoid.get_zr_mode() == 1){
-
-        AP_Proximity *proximity = AP::proximity();
-        if(proximity == nullptr)
-            return;
-        bool get_new_dist = proximity->get_horizontal_distance(0,proxy_dist);
-        if(get_new_dist == false){
-            proxy_dist = avoid.get_zr_avd_max() + 1.0;
-        }
-    }
-    if(avoid.get_zr_mode() == 2){
-        uint16_t rc10_in = RC_Channels::rc_channel(CH_12)->get_radio_in();
-        proxy_dist = (rc10_in - 1100)/900.0*50.0;
+    AP_Proximity *proximity = AP::proximity();
+    if(proximity == nullptr)
+        return;
+    bool get_new_dist = proximity->get_horizontal_distance(0,proxy_dist);
+    if(get_new_dist == false){
+        proxy_dist = avoid.get_zr_avd_max() + 1.0;
     }
 #endif
 
@@ -214,7 +207,7 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
     Vector3f pos_neu_cm;
     static int32_t max_climb_alt = 0;
     int32_t turn_yaw;
-    static uint32_t ctl_wait_time = AP_HAL::millis();
+    static uint32_t rtl_wait_time = AP_HAL::millis();
     switch (action)
     {
     case PROXY_WAIT:
@@ -281,7 +274,7 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
         // mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
         if(current_loc.alt > max_climb_alt){
             action = PROXY_NEED_CTL;
-            ctl_wait_time = AP_HAL::millis();
+            rtl_wait_time = AP_HAL::millis();
             gcs().send_text(MAV_SEVERITY_CRITICAL,"超出爬升高度限制:%.2f，请手动控制",current_loc.alt/100.0f);
 
         }
@@ -323,13 +316,26 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
                 action = PROXY_WAIT;
                 gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:人为控制，已离开障碍");
             }
-            if(tnow - ctl_wait_time > g.zr_avd_wait * 1000){
-                action = PROXY_WAIT;
-                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，原地降落");
-                set_mode(Mode::Number::LAND,ModeReason::AVOIDANCE);
+            if(tnow - rtl_wait_time > (uint32_t)(g.zr_avd_wait * 1000)){
+                if(avoid.get_zr_mode() == 1){ 
+                    action = PROXY_WAIT;
+                    gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，原地降落");
+                    set_mode(Mode::Number::LAND,ModeReason::AVOIDANCE);
+                }else{
+                    action = PROXY_RTL_ERR;
+                    gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，关闭避障返航");
+                    set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
+                }
             }
         }
         break;
+    case PROXY_RTL_ERR:
+    {
+        if(control_mode == Mode::Number::LAND ){    // 如果切换到Land，则可以重新开始
+            action = PROXY_WAIT;
+            break;
+        }
+    }
     
     default:
         break;

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -72,8 +72,9 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
     AP_GROUPINFO("ZR_TRIG", 7, AC_Avoid, _zr_trig, 25),     //  触发避障距离，单位米
     AP_GROUPINFO("ZR_MAX", 8, AC_Avoid, _zr_max, 35),     //  检测最大距离，单位米
     AP_GROUPINFO("ZR_MIN", 9, AC_Avoid, _zr_min, 5),      //  检测最小距离，单位米   
-    AP_GROUPINFO("ZR_ALT", 10, AC_Avoid, _zr_alt, 2500),        // 启用避障高度，单位厘米
+    AP_GROUPINFO("ZR_ON_H", 10, AC_Avoid, _zr_alt, 2500),        // 启用避障高度，单位厘米
     AP_GROUPINFO("ZR_CLB", 11, AC_Avoid, _zr_max_climb, 3000),  // 最大爬升高度设置，单位厘米
+    AP_GROUPINFO("ZR_ON_S", 12, AC_Avoid, _zr_on_dist, 2500),        // 启用避障高度，单位厘米
 
     AP_GROUPEND
 };

libraries/AC_Avoidance/AC_Avoid.h

@@ -87,6 +87,8 @@ public:
     int8_t get_zr_avd_max() const { return _zr_max; }
     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; }
 
     static const struct AP_Param::GroupInfo var_info[];
@@ -159,6 +161,8 @@ private:
     AP_Int8 _zr_max;
     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)