fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 01:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: return saved mag bias variance when not in 3d magnetometer fusion

Browse Source 
- the estimated mag bias was requiring > 30 seconds of continuous 3d
mag fusion to be reported stable (and saved back to mag cal), this
restores the original intent requiring 30 seconds of accumulated valid
3d mag fusion
This commit is contained in:
Daniel Agar
2022-03-14 21:03:41 -04:00
parent 5717434e93
commit 7ef38112d2
2 changed files with 29 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+12 -3
View File
@@ -246,7 +246,15 @@ public:
Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; }
Vector3f getMagBiasVariance() const
{
if (_control_status.flags.mag_3D) {
return Vector3f{P(19, 19), P(20, 20), P(21, 21)};
}
return _saved_mag_bf_variance;
}
bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
@@ -518,8 +526,9 @@ private:
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
Matrix2f _saved_mag_ef_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
float _saved_mag_ef_d_variance{}; ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
gps_check_fail_status_u _gps_check_fail_status{};