避障增加等待用户控制最大延时，时间到原地降落,降落不触发避障;超出范围条件调整

ArduCopter/zr_flight.cpp

@@ -169,11 +169,11 @@ uint8_t Copter::avoid_area_detect(float dist)
 {
     Proxy_Area area;
     if(dist < avoid.get_zr_avd_min()){
-
-        if(area != PROXY_TOO_CLOSE)  // 前一个状态不是障碍物过近
+        if(area != PROXY_TOO_CLOSE && area != PROXY_FIND_BARRIER){  // 前一个状态不是障碍物过近
             area = PROXY_OUTRANGE;
-        area = PROXY_OUTRANGE;
-        
+        }else{
+            area = PROXY_TOO_CLOSE;
+        }
     }
     else if(dist< avoid.get_zr_avd_trig())
         area = PROXY_TOO_CLOSE;
@@ -208,17 +208,16 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
 {
     static uint8_t delay_cnt;
     static Proxy_Action action;
-    static const float get_org_max_speed = wp_nav->get_default_speed_xy();
-    static float avoid_slow_speed = 100.0f;
-
     Vector3f pos_neu_cm;
     static int32_t max_climb_alt = 0;
-
     int32_t turn_yaw;
-
+    static uint32_t ctl_wait_time = AP_HAL::millis();
     switch (action)
     {
     case PROXY_WAIT:
+        if(control_mode == Mode::Number::LAND ){    // 降落模式不触发避障
+            break;
+        }
         if(area == PROXY_FIND_BARRIER && state == PROXY_APPROACH)
             action = PROXY_SLOW_DOWN;
         if(area == PROXY_TOO_CLOSE)
@@ -226,13 +225,7 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
         break;
     case PROXY_SLOW_DOWN:
         if(state == PROXY_APPROACH){
-            
-            avoid_slow_speed = (dist - avoid.get_zr_avd_trig())/(avoid.get_zr_avd_max() - avoid.get_zr_avd_trig())*loiter_nav->get_loiter_speed();
-            avoid_slow_speed = MAX(avoid_slow_speed,0);
-            
-            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 发现障碍物，距离:%3.2fm,v:%.2f",dist,avoid_slow_speed);
-            wp_nav->set_speed_xy(avoid_slow_speed);
-            loiter_nav->set_loiter_speed(avoid_slow_speed);
+            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 发现障碍物，距离:%3.2fm",dist);
             // last_prx_diff = dist;
         }
         if(area == PROXY_TOO_CLOSE)
@@ -285,6 +278,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();
             gcs().send_text(MAV_SEVERITY_CRITICAL,"超出爬升高度限制:%d，请手动控制",current_loc.alt);
 
         }
@@ -315,17 +309,22 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
         }else{       
             gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:开始返航");
             set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
-            wp_nav->set_speed_xy(get_org_max_speed);
-            loiter_nav->set_loiter_speed(get_org_max_speed);
             action = PROXY_WAIT;
         }
  
         break;
     case PROXY_NEED_CTL:
-        
-        if(area == PROXY_OUTRANGE){
-            action = PROXY_WAIT;
-            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:人为控制，已离开障碍");
+        {
+            uint32_t tnow = AP_HAL::millis();
+            if(area == PROXY_OUTRANGE){
+                action = PROXY_WAIT;
+                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:人为控制，已离开障碍");
+            }
+            if(tnow - ctl_wait_time > 30000){
+                action = PROXY_WAIT;
+                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，原地降落");
+                set_mode(Mode::Number::LAND,ModeReason::AVOIDANCE);
+            }
         }
         break;