增加参数设置避障等待人为控制延时时间

ArduCopter/Parameters.cpp

@@ -145,6 +145,7 @@ const AP_Param::Info Copter::var_info[] = {
     GSCALAR(zr_tk_delay,  "ZR_TK_DELAY", ZR_TK_DELAY),
     GSCALAR(zr_use_rc,  "ZR_USE_RC", 1),
     GSCALAR(zr_reg_date,  "ZR_REG_DATE", 1),
+    GSCALAR(zr_avd_wait,  "ZR_AVD_WAIT", 60),   // 避障无法绕过，等待人为控制延时,单位s
 
     // @Param: ZR_RTL_DELAY
     // @DisplayName: rtl  Altitude  when at final decent alt
@@ -1721,9 +1722,9 @@ const char* Copter::get_sysid_board_id(void)
     
         int32_t deadline = 0;
         get_deadline_params(deadline);
-        gcs().send_text(MAV_SEVERITY_INFO, "deadline:%ld",deadline);
+        gcs().send_text(MAV_SEVERITY_INFO, "date:%ld",deadline);
         if(g.zr_reg_date != deadline){
-            gcs().send_text(MAV_SEVERITY_INFO, "reload deadline:%ld, ->, %ld",deadline,g.zr_reg_date);
+            gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld ->  %ld",(int32_t)g.zr_reg_date,deadline);
             g.zr_reg_date.set_and_save_ifchanged(deadline);
         }
 

ArduCopter/Parameters.h

@@ -396,6 +396,7 @@ public:
         k_param_hardware_flag,
         k_param_zr_reg_date,
         k_param_zr_8_circle,
+        k_param_zr_avd_wait,
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -512,6 +513,7 @@ public:
     AP_Int8                  zr_use_rc;
     AP_Int32                 zr_reg_date;
     AP_Int8                  zr_8_circle;
+    AP_Int16                 zr_avd_wait;
     
     // Note: keep initializers here in the same order as they are declared
     // above.

ArduCopter/zr_flight.cpp

@@ -251,7 +251,7 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
             // if(need_turn_yaw){
             //     action = PROXY_CLIMB;
                 max_climb_alt = current_loc.alt+avoid.get_zr_avd_max_climb();
-                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:准备爬升，Now:%d，MAX:%d，",current_loc.alt,max_climb_alt);
+                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:准备爬升，Now:%.2f，MAX:%.2f，",current_loc.alt/100.0f,max_climb_alt/100.0f);
 
             // }else{
             //     action = PROXY_RTL;
@@ -282,7 +282,7 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
         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);
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"超出爬升高度限制:%.2f，请手动控制",current_loc.alt/100.0f);
 
         }
         if(area == PROXY_OUTRANGE){
@@ -323,7 +323,7 @@ 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 > 30000){
+            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);

libraries/AP_Notify/Display.cpp

@@ -932,16 +932,7 @@ void Display::update_camera(uint8_t r)
 void Display::update_display_manual()
 {
     static uint8_t time_cnt;
-
     static uint8_t display_stage = 1;
-
-    // if (time_cnt++ > 10) {
-    //     time_cnt = 0;
-    //     display_stage++;
-    // }
-    // if(display_stage>5)
-    //     display_stage = 1;
-    RESTART1:
     switch (display_stage)
     {
         case 1:
@@ -962,33 +953,7 @@ void Display::update_display_manual()
             update_chinese(0);
             update_camera(2);
             update_battery(4);
-            
             break;
-        // case 5:
-        //     // update_text(0);
-        //     update_chinese(0);
-        //     // update_gps(2);
-        //     update_camera(4);
-        //     break;
-        // case 6:
-
-        //     display_stage = 1;
-        //     goto RESTART1;
-        //     // update_text(0);
-        //     update_chinese(0);
-        //     update_mode(2);
-        //     update_battery(4);
-        //     break;
-        // case 7:
-        //     // update_text(0);
-        //     update_chinese(0);
-        //     update_gps(2);
-        //     update_camera(4);
-        //     break;
-        // case 8:
-        //     display_stage = 1;
-        //     goto RESTART1;
-
     
         default:
             break;