EKF: update mag LPF immediately

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))) {

EKF/ekf.h

@@ -762,7 +762,6 @@ private:
 
 	// control fusion of magnetometer observations
 	void controlMagFusion();
-	void updateMagFilter();
 
 	bool noOtherYawAidingThanMag() const;
 

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