From 05f139794e51c7deafd7c52f17c9632ad2074806 Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 25 Nov 2021 15:53:24 +0800 Subject: [PATCH] =?UTF-8?q?=E9=81=BF=E9=9A=9C=E8=B0=83=E6=95=B4=EF=BC=8C?= =?UTF-8?q?=E6=89=8B=E5=8A=A8=E9=81=BF=E9=9A=9Cbug=E4=BF=AE=E5=A4=8D?= =?UTF-8?q?=EF=BC=8C=E8=87=AA=E5=8A=A8=E9=81=BF=E9=9A=9C=E6=9C=89=E6=95=88?= =?UTF-8?q?=E5=80=BC=E5=88=A4=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/zr_flight.cpp | 5 +++-- all.sh | 10 +++++----- libraries/AC_Avoidance/AC_Avoid.cpp | 4 +--- libraries/AC_Avoidance/AC_Avoid.h | 1 - 4 files changed, 9 insertions(+), 11 deletions(-) diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 781bc577c3..e05d0a120c 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -166,8 +166,9 @@ void Copter::do_avoid_action(){ 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){ + bool get_new_dist = proximity->get_horizontal_distance(315,proxy_dist); + bool data_failed = proximity->sensor_failed(); + if(get_new_dist == false || data_failed){ proxy_dist = avoid.get_zr_avd_max() + 1.0; } #endif diff --git a/all.sh b/all.sh index 049f9debe8..d5ccdc9083 100755 --- a/all.sh +++ b/all.sh @@ -1,20 +1,20 @@ ./waf configure --board rs100 ./waf clean ./waf copter -cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100-v4.0.16-RC3.px4 +cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100-v4.0.16-RC5.px4 ./waf configure --board rs100h ./waf clean ./waf copter -cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100h-v4.0.16-RC3.px4 +cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100h-v4.0.16-RC5.px4 ./waf configure --board d100 ./waf clean ./waf copter -cp ./build/d100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100-v4.0.16-RC3.px4 +cp ./build/d100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100-v4.0.16-RC5.px4 ./waf configure --board d100h ./waf clean ./waf copter -cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100h-v4.0.16-RC3.px4 +cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100h-v4.0.16-RC5.px4 ./waf configure --board zr-hexa ./waf clean ./waf copter -cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/六轴m66-v4.0.16-RC3.px4 +cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/六轴m66-v4.0.16-RC5.px4 diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 56c97cb5d7..c7d1fa6fc3 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -98,9 +98,7 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel _avoid_action = false; return; } - if(_enable_aviod == false){ - return; - } + // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 010bf7ac44..db3a485922 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -164,7 +164,6 @@ private: 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) - bool _enable_aviod; // 离开Home点一点高度和距离后有效 bool _far_form_home; // 离开Home点一定高度和距离后有效 bool _avoid_action;