EKF: update mag LPF immediately

PX4 · Dec 8, 2020 · e7060fe · e7060fe
1 parent 4d767f9
commit e7060fe
Show file tree

Hide file tree

Showing 3 changed files with 2 additions and 9 deletions.
diff --git a/EKF/control.cpp b/EKF/control.cpp
@@ -97,6 +97,8 @@ void Ekf::controlFusionModes()
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
 
 	if (_mag_data_ready) {
+		_mag_lpf.update(_mag_sample_delayed.mag);
+
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
 		if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised || ISFINITE(_mag_declination_gps))) {

diff --git a/EKF/ekf.h b/EKF/ekf.h
@@ -754,7 +754,6 @@ class Ekf final : public EstimatorInterface
 
 	// control fusion of magnetometer observations
 	void controlMagFusion();
-	void updateMagFilter();
 
 	bool noOtherYawAidingThanMag() const;
 

diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp
@@ -58,7 +58,6 @@ void Ekf::controlMagFusion()
 		return;
 	}
 
-	updateMagFilter();
 	checkMagFieldStrength();
 
 	// If we are on ground, reset the flight alignment flag so that the mag fields will be
@@ -112,13 +111,6 @@ void Ekf::controlMagFusion()
 	}
 }
 
-void Ekf::updateMagFilter()
-{
-	if (_mag_data_ready) {
-		_mag_lpf.update(_mag_sample_delayed.mag);
-	}
-}
-
 bool Ekf::noOtherYawAidingThanMag() const
 {
 	// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used