diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index f2e03327cc..80689b6cb2 100644 --- a/ArduCopter/crash_check.cpp +++ b/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; + } } } diff --git a/ArduCopter/version.h b/ArduCopter/version.h index a9aa7d0aba..889d5d16b9 100644 --- a/ArduCopter/version.h +++ b/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 diff --git a/hexa-zrv4.sh b/hexa-zrv4.sh index 76d0ab11ce..b935e1f9e4 100755 --- a/hexa-zrv4.sh +++ b/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 diff --git a/版本说明.txt b/版本说明.txt index 181c7eb1ca..ff11f4d58f 100644 --- a/版本说明.txt +++ b/版本说明.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: 新添加的功能,并入主分支前先验证