From b64e40b5dabffecdff6ae189ff2ee49298e68154 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 22 Apr 2017 09:58:34 +1000 Subject: [PATCH] ekf2: format fixes --- src/modules/ekf2/ekf2_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 0ee36020d4..640a1db9d9 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -158,7 +158,8 @@ private: float _acc_hor_filt = 0.0f; // low-pass filtered horizontal acceleration // Used to check, save and use learned magnetometer biases - hrt_abstime _last_invalid_magcal_us = 0; // last time the conditions for a valid ekf magnetometer cal were not met (usec) + hrt_abstime _last_invalid_magcal_us = + 0; // last time the conditions for a valid ekf magnetometer cal were not met (usec) float _last_valid_mag_cal[3] = {}; // last valid XYZ magnetometer bias estimates (mGauss) bool _valid_cal_available[3] = {}; // true when an unsaved valid calibration for the XYZ magnetometer bias is available float _last_valid_variance[3] = {}; // variances for the last valid magnetometer XYZ bias estimates (mGauss**2) @@ -1018,12 +1019,13 @@ void Ekf2::task_main() || !mag_cal_active) { _last_invalid_magcal_us = now; - } else if (((now - _last_invalid_magcal_us) > 180*1000*1000ULL) + } else if (((now - _last_invalid_magcal_us) > 180 * 1000 * 1000ULL) && (_invalid_mag_id_count == 0)) { // we have sufficient continuous valid flight time to form a bias estimate // Don't record bias estimates to save later if variances are outside the valid range float max_var_allowed = 100.0f * _mag_bias_saved_variance.get(); float min_var_allowed = 0.01f * _mag_bias_saved_variance.get(); + for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) { if (status.covariances[axis_index + 19] > min_var_allowed && status.covariances[axis_index + 19] < max_var_allowed) {