Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: pre-arm consistency check of gyros

Browse Source
This commit is contained in:
Randy Mackay 2014-09-03 22:00:09 +09:00
parent 28f31166b9
commit 641c8317a5
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/config.h
View File

@ -304,6 +304,11 @@
#define PREARM_MAX_ACCEL_VECTOR_DIFF 1.0f // pre arm accel check will fail if primary and backup accelerometer vectors differ by 1m/s/s #define PREARM_MAX_ACCEL_VECTOR_DIFF 1.0f // pre arm accel check will fail if primary and backup accelerometer vectors differ by 1m/s/s
#endif #endif
// arming check's maximum acceptable rotation rate difference (in rad/sec) between primary and backup gyros
#ifndef PREARM_MAX_GYRO_VECTOR_DIFF
#define PREARM_MAX_GYRO_VECTOR_DIFF 0.35f // pre arm gyro check will fail if primary and backup gyro vectors differ by 0.35 rad/sec (=20deg/sec)
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// EKF Checker // EKF Checker
#ifndef EKFCHECK_THRESHOLD_DEFAULT #ifndef EKFCHECK_THRESHOLD_DEFAULT

16
ArduCopter/motors.pde
View File

@ -374,6 +374,22 @@ static void pre_arm_checks(bool display_failure)
} }
return; return;
} }
#if INS_MAX_INSTANCES > 1
// check all gyros are consistent
if (ins.get_gyro_count() > 1) {
for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
// get rotation rate difference between gyro #i and primary gyro
Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro();
if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros inconsistent"));
}
return;
}
}
}
#endif
} }
#if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1