diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index bce2798efb..2bca229df5 100644 --- a/ArduCopter/Parameters.cpp +++ b/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); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 9071dfe08a..3490d71dab 100644 --- a/ArduCopter/Parameters.h +++ b/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. diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 9e86ac58db..6173097a80 100644 --- a/ArduCopter/zr_flight.cpp +++ b/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); diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp index d8760ce4b1..72ab38c4cf 100644 --- a/libraries/AP_Notify/Display.cpp +++ b/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;