From ab312b18f61edb4c8e453a19a05e571e9afba07e Mon Sep 17 00:00:00 2001 From: zbr Date: Wed, 26 Jan 2022 17:27:37 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B5=B7=E9=A3=9E=E4=BE=A7=E7=BF=BB=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E6=9C=9F=E6=9C=9B=E5=80=BC=E5=81=8F=E5=B7=AE=E5=8F=82?= =?UTF-8?q?=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 3 ++- ArduCopter/Parameters.h | 2 ++ ArduCopter/UserCode.cpp | 16 +++++++++------- ArduCopter/version.h | 2 +- rs100.sh | 2 +- 5 files changed, 15 insertions(+), 10 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 546efb0ee9..4c1e7415c9 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -158,8 +158,9 @@ const AP_Param::Info Copter::var_info[] = { GSCALAR(zr_8_circle, "ZR_8_CICLE", 0), // 是否启用绕8字 GSCALAR(zr_land_lock_att, "ZR_ATT_LOCK", 1), // 是否启用锁定 - GSCALAR(zr_takeoff_prt_deg, "ZR_TK_PTDG", 20), // 起飞侧翻保护，角度 + GSCALAR(zr_takeoff_prt_deg, "ZR_TK_PTDG", 25), // 起飞侧翻保护，角度 GSCALAR(zr_takeoff_prt_ps, "ZR_TK_PTPS", 20), // 起飞侧翻保护，延时，100hz计数 + GSCALAR(zr_takeoff_prt_errdeg, "ZR_TK_ERRD", 15), // 起飞侧翻保护，角度 #if MODE_RTL_ENABLED == ENABLED // @Param: RTL_ALT // @DisplayName: RTL Altitude diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index f1c5110bbb..6ae4000178 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -401,6 +401,7 @@ public: k_param_zr_land_lock_att, k_param_zr_takeoff_prt_deg, k_param_zr_takeoff_prt_ps, + k_param_zr_takeoff_prt_errdeg, // the k_param_* space is 9-bits in size // 511: reserved @@ -523,6 +524,7 @@ public: AP_Int8 zr_land_lock_att; AP_Int8 zr_takeoff_prt_deg; AP_Int8 zr_takeoff_prt_ps; + AP_Int8 zr_takeoff_prt_errdeg; // Note: keep initializers here in the same order as they are declared // above. diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 953eba4c1b..9699ead2df 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -282,10 +282,12 @@ void Copter::zr_mkdir_in_takeoff() void Copter::takeoff_crash_detect() { - static uint16_t crash_counter; // number of iterations vehicle may have been crashed + static uint16_t crash_counter; + static uint16_t err_counter; // return immediately if disarmed, or crash checking disabled if (!motors->armed() || g.fs_crash_check == 0) { crash_counter = 0; + err_counter = 0; return; } @@ -296,17 +298,17 @@ void Copter::takeoff_crash_detect() // check for lean angle over 15 degrees 角度判断，角度小于稳定飞行值退出 const float lean_angle_deg = degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch())); if (lean_angle_deg > float(g.zr_takeoff_prt_deg)) { - crash_counter +=1 ; + crash_counter += 1 ; }else{ crash_counter = 0; } // // check for angle error over 30 degrees const float angle_error = attitude_control->get_att_error_angle_deg(); - if (angle_error > float(g.zr_takeoff_prt_deg)/2.0) { - crash_counter +=1 ; + if (angle_error > float(g.zr_takeoff_prt_errdeg)) { + err_counter += 1; }else{ - crash_counter = 0; + err_counter = 0; } // check if crashing for 2 seconds AP::logger().Write("TKPT", "TimeUS,Ldeg,Aerr,Alt", "Qfff", @@ -315,12 +317,12 @@ void Copter::takeoff_crash_detect() angle_error, rangefinder_state.alt_cm_filt.get()); - if (crash_counter >= (g.zr_takeoff_prt_ps)) { + if (crash_counter >= (g.zr_takeoff_prt_ps) || err_counter >= (g.zr_takeoff_prt_ps) ) { AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); // keep logging even if disarmed: AP::logger().set_force_log_disarmed(true); // send message to gcs - gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Takeoff crash protect");//Crash: Disarming + gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Takeoff crash protect"); // disarm motors arming.disarm(); } diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 981688faca..2a194cef67 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -5,7 +5,7 @@ #endif #include "ap_version.h" -#define GIT_VERSION "3" +#define GIT_VERSION "5" #define THISFIRMWARE "ZRUAV v4.1.16" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts diff --git a/rs100.sh b/rs100.sh index bb628ec5e0..461bcd3e2f 100755 --- a/rs100.sh +++ b/rs100.sh @@ -1,3 +1,3 @@ ./waf configure --board rs100 ./waf copter -cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100-v4.1.16.px4 +cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100-v4.1.16-5.px4