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

ArduCopter/crash_check.cpp

@@ -1,9 +1,12 @@
 #include "Copter.h"
 
 // Code to detect a crash main ArduCopter code
-#define CRASH_CHECK_TRIGGER_SEC         2       // 2 seconds inverted indicates a crash
+#define CRASH_CHECK_TRIGGER_SEC         5       // 2 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_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
@@ -49,6 +52,13 @@ void Copter::crash_check()
         return;
     }
 
+    // check for lean angle over 15 degrees  角度判断，角度小于稳定飞行值退出
+    const float lean_angle_deg = degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()));
+    if (lean_angle_deg <= CRASH_CHECK_ANGLE_MIN_DEG) {
+        crash_counter = 0;
+        return;
+    }
+
     // check for angle error over 30 degrees
     const float angle_error = attitude_control->get_att_error_angle_deg();
     if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
@@ -56,6 +66,14 @@ void Copter::crash_check()
         return;
     }
 
+    // check for speed under 10m/s (if available) 增加速度判断
+    Vector3f vel_ned;
+    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++;
 
@@ -67,7 +85,11 @@ void Copter::crash_check()
         // send message to gcs
         gcs().send_text(MAV_SEVERITY_EMERGENCY,"坠机:锁定");//Crash: Disarming
         // disarm motors
-        copter.arming.disarm();
+        if(g.zr_use_rc != 2){
+            copter.arming.disarm();
+        }else{
+            crash_counter = 0;
+        }
     }
 }
 

ArduCopter/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.16-RC2"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.16-RC3"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,16,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-rc2.px4
+cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.16-rc3.px4

版本说明.txt

@@ -1,3 +1,41 @@
+zr-flow_v4.2
+
+	测流合并到zr-v4-base v4.0.15-RC6
+
+* v4.0.16
+
+  * RC3
+
+    * 坠机保护优化，增加速度和角度限制
+    * `g.zr_use_rc != 2`用于测试，只提示，不执行`disarm()`
+  * RC2
+
+    * 避障增加avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障
+    * 降落速度限制
+    * 电池损耗算法
+  * `AVDv6: avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障`
+  * RC1 
+
+    * 增加避障
+
+* v4.0.15
+
+  * RC7
+
+    * 合并测流
+  * RC6 
+
+    * 增加到期时间全部参数zr_reg_date
+    * 增加M66hwdef，sysid类型改成<6启用参数配置
+  * RC5
+
+    * 新增参数设置是否自适应
+    * 固件版本前缀修改
+    * 固件名类型判断调整，根据hwdef选择，如果没有则用sysid_type
+
+
+	
+
 zr-v4.0:   经过飞行测试，稳定版本
 zr-v4.0-dev:   新添加的功能，并入主分支前先验证