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

Copter: pre-arm consistency check of accels

Browse Source
This commit is contained in:
Randy Mackay 2014-09-03 13:50:41 +09:00
parent 3a5e960fe5
commit 59404c25e3
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/config.h
View File

@ -299,6 +299,11 @@
# define PREARM_MAX_VELOCITY_CMS 50.0f // vehicle must be travelling under 50cm/s before arming
#endif
// arming check's maximum acceptable accelerometer vector difference (in m/s/s) between primary and backup accelerometers
#ifndef PREARM_MAX_ACCEL_VECTOR_DIFF
#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
//////////////////////////////////////////////////////////////////////////////
// EKF Checker
#ifndef EKFCHECK_THRESHOLD_DEFAULT

18
ArduCopter/motors.pde
View File

@ -346,6 +346,24 @@ static void pre_arm_checks(bool display_failure)
return;
}
#if INS_MAX_INSTANCES > 1
// check all accelerometers point in roughly same direction
if (ins.get_accel_count() > 1) {
const Vector3f &prime_accel_vec = ins.get_accel();
for(uint8_t i=0; i<ins.get_accel_count(); i++) {
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
if (vec_diff.length() > PREARM_MAX_ACCEL_VECTOR_DIFF) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels inconsistent"));
}
return;
}
}
}
#endif
// check gyros are healthy
if(!ins.get_gyro_health_all()) {
if (display_failure) {