坠机保护增加角度和飞行速度判断,角度过大时取消速度判断

ArduCopter/crash_check.cpp

@@ -1,11 +1,12 @@
 #include "Copter.h"
 
 // Code to detect a crash main ArduCopter code
-#define CRASH_CHECK_TRIGGER_SEC         5       // 2 seconds inverted indicates a crash
+#define CRASH_CHECK_TRIGGER_SEC         3       // 3 seconds inverted indicates a crash
 #define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f   // 30 degrees beyond angle max is signal we are inverted
 #define CRASH_CHECK_ACCEL_MAX           3.0f    // vehicle must be accelerating less than 3m/s/s to be considered crashed
 #define CRASH_CHECK_ANGLE_MIN_DEG       15.0f   // vehicle must be leaning at least 15deg to trigger crash check
-#define CRASH_CHECK_SPEED_MAX           2.0f   // vehicle must be moving at less than 2m/s to trigger crash check
+#define CRASH_CHECK_ANGLE_MAX_DEG       90.0f   // 超过这个角度，不做速度限制判断
+#define CRASH_CHECK_SPEED_MAX           1.0f   // vehicle must be moving at less than 2m/s to trigger crash check
 
 // Code to detect a thrust loss main ArduCopter code
 #define THRUST_LOSS_CHECK_TRIGGER_SEC         1     // 1 second descent while level and high throttle indicates thrust loss
@@ -67,9 +68,11 @@ void Copter::crash_check()
 
     // check for speed under 10m/s (if available) 增加速度判断
     Vector3f vel_ned;
-    if (ahrs.get_velocity_NED(vel_ned) && (vel_ned.length() >= CRASH_CHECK_SPEED_MAX)) {
-        crash_counter = 0;
-        return;
+    if (ahrs.get_velocity_NED(vel_ned)) {
+        if(vel_ned.length() >= CRASH_CHECK_SPEED_MAX && lean_angle_deg <= CRASH_CHECK_ANGLE_MAX_DEG){
+            crash_counter = 0;
+            return;
+        }
     }
     // we may be crashing
     crash_counter++;
@@ -82,9 +85,7 @@ void Copter::crash_check()
         // send message to gcs
         gcs().send_text(MAV_SEVERITY_EMERGENCY,"坠机:锁定");//Crash: Disarming
         // disarm motors
-        if(g.zr_use_rc != 2){
-            copter.arming.disarm();
-        }
+        copter.arming.disarm();
     }
 }
 

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-RC2.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100-v4.0.16-RC3.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-RC2.px4
+cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100h-v4.0.16-RC3.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-RC2.px4
+cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100-v4.0.16-RC3.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-RC2.px4
+cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100h-v4.0.16-RC3.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-RC2.px4
+cp ./build/zr-hexa/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/六轴m66-v4.0.16-RC3.px4

hexa-zrv4.sh

@@ -1,7 +1,3 @@
 ./waf configure --board zr-hexa
 ./waf copter
-<<<<<<< HEAD
-cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/hexa-flow-v4.px4
-=======
-cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.16-rc2.px4
->>>>>>> zr-dev-4.0.16-RC3
+cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.17-rc1.px4

release_note.txt

@@ -5,7 +5,7 @@ zr-flow_v4.2
 * v4.0.16
 
   * RC3
-
+	* 坠机锁定增加侧翻时取消速度判断
     * 坠机保护优化，增加速度和角度限制
     * `g.zr_use_rc != 2`用于测试，只提示，不执行`disarm()`
   * RC2