Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -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
#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 // OPTICAL_FLOW
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) #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; 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 // check GPS