ekf2: move gps buffer pop to controlGpsFusion()

- controlGpsFusion() now owns yaw estimator update

src/modules/ekf2/EKF/control.cpp

@@ -102,41 +102,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 		}
 	}
 
-	if (_gps_buffer) {
-		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
-
-		// check for arrival of new sensor data at the fusion time horizon
-		_time_prev_gps_us = _gps_sample_delayed.time_us;
-		_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
-
-		if (_gps_data_ready) {
-			// correct velocity for offset relative to IMU
-			const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-			const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-			_gps_sample_delayed.vel -= vel_offset_earth;
-
-			// correct position and height for offset relative to IMU
-			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_gps_sample_delayed.pos -= pos_offset_earth.xy();
-			_gps_sample_delayed.hgt += pos_offset_earth(2);
-
-			// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
-			if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
-			    && _gps_sample_delayed.vel.isAllFinite()
-			   ) {
-				_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
-			}
-		}
-	}
-
-	// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
-	runYawEKFGSF(imu_delayed);
-
 	// control use of observations for aiding
 	controlMagFusion();
 	controlOpticalFlowFusion(imu_delayed);
-	controlGpsFusion();
+	controlGpsFusion(imu_delayed);
 	controlAirDataFusion(imu_delayed);
 	controlBetaFusion(imu_delayed);
 	controlDragFusion();

src/modules/ekf2/EKF/ekf.h

@@ -491,8 +491,6 @@ private:
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
-	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
-
 	uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
 	uint64_t _time_last_v_pos_aiding{0};
 	uint64_t _time_last_v_vel_aiding{0};
@@ -856,7 +854,7 @@ private:
 	void resetOnGroundMotionForOpticalFlowChecks();
 
 	// control fusion of GPS observations
-	void controlGpsFusion();
+	void controlGpsFusion(const imuSample &imu_delayed);
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;
 
@@ -1038,8 +1036,6 @@ private:
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
 
-	void runYawEKFGSF(const imuSample &imu_delayed);
-
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
 	// Resets the horizontal velocity and position to the default navigation sensor
 	// Returns true if the reset was successful

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1346,11 +1346,6 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
 	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
 }
 
-void Ekf::runYawEKFGSF(const imuSample &imu_delayed)
-{
-	_yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias());
-}
-
 void Ekf::resetGpsDriftCheckFilters()
 {
 	_gps_velNE_filt.setZero();

src/modules/ekf2/EKF/gps_control.cpp

@@ -39,13 +39,41 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
-void Ekf::controlGpsFusion()
+void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
-	if (!((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
+	if (!_gps_buffer || !((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
 		stopGpsFusion();
 		return;
 	}
 
+	_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
+
+	// check for arrival of new sensor data at the fusion time horizon
+	_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
+
+	if (_gps_data_ready) {
+		// correct velocity for offset relative to IMU
+		const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+		const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+		_gps_sample_delayed.vel -= vel_offset_earth;
+
+		// correct position and height for offset relative to IMU
+		const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+		_gps_sample_delayed.pos -= pos_offset_earth.xy();
+		_gps_sample_delayed.hgt += pos_offset_earth(2);
+
+		// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
+		if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
+		    && _gps_sample_delayed.vel.isAllFinite()
+		   ) {
+			_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
+		}
+	}
+
+	// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
+	_yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias());
+
 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {
 
@@ -236,8 +264,8 @@ bool Ekf::shouldResetGpsFusion() const
 	 * with no aiding we need to do something
 	 */
 	const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-						     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+			&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-						     && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
+			&& isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
 
 	const bool is_reset_required = has_horizontal_aiding_timed_out
 				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
@@ -280,7 +308,8 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 
 		const bool continuing_conditions_passing = !gps_checks_failing;
 
-		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
+		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push,
+				2 * GNSS_YAW_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _control_status.flags.tilt_align
@@ -336,7 +365,9 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 			}
 		}
 
-	} else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
+	} else if (_control_status.flags.gps_yaw
+		   && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
+
 		// No yaw data in the message anymore. Stop until it comes back.
 		stopGpsYawFusion();
 	}