增加起飞侧翻保护

起飞侧翻保护增加期望值差异判断，日志记录相关数据

.gitignore

@@ -124,4 +124,4 @@ segv_*out
 /ArduSub/scripts/
 persistent.dat
 .history
-
+build_log.txt

ArduCopter/APM_Config.h

@@ -65,8 +65,8 @@
 //#define USERHOOK_VARIABLES "UserVariables.h"
 // Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below
 #define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
-//#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
-//#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
+#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
+#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
 #define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
 #define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
 #define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz

ArduCopter/Parameters.cpp

@@ -158,6 +158,8 @@ 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_ps,  "ZR_TK_PTPS", 20), // 起飞侧翻保护，延时，100hz计数
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT
     // @DisplayName: RTL Altitude
@@ -1716,6 +1718,9 @@ const char* Copter::get_sysid_board_id(void)
         case 10:
             snprintf(buf, sizeof(buf),  "Version: zr-v%u.%u.%u ,ID: ZRZK.19QT2.%d",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,(int)nameValue2);
             break;
+        case 11:
+            snprintf(buf, sizeof(buf),  "Version: zr-v%u.%u.%u ,ID: M6602007004",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch);
+            break;
 
     }
     

ArduCopter/Parameters.h

@@ -399,6 +399,8 @@ public:
         k_param_zr_8_circle,
         k_param_zr_bat_cycled,
         k_param_zr_land_lock_att,
+        k_param_zr_takeoff_prt_deg,
+        k_param_zr_takeoff_prt_ps,
 
         // the k_param_* space is 9-bits in size
         // 511: reserved
@@ -519,6 +521,8 @@ public:
     AP_Int8                  zr_8_circle;
     AP_Int16                 zr_bat_cycled;
     AP_Int8                  zr_land_lock_att;
+    AP_Int8                  zr_takeoff_prt_deg;
+    AP_Int8                  zr_takeoff_prt_ps;
     
     // Note: keep initializers here in the same order as they are declared
     // above.

ArduCopter/UserCode.cpp

@@ -32,6 +32,7 @@ void Copter::userhook_init()
 void Copter::userhook_FastLoop()
 {
     // put your 100Hz code here
+    takeoff_crash_detect();
 }
 #endif
 
@@ -278,4 +279,51 @@ void Copter::zr_mkdir_in_takeoff()
     }
 }
 
+void Copter::takeoff_crash_detect()
+{
+            
+    static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
+        // return immediately if disarmed, or crash checking disabled
+    if (!motors->armed() || g.fs_crash_check == 0) {
+        crash_counter = 0;
+        return;
+    }
+
+    if(g.zr_land_lock_att && rangefinder_alt_ok() && g.land_lock_alt \
+        && current_loc.alt < g.land_slow_2nd_alt \
+        && rangefinder_state.alt_cm_filt.get() < g.land_lock_alt){
+        
+        // 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 ;
+        }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 ;
+        }else{
+            crash_counter = 0;
+        }
+        // check if crashing for 2 seconds
+        AP::logger().Write("TKPT", "TimeUS,Ldeg,Aerr,Alt", "Qfff",
+                            AP_HAL::micros64(),
+                            lean_angle_deg,
+                            angle_error,
+                            rangefinder_state.alt_cm_filt.get());
+
+        if (crash_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
+            // disarm motors
+            arming.disarm();
+        }
+    }
+}
 #endif

ArduCopter/version.h

@@ -5,14 +5,14 @@
 #endif
 
 #include "ap_version.h"
-
-#define THISFIRMWARE "ZRUAV v4.0.16-rc8"  //"ArduCopter V4.0.0"
+#define GIT_VERSION "3"
+#define THISFIRMWARE "ZRUAV v4.1.16"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,0,16,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,1,16,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
-#define FW_MINOR 0
+#define FW_MINOR 1
 #define FW_PATCH 16
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
 /**

hexa-zrv4.sh

@@ -1,3 +1,3 @@
 ./waf configure --board zr-hexa
 ./waf copter
-cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.16-rc3.px4
+cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/编译/六轴_v4.1.16.px4

hrs100h.sh

@@ -1,3 +1,3 @@
 ./waf configure --board rs100h
 ./waf copter
-cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/rs100h-v4.0.16.px4
+cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.0.16.px4

rs100.sh

@@ -1,3 +1,3 @@
 ./waf configure --board rs100
 ./waf copter
-cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/rs100-v4.0.16.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100-v4.1.16.px4