Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: pre-arm check of internal vs ext compass

Browse Source
This commit is contained in:
Randy Mackay 2014-08-26 22:31:43 +09:00
parent 2b0a7c59bc
commit eb51a8e5da
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/config.h
View File

@ -333,6 +333,11 @@
#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.75 // pre arm compass check will fail if internal vs external compass direction differ by more than 45 degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)

21
ArduCopter/motors.pde
View File

@ -290,6 +290,27 @@ static void pre_arm_checks(bool display_failure)
}
return;
}
#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();
prime_mag_vec.normalize();
for(uint8_t i=0; i<compass.get_count(); i++) {
// get next compass
Vector3f mag_vec = compass.get_field(i);
mag_vec.normalize();
Vector3f vec_diff = mag_vec - prime_mag_vec;
if (vec_diff.length() > COMPASS_ACCEPTABLE_VECTOR_DIFF) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: compasses inconsistent"));
}
return;
}
}
}
#endif
}
// check GPS