起飞侧翻增加期望值偏差参数

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

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.

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();
         }

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

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