From 882d67b95dfd65b337c703b28172bfff47929c0c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AD=A4=E5=B8=86=E8=BF=9C=E5=BD=B1Faraway?= Date: Thu, 9 Sep 2021 23:47:01 +0800 Subject: [PATCH] =?UTF-8?q?=E9=81=BF=E9=9A=9C=E6=98=AF=E4=BD=9B=E5=90=AF?= =?UTF-8?q?=E7=94=A8=E6=9D=A1=E4=BB=B6=E5=A2=9E=E5=8A=A0Home=E7=82=B9?= =?UTF-8?q?=E8=B7=9D=E7=A6=BB=EF=BC=8C=E5=A2=9E=E5=8A=A0=E9=81=BF=E4=B8=8D?= =?UTF-8?q?=E5=BC=80=E5=90=8E=E6=89=A7=E8=A1=8C=E5=8A=A8=E4=BD=9C=E8=BF=94?= =?UTF-8?q?=E8=88=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Copter.h | 3 +- ArduCopter/UserCode.cpp | 14 +++++++-- ArduCopter/zr_flight.cpp | 44 ++++++++++++++++------------- libraries/AC_Avoidance/AC_Avoid.cpp | 3 +- libraries/AC_Avoidance/AC_Avoid.h | 4 +++ 5 files changed, 45 insertions(+), 23 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2435b802fc..1db2093b03 100644 --- a/ArduCopter/Copter.h +++ b/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; diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index f254866b9a..96eea4858d 100644 --- a/ArduCopter/UserCode.cpp +++ b/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 diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 6173097a80..074e52f3b4 100644 --- a/ArduCopter/zr_flight.cpp +++ b/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; diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index baedfadb9a..22fe7edf86 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/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 }; diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 8541203dbd..8f1e4650a9 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/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)