Browse Source

Copter: Access EKF variance checks through AHRS object

Supports flight using EKF2
mission-4.1.18
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
commit
7c6b31b585
  1. 2
      ArduCopter/ekf_check.cpp
  2. 2
      ArduCopter/motors.cpp

2
ArduCopter/ekf_check.cpp

@ -107,7 +107,7 @@ bool Copter::ekf_over_threshold() @@ -107,7 +107,7 @@ bool Copter::ekf_over_threshold()
Vector2f offset;
float compass_variance;
float vel_variance;
ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
compass_variance = magVar.length();
// return true if compass and velocity variance over the threshold

2
ArduCopter/motors.cpp

@ -644,7 +644,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) @@ -644,7 +644,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
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);
ahrs.get_variances(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(MAV_SEVERITY_CRITICAL,PSTR("PreArm: EKF compass variance"));

Loading…
Cancel
Save