Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-24 08:44:08 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: pre-arm check of EKF compass variance

Browse Source
This commit is contained in:
Randy Mackay 2015-08-26 13:57:54 +09:00
parent 873d31915a
commit e1a7760d38
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/motors.cpp
View File

@ -652,6 +652,18 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
return false; return false;
} }
// check EKF compass variance is below failsafe threshold
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
ahrs.get_NavEKF().getVariances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
if (mag_variance.length() >= g.fs_ekf_thresh) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: EKF compass variance"));
}
return false;
}
// check home and EKF origin are not too far // check home and EKF origin are not too far
if (far_from_EKF_origin(ahrs.get_home())) { if (far_from_EKF_origin(ahrs.get_home())) {
if (display_failure) { if (display_failure) {