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: replace vehicle compass consistency check

Browse Source
This commit is contained in:
Randy Mackay 2015-09-15 15:58:43 +09:00
parent 8cfde42e15
commit 0f55b2a0eb
2 changed files with 4 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/config.h
View File

@ -313,11 +313,6 @@
#endif
#endif
// arming check's maximum acceptable vector difference between internal and external compass after vectors are normalized to field length of 1.0
#ifndef COMPASS_ACCEPTABLE_VECTOR_DIFF
#define COMPASS_ACCEPTABLE_VECTOR_DIFF 0.75f // pre arm compass check will fail if internal vs external compass direction differ by more than 45 degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#ifndef OPTFLOW

20
ArduCopter/motors.cpp
View File

@ -373,25 +373,13 @@ bool Copter::pre_arm_checks(bool display_failure)
return false;
}
#if COMPASS_MAX_INSTANCES > 1
// check all compasses point in roughly same direction
if (compass.get_count() > 1) {
Vector3f prime_mag_vec = compass.get_field_milligauss();
prime_mag_vec.normalize();
for(uint8_t i=0; i<compass.get_count(); i++) {
// get next compass
Vector3f mag_vec = compass.get_field_milligauss(i);
mag_vec.normalize();
Vector3f vec_diff = mag_vec - prime_mag_vec;
if (compass.use_for_yaw(i) && vec_diff.length() > COMPASS_ACCEPTABLE_VECTOR_DIFF) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses"));
}
return false;
}
if (!compass.consistent()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses"));
}
return false;
}
#endif
}