ekf2: perform GNSS checks at delayed-time horizon

- never fuse a measurement that is not passing the checks
- cleanup and simplify GNSS vel/pos control logic

src/modules/ekf2/EKF/common.h

@@ -160,25 +160,6 @@ enum class MagCheckMask : uint8_t {
 	FORCE_WMM   = (1 << 2)
 };
 
-struct gpsMessage {
-	uint64_t    time_usec{};
-	int32_t     lat{};              ///< Latitude in 1E-7 degrees
-	int32_t     lon{};              ///< Longitude in 1E-7 degrees
-	int32_t     alt{};              ///< Altitude in 1E-3 meters (millimeters) above MSL
-	float       yaw{};              ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
-	float       yaw_offset{};       ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
-	float       yaw_accuracy{};	///< yaw measurement accuracy (rad, [0, 2PI])
-	uint8_t     fix_type{};         ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
-	float       eph{};              ///< GPS horizontal position accuracy in m
-	float       epv{};              ///< GPS vertical position accuracy in m
-	float       sacc{};             ///< GPS speed accuracy in m/s
-	float       vel_m_s{};          ///< GPS ground speed (m/sec)
-	Vector3f    vel_ned{};          ///< GPS ground speed NED
-	bool        vel_ned_valid{};    ///< GPS ground speed is valid
-	uint8_t     nsats{};            ///< number of satellites used
-	float       pdop{};             ///< position dilution of precision
-};
-
 struct imuSample {
 	uint64_t    time_us{};                ///< timestamp of the measurement (uSec)
 	Vector3f    delta_ang{};              ///< delta angle in body frame (integrated gyro measurements) (rad)
@@ -188,16 +169,21 @@ struct imuSample {
 	bool        delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping
 };
 
-struct gpsSample {
-	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
-	Vector2f    pos{};      ///< NE earth frame gps horizontal position measurement (m)
-	float       hgt{};      ///< gps height measurement (m)
-	Vector3f    vel{};      ///< NED earth frame gps velocity measurement (m/sec)
-	float       yaw{};      ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
-	float       hacc{};     ///< 1-std horizontal position error (m)
-	float       vacc{};     ///< 1-std vertical position error (m)
-	float       sacc{};     ///< 1-std speed error (m/sec)
-	float       yaw_acc{};  ///< 1-std yaw error (rad)
+struct gnssSample {
+	uint64_t    time_us{};    ///< timestamp of the measurement (uSec)
+	double      lat{};        ///< latitude (degrees)
+	double      lon{};        ///< longitude (degrees)
+	float       alt{};        ///< GNSS altitude above MSL (m)
+	Vector3f    vel{};        ///< NED earth frame GNSS velocity measurement (m/sec)
+	float       hacc{};       ///< 1-std horizontal position error (m)
+	float       vacc{};       ///< 1-std vertical position error (m)
+	float       sacc{};       ///< 1-std speed error (m/sec)
+	uint8_t     fix_type{};   ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
+	uint8_t     nsats{};      ///< number of satellites used
+	float       pdop{};       ///< position dilution of precision
+	float       yaw{};        ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
+	float       yaw_acc{};    ///< 1-std yaw error (rad)
+	float       yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
 };
 
 struct magSample {

src/modules/ekf2/EKF/ekf.h

@@ -431,8 +431,7 @@ public:
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_GNSS)
-	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
-	bool collect_gps(const gpsMessage &gps) override;
+	void collect_gps(const gnssSample &gps);
 
 	// set minimum continuous period without GPS fail required to mark a healthy GPS status
 	void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
@@ -636,7 +635,6 @@ private:
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_GNSS)
-	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 
 	// variables used for the GPS quality checks
@@ -989,29 +987,38 @@ private:
 	// control fusion of GPS observations
 	void controlGpsFusion(const imuSample &imu_delayed);
 	void stopGpsFusion();
-
+	void updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
+	void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
+	void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
+	bool tryYawEmergencyReset();
+	void resetVelocityToGnss(estimator_aid_source3d_s &aid_src);
+	void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src);
 	bool shouldResetGpsFusion() const;
 
-	// return true id the GPS quality is good enough to set an origin and start aiding
-	bool gps_is_good(const gpsMessage &gps);
+	/*
+	 * Return true if the GPS solution quality is adequate.
+	 * Checks are activated using the EKF2_GPS_CHECK bitmask parameter
+	 * Checks are adjusted using the EKF2_REQ_* parameters
+	*/
+	bool runGnssChecks(const gnssSample &gps);
 
-	void controlGnssHeightFusion(const gpsSample &gps_sample);
+	void controlGnssHeightFusion(const gnssSample &gps_sample);
 	void stopGpsHgtFusion();
 
 	void resetGpsDriftCheckFilters();
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
-	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
+	void controlGpsYawFusion(const gnssSample &gps_sample);
 	void stopGpsYawFusion();
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
-	void fuseGpsYaw();
+	void fuseGpsYaw(float antenna_yaw_offset);
 
 	// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
 	// return true if the reset was successful
-	bool resetYawToGps(const float gnss_yaw);
+	bool resetYawToGps(float gnss_yaw, float gnss_yaw_offset);
 
-	void updateGpsYaw(const gpsSample &gps_sample);
+	void updateGpsYaw(const gnssSample &gps_sample);
 
 # endif // CONFIG_EKF2_GNSS_YAW
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -147,7 +147,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS)
-void EstimatorInterface::setGpsData(const gpsMessage &gps)
+void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 {
 	if (!_initialised) {
 		return;
@@ -155,7 +155,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 
 	// Allocate the required buffer size if not previously done
 	if (_gps_buffer == nullptr) {
-		_gps_buffer = new RingBuffer<gpsSample>(_obs_buffer_length);
+		_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);
 
 		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
 			delete _gps_buffer;
@@ -165,61 +165,24 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 		}
 	}
 
-	const int64_t time_us = gps.time_usec
+	const int64_t time_us = gnss_sample.time_us
 				- static_cast<int64_t>(_params.gps_delay_ms * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
 
-		if (!gps.vel_ned_valid || (gps.fix_type == 0)) {
-			return;
-		}
+		gnssSample gnss_sample_new(gnss_sample);
 
-		gpsSample gps_sample_new;
+		gnss_sample_new.time_us = time_us;
 
-		gps_sample_new.time_us = time_us;
-
-		gps_sample_new.vel = gps.vel_ned;
-
-		gps_sample_new.sacc = gps.sacc;
-		gps_sample_new.hacc = gps.eph;
-		gps_sample_new.vacc = gps.epv;
-
-		gps_sample_new.hgt = (float)gps.alt * 1e-3f;
-
-#if defined(CONFIG_EKF2_GNSS_YAW)
-
-		if (PX4_ISFINITE(gps.yaw)) {
-			_time_last_gps_yaw_buffer_push = _time_latest_us;
-			gps_sample_new.yaw = gps.yaw;
-			gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
-
-		} else {
-			gps_sample_new.yaw = NAN;
-			gps_sample_new.yaw_acc = 0.f;
-		}
-
-		if (PX4_ISFINITE(gps.yaw_offset)) {
-			_gps_yaw_offset = gps.yaw_offset;
-
-		} else {
-			_gps_yaw_offset = 0.0f;
-		}
-
-#endif // CONFIG_EKF2_GNSS_YAW
-
-		// Only calculate the relative position if the WGS-84 location of the origin is set
-		if (collect_gps(gps)) {
-			gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));
-
-		} else {
-			gps_sample_new.pos(0) = 0.0f;
-			gps_sample_new.pos(1) = 0.0f;
-		}
-
-		_gps_buffer->push(gps_sample_new);
+		_gps_buffer->push(gnss_sample_new);
 		_time_last_gps_buffer_push = _time_latest_us;
 
+#if defined(CONFIG_EKF2_GNSS_YAW)
+		if (PX4_ISFINITE(gnss_sample.yaw)) {
+			_time_last_gps_yaw_buffer_push = _time_latest_us;
+		}
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	} else {
 		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);

src/modules/ekf2/EKF/estimator_interface.h

@@ -84,15 +84,14 @@ public:
 	void setIMUData(const imuSample &imu_sample);
 
 #if defined(CONFIG_EKF2_GNSS)
-	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
-	virtual bool collect_gps(const gpsMessage &gps) = 0;
-	void setGpsData(const gpsMessage &gps);
+	void setGpsData(const gnssSample &gnss_sample);
 
-	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
+	const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
 
 	float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
 	float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
 	float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
+
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -390,10 +389,10 @@ protected:
 	float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
 
 #if defined(CONFIG_EKF2_GNSS)
-	RingBuffer<gpsSample> *_gps_buffer{nullptr};
+	RingBuffer<gnssSample> *_gps_buffer{nullptr};
 	uint64_t _time_last_gps_buffer_push{0};
 
-	gpsSample _gps_sample_delayed{};
+	gnssSample _gps_sample_delayed{};
 
 	float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
 	float _gps_vertical_position_drift_rate_m_s{NAN};   // Vertical position drift rate (m/s)
@@ -403,7 +402,6 @@ protected:
 	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
-	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
 	// innovation consistency check monitoring ratios
 	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 	uint64_t _time_last_gps_yaw_buffer_push{0};

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -38,7 +38,7 @@
 
 #include "ekf.h"
 
-void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
+void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "GNSS";
 
@@ -60,7 +60,11 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 			}
 		}
 
-		const float measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
+		const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+		const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+		const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2);
+
+		const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
 		const float measurement_var = sq(noise);
 
 		const float innov_gate = math::max(_params.gps_pos_innov_gate, 1.f);
@@ -74,12 +78,9 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 						   innov_gate,
 						   aid_src);
 
-		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
-		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
-
 		// update the bias estimator before updating the main filter but after
 		// using its current state to compute the vertical position innovation
-		if (measurement_valid && gps_checks_passing && !gps_checks_failing) {
+		if (measurement_valid) {
 			bias_est.setMaxStateNoise(sqrtf(measurement_var));
 			bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
 			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
@@ -92,10 +93,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 				&& _gps_checks_passed;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)
-				&& _gps_checks_passed
-				&& gps_checks_passing
-				&& !gps_checks_failing;
+				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
 
 		if (_control_status.flags.gps_hgt) {
 			if (continuing_conditions_passing) {

src/modules/ekf2/EKF/gps_checks.cpp

@@ -58,38 +58,35 @@
 #define MASK_GPS_HSPD   (1<<7)
 #define MASK_GPS_VSPD   (1<<8)
 
-bool Ekf::collect_gps(const gpsMessage &gps)
+void Ekf::collect_gps(const gnssSample &gps)
 {
-	// Run GPS checks always
-	_gps_checks_passed = gps_is_good(gps);
-
 	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-		const double lat = gps.lat * 1.0e-7;
-		const double lon = gps.lon * 1.0e-7;
+		const double lat = gps.lat;
+		const double lon = gps.lon;
 
 		if (!_pos_ref.isInitialized()) {
-			_pos_ref.initReference(lat, lon, gps.time_usec);
+			_pos_ref.initReference(lat, lon, gps.time_us);
 
 			// if we are already doing aiding, correct for the change in position since the EKF started navigating
 			if (isHorizontalAidingActive()) {
 				double est_lat;
 				double est_lon;
 				_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
-				_pos_ref.initReference(est_lat, est_lon, gps.time_usec);
+				_pos_ref.initReference(est_lat, est_lon, gps.time_us);
 			}
 		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
 		if (!PX4_ISFINITE(_gps_alt_ref)) {
-			_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
+			_gps_alt_ref = gps.alt + _state.pos(2);
 		}
 
 		_NED_origin_initialised = true;
 
 		// save the horizontal and vertical position uncertainty of the origin
-		_gpos_origin_eph = gps.eph;
-		_gpos_origin_epv = gps.epv;
+		_gpos_origin_eph = gps.hacc;
+		_gpos_origin_epv = gps.vacc;
 
 		_information_events.flags.gps_checks_passed = true;
 		ECL_INFO("GPS checks passed");
@@ -97,15 +94,15 @@ bool Ekf::collect_gps(const gpsMessage &gps)
 
 	if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
 		// a rough 2D fix is sufficient to lookup declination
-		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.eph < 1000);
+		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
 
 		if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
 
 			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-			const double lat = gps.lat * 1.0e-7;
+			const double lat = gps.lat;
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-			const double lon = gps.lon * 1.0e-7;
+			const double lon = gps.lon;
 
 			// set the magnetic field data returned by the geo library using the current GPS position
 			const float mag_declination_gps = get_mag_declination_radians(lat, lon);
@@ -138,19 +135,9 @@ bool Ekf::collect_gps(const gpsMessage &gps)
 
 		_wmm_gps_time_last_checked = _time_delayed_us;
 	}
-
-	// start collecting GPS if there is a 3D fix and the NED origin has been set
-	return _NED_origin_initialised && (gps.fix_type >= 3);
 }
 
-/*
- * Return true if the GPS solution quality is adequate to set an origin for the EKF
- * and start GPS aiding.
- * All activated checks must pass for 10 seconds.
- * Checks are activated using the EKF2_GPS_CHECK bitmask parameter
- * Checks are adjusted using the EKF2_REQ_* parameters
-*/
-bool Ekf::gps_is_good(const gpsMessage &gps)
+bool Ekf::runGnssChecks(const gnssSample &gps)
 {
 	// Check the fix type
 	_gps_check_fail_status.flags.fix = (gps.fix_type < 3);
@@ -162,24 +149,24 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 	_gps_check_fail_status.flags.pdop = (gps.pdop > _params.req_pdop);
 
 	// Check the reported horizontal and vertical position accuracy
-	_gps_check_fail_status.flags.hacc = (gps.eph > _params.req_hacc);
-	_gps_check_fail_status.flags.vacc = (gps.epv > _params.req_vacc);
+	_gps_check_fail_status.flags.hacc = (gps.hacc > _params.req_hacc);
+	_gps_check_fail_status.flags.vacc = (gps.vacc > _params.req_vacc);
 
 	// Check the reported speed accuracy
 	_gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc);
 
 	// check if GPS quality is degraded
-	_gps_error_norm = fmaxf((gps.eph / _params.req_hacc), (gps.epv / _params.req_vacc));
+	_gps_error_norm = fmaxf((gps.hacc / _params.req_hacc), (gps.vacc / _params.req_vacc));
 	_gps_error_norm = fmaxf(_gps_error_norm, (gps.sacc / _params.req_sacc));
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
 	constexpr float filt_time_const = 10.0f;
-	const float dt = math::constrain(float(int64_t(gps.time_usec) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
+	const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
 	const float filter_coef = dt / filt_time_const;
 
 	// The following checks are only valid when the vehicle is at rest
-	const double lat = gps.lat * 1.0e-7;
-	const double lon = gps.lon * 1.0e-7;
+	const double lat = gps.lat;
+	const double lon = gps.lon;
 
 	if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 		// Calculate position movement since last measurement
@@ -192,13 +179,13 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 
 		} else {
 			// no previous position has been set
-			_gps_pos_prev.initReference(lat, lon, gps.time_usec);
-			_gps_alt_prev = 1e-3f * (float)gps.alt;
+			_gps_pos_prev.initReference(lat, lon, gps.time_us);
+			_gps_alt_prev = gps.alt;
 		}
 
 		// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
 		const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
-		Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - 1e-3f * (float)gps.alt));
+		Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
 		pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit);
 
 		// Apply a low pass filter
@@ -213,7 +200,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 		_gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift);
 
 		// Check the magnitude of the filtered horizontal GPS velocity
-		const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
+		const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel.xy()),
 					   -10.0f * _params.req_hdrift,
 					   10.0f * _params.req_hdrift);
 		_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
@@ -235,12 +222,12 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 	}
 
 	// save GPS fix for next time
-	_gps_pos_prev.initReference(lat, lon, gps.time_usec);
-	_gps_alt_prev = 1e-3f * (float)gps.alt;
+	_gps_pos_prev.initReference(lat, lon, gps.time_us);
+	_gps_alt_prev = gps.alt;
 
 	// Check  the filtered difference between GPS and EKF vertical velocity
 	const float vz_diff_limit = 10.0f * _params.req_vdrift;
-	const float vertVel = math::constrain(gps.vel_ned(2) - _state.vel(2), -vz_diff_limit, vz_diff_limit);
+	const float vertVel = math::constrain(gps.vel(2) - _state.vel(2), -vz_diff_limit, vz_diff_limit);
 	_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
 	_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);
 
@@ -263,13 +250,12 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 		(_gps_check_fail_status.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD))
 	) {
 		_last_gps_fail_us = _time_delayed_us;
+		return false;
 
 	} else {
 		_last_gps_pass_us = _time_delayed_us;
+		return true;
 	}
-
-	// continuous period without fail of x seconds required to return a healthy status
-	return isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us);
 }
 
 void Ekf::resetGpsDriftCheckFilters()

src/modules/ekf2/EKF/gps_control.cpp

@@ -46,31 +46,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 		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));
-		}
-	}
-
 	if (!gyro_bias_inhibited()) {
 		_yawEstimator.setGyroBias(getGyroBias());
 	}
@@ -78,155 +53,93 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	// run EKF-GSF yaw estimator once per imu_delayed update
 	_yawEstimator.update(imu_delayed, _control_status.flags.in_air && !_control_status.flags.vehicle_at_rest);
 
-	// Check for new GPS data that has fallen behind the fusion time horizon
+	_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) {
+		const gnssSample &gnss_sample = _gps_sample_delayed;
 
-		const gpsSample &gps_sample{_gps_sample_delayed};
+		if (runGnssChecks(gnss_sample) && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us / 2)) {
+			if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
+				// First time checks are passing, latching.
+				_gps_checks_passed = true;
+			}
 
-		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
-		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
+			collect_gps(gnss_sample);
 
+		} else {
+			// Skip this sample
+			_gps_data_ready = false;
+
+			if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
+				stopGpsFusion();
+				_warning_events.flags.gps_quality_poor = true;
+				ECL_WARN("GPS quality poor - stopping use");
+			}
+		}
+
+		updateGnssVel(gnss_sample, _aid_src_gnss_vel);
+		updateGnssPos(gnss_sample, _aid_src_gnss_pos);
+
+	} else if (_control_status.flags.gps) {
+		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
+			stopGpsFusion();
+			_warning_events.flags.gps_data_stopped = true;
+			ECL_WARN("GPS data stopped");
+		}
+	}
+
+	if (_gps_data_ready) {
 #if defined(CONFIG_EKF2_GNSS_YAW)
-		controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
+		const gnssSample &gnss_sample = _gps_sample_delayed;
+		controlGpsYawFusion(gnss_sample);
 #endif // CONFIG_EKF2_GNSS_YAW
 
-		// GNSS velocity
-		const Vector3f velocity{gps_sample.vel};
-		const float vel_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
-		const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
-		updateVelocityAidSrcStatus(gps_sample.time_us,
-					   velocity,                                                   // observation
-					   vel_obs_var,                                                // observation variance
-					   math::max(_params.gps_vel_innov_gate, 1.f),                 // innovation gate
-					   _aid_src_gnss_vel);
+		controlGnssYawEstimator(_aid_src_gnss_vel);
+
 		const bool gnss_vel_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL));
-
-		// GNSS position
-		const Vector2f position{gps_sample.pos};
-		// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
-		float pos_noise = math::max(gps_sample.hacc, _params.gps_pos_noise);
-
-		if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
-			// if we are not using another source of aiding, then we are reliant on the GPS
-			// observations to constrain attitude errors and must limit the observation noise value.
-			if (pos_noise > _params.pos_noaid_noise) {
-				pos_noise = _params.pos_noaid_noise;
-			}
-		}
-
-		const float pos_var = sq(pos_noise);
-		const Vector2f pos_obs_var(pos_var, pos_var);
-		updateHorizontalPositionAidSrcStatus(gps_sample.time_us,
-						     position,                                   // observation
-						     pos_obs_var,                                // observation variance
-						     math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
-						     _aid_src_gnss_pos);
 		const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
 
-		// Determine if we should use GPS aiding for velocity and horizontal position
-		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
-		bool mandatory_conditions_passing = false;
-
-		if ((gnss_pos_enabled || gnss_vel_enabled)
-		    && _control_status.flags.tilt_align
-		    && _NED_origin_initialised
-		   ) {
-			// if GPS is otherwise ready to go other than yaw align
-			if (!_control_status.flags.yaw_align && gps_checks_passing && !gps_checks_failing) {
-
-				if (resetYawToEKFGSF()) {
-					ECL_INFO("GPS yaw aligned using IMU");
-				}
-			}
-
-			if (_control_status.flags.yaw_align) {
-				mandatory_conditions_passing = true;
-			}
-		}
-
-		const bool continuing_conditions_passing = mandatory_conditions_passing && !gps_checks_failing;
-		const bool starting_conditions_passing = continuing_conditions_passing && gps_checks_passing;
+		const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled)
+				&& _control_status.flags.tilt_align
+				&& _control_status.flags.yaw_align
+				&& _NED_origin_initialised;
+		const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
 
 		if (_control_status.flags.gps) {
-			if (mandatory_conditions_passing) {
-				if (continuing_conditions_passing
-				    || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
+			if (continuing_conditions_passing) {
+				if (gnss_vel_enabled) {
+					fuseVelocity(_aid_src_gnss_vel);
+				}
+
+				if (gnss_pos_enabled) {
+					fuseHorizontalPosition(_aid_src_gnss_pos);
+				}
+
+				bool do_vel_pos_reset = shouldResetGpsFusion();
+
+				if (isYawFailure()
+				    && _control_status.flags.in_air
+				    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+				    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
+					do_vel_pos_reset = tryYawEmergencyReset();
+				}
+
+				if (do_vel_pos_reset) {
+					ECL_WARN("GPS fusion timeout, resetting velocity / position");
 
 					if (gnss_vel_enabled) {
-						fuseVelocity(_aid_src_gnss_vel);
+						resetVelocityToGnss(_aid_src_gnss_vel);
 					}
 
 					if (gnss_pos_enabled) {
-						fuseHorizontalPosition(_aid_src_gnss_pos);
+						resetHorizontalPositionToGnss(_aid_src_gnss_pos);
 					}
-
-					bool do_vel_pos_reset = shouldResetGpsFusion();
-
-					if (isYawFailure()
-					    && _control_status.flags.in_air
-					    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
-					    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
-						/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
-						 * fails while the difference between the yaw emergency estimator and the yas estimate is large.
-						 * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
-						 * present before flight to prevent triggering due to GPS glitches or other sensor errors.
-						 */
-						if (resetYawToEKFGSF()) {
-							ECL_WARN("GPS emergency yaw reset");
-
-							if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-								// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
-								// and cause another navigation failure
-								_control_status.flags.mag_fault = true;
-								_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
-							}
-
-#if defined(CONFIG_EKF2_GNSS_YAW)
-
-							if (_control_status.flags.gps_yaw) {
-								_control_status.flags.gps_yaw_fault = true;
-								_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
-							}
-
-#endif // CONFIG_EKF2_GNSS_YAW
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-
-							if (_control_status.flags.ev_yaw) {
-								_control_status.flags.ev_yaw_fault = true;
-							}
-
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-							do_vel_pos_reset = true;
-						}
-					}
-
-					if (do_vel_pos_reset) {
-						ECL_WARN("GPS fusion timeout, resetting velocity and position");
-
-						if (gnss_vel_enabled) {
-							// reset velocity
-							_information_events.flags.reset_vel_to_gps = true;
-							resetVelocityTo(velocity, vel_obs_var);
-							_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
-						}
-
-						if (gnss_pos_enabled) {
-							// reset position
-							_information_events.flags.reset_pos_to_gps = true;
-							resetHorizontalPositionTo(position, pos_obs_var);
-							_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
-						}
-					}
-
-				} else {
-					stopGpsFusion();
-					_warning_events.flags.gps_quality_poor = true;
-					ECL_WARN("GPS quality poor - stopping use");
 				}
 
-			} else { // mandatory conditions are not passing
+			} else {
 				stopGpsFusion();
 			}
 
@@ -242,40 +155,146 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				   ) {
 					// reset velocity
 					if (gnss_vel_enabled) {
-						_information_events.flags.reset_vel_to_gps = true;
-						resetVelocityTo(velocity, vel_obs_var);
-						_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
+						resetVelocityToGnss(_aid_src_gnss_vel);
 					}
 				}
 
 				if (gnss_pos_enabled) {
-					// reset position
-					_information_events.flags.reset_pos_to_gps = true;
-					resetHorizontalPositionTo(position, pos_obs_var);
-					_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
-					_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
+					resetHorizontalPositionToGnss(_aid_src_gnss_pos);
 				}
 
 				_control_status.flags.gps = true;
 			}
 		}
-
-	} else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)10e6)) {
-		stopGpsFusion();
-		_warning_events.flags.gps_data_stopped = true;
-		ECL_WARN("GPS data stopped");
-
-	}  else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)1e6)
-		    && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
-
-		// Handle the case where we are fusing another position source along GPS,
-		// stop waiting for GPS after 1 s of lost signal
-		stopGpsFusion();
-		_warning_events.flags.gps_data_stopped_using_alternate = true;
-		ECL_WARN("GPS data stopped, using only EV, OF or air data");
 	}
 }
 
+void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
+{
+	// 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;
+	const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
+
+	const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise));
+	const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
+	updateVelocityAidSrcStatus(gnss_sample.time_us,
+				   velocity,                                                   // observation
+				   vel_obs_var,                                                // observation variance
+				   math::max(_params.gps_vel_innov_gate, 1.f),                 // innovation gate
+				   aid_src);
+}
+
+void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src)
+{
+	// correct position and height for offset relative to IMU
+	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy();
+
+	// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
+	float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
+
+	if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
+		// if we are not using another source of aiding, then we are reliant on the GPS
+		// observations to constrain attitude errors and must limit the observation noise value.
+		if (pos_noise > _params.pos_noaid_noise) {
+			pos_noise = _params.pos_noaid_noise;
+		}
+	}
+
+	const float pos_var = sq(pos_noise);
+	const Vector2f pos_obs_var(pos_var, pos_var);
+	updateHorizontalPositionAidSrcStatus(gnss_sample.time_us,
+					     position,                                   // observation
+					     pos_obs_var,                                // observation variance
+					     math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
+					     aid_src);
+}
+
+void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
+{
+	// update yaw estimator velocity (basic sanity check on GNSS velocity data)
+	const float vel_var = aid_src_vel.observation_variance[0];
+	const Vector2f vel_xy(aid_src_vel.observation);
+
+	if ((vel_var > 0.f)
+	    && (vel_var < _params.req_sacc)
+	    && vel_xy.isAllFinite()) {
+
+		_yawEstimator.setVelocity(vel_xy, vel_var);
+
+		// Try to align yaw using estimate if available
+		if (((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
+		     || (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
+		    && !_control_status.flags.yaw_align
+		    && _control_status.flags.tilt_align) {
+			if (resetYawToEKFGSF()) {
+				ECL_INFO("GPS yaw aligned using IMU");
+			}
+		}
+	}
+}
+
+bool Ekf::tryYawEmergencyReset()
+{
+	bool success = false;
+
+	/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
+	 * fails while the difference between the yaw emergency estimator and the yas estimate is large.
+	 * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
+	 * present before flight to prevent triggering due to GPS glitches or other sensor errors.
+	 */
+	if (resetYawToEKFGSF()) {
+		ECL_WARN("GPS emergency yaw reset");
+
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+			// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
+			// and cause another navigation failure
+			_control_status.flags.mag_fault = true;
+			_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
+		}
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
+
+		if (_control_status.flags.gps_yaw) {
+			_control_status.flags.gps_yaw_fault = true;
+			_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
+		}
+
+#endif // CONFIG_EKF2_GNSS_YAW
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+
+		if (_control_status.flags.ev_yaw) {
+			_control_status.flags.ev_yaw_fault = true;
+		}
+
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+		success = true;
+	}
+
+	return success;
+}
+
+void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
+{
+	_information_events.flags.reset_vel_to_gps = true;
+	resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
+void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
+{
+	_information_events.flags.reset_pos_to_gps = true;
+	resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
+	_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
 bool Ekf::shouldResetGpsFusion() const
 {
 	/* We are relying on aiding to constrain drift so after a specified time
@@ -308,7 +327,7 @@ bool Ekf::shouldResetGpsFusion() const
 }
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
-void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
+void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 {
 	if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
 	    || _control_status.flags.gps_yaw_fault) {
@@ -323,14 +342,13 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 
 	if (is_new_data_available) {
 
-		const bool continuing_conditions_passing = !gps_checks_failing;
+		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
 
 		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
-				&& gps_checks_passing
+				&& _gps_checks_passed
 				&& !is_gps_yaw_data_intermittent
 				&& !_gps_intermittent;
 
@@ -338,14 +356,14 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 
 			if (continuing_conditions_passing) {
 
-				fuseGpsYaw();
+				fuseGpsYaw(gps_sample.yaw_offset);
 
 				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
 					if (_nb_gps_yaw_reset_available > 0) {
 						// Data seems good, attempt a reset
-						resetYawToGps(gps_sample.yaw);
+						resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset);
 
 						if (_control_status.flags.in_air) {
 							_nb_gps_yaw_reset_available--;
@@ -374,7 +392,7 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 		} else {
 			if (starting_conditions_passing) {
 				// Try to activate GPS yaw fusion
-				if (resetYawToGps(gps_sample.yaw)) {
+				if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
 					ECL_INFO("starting GPS yaw fusion");
 
 					_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
@@ -420,6 +438,8 @@ void Ekf::stopGpsFusion()
 		ECL_INFO("stopping GPS position and velocity fusion");
 		resetEstimatorAidStatus(_aid_src_gnss_pos);
 		resetEstimatorAidStatus(_aid_src_gnss_vel);
+		_last_gps_fail_us = 0;
+		_last_gps_pass_us = 0;
 
 		_control_status.flags.gps = false;
 	}

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -46,7 +46,7 @@
 #include <mathlib/mathlib.h>
 #include <cstdlib>
 
-void Ekf::updateGpsYaw(const gpsSample &gps_sample)
+void Ekf::updateGpsYaw(const gnssSample &gps_sample)
 {
 	if (PX4_ISFINITE(gps_sample.yaw)) {
 
@@ -56,16 +56,17 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
 		// initially populate for estimator_aid_src_gnss_yaw logging
 
 		// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
-		const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset);
+		const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset);
 
-		const float R_YAW = sq(fmaxf(gps_sample.yaw_acc, _params.gps_heading_noise));
+		const float yaw_acc = PX4_ISFINITE(gps_sample.yaw_acc) ? gps_sample.yaw_acc : 0.f;
+		const float R_YAW = sq(fmaxf(yaw_acc, _params.gps_heading_noise));
 
 		float heading_pred;
 		float heading_innov_var;
 
 		{
 		VectorState H;
-		sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
+		sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
 		}
 
 		gnss_yaw.observation = measured_hdg;
@@ -80,7 +81,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
 	}
 }
 
-void Ekf::fuseGpsYaw()
+void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 {
 	auto &gnss_yaw = _aid_src_gnss_yaw;
 
@@ -89,6 +90,10 @@ void Ekf::fuseGpsYaw()
 		return;
 	}
 
+	if (!PX4_ISFINITE(antenna_yaw_offset)) {
+		antenna_yaw_offset = 0.f;
+	}
+
 	VectorState H;
 
 	{
@@ -97,7 +102,7 @@ void Ekf::fuseGpsYaw()
 
 	// Note: we recompute innov and innov_var because it doesn't cost much more than just computing H
 	// making a separate function just for H uses more flash space without reducing CPU load significantly
-	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
 	}
 
 	// check if the innovation variance calculation is badly conditioned
@@ -139,10 +144,10 @@ void Ekf::fuseGpsYaw()
 	}
 }
 
-bool Ekf::resetYawToGps(const float gnss_yaw)
+bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset)
 {
 	// define the predicted antenna array vector and rotate into earth frame
-	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+	const Vector3f ant_vec_bf = {cosf(gnss_yaw_offset), sinf(gnss_yaw_offset), 0.0f};
 	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
 
 	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading

src/modules/ekf2/EKF2.cpp

@@ -2435,32 +2435,38 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			perf_count(_msg_missed_gps_perf);
 		}
 
-		gpsMessage gps_msg{
-			.time_usec = vehicle_gps_position.timestamp,
-			.lat = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7)),
-			.lon = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7)),
-			.alt = static_cast<int32_t>(round(vehicle_gps_position.altitude_msl_m * 1e3)),
-			.yaw = vehicle_gps_position.heading,
-			.yaw_offset = vehicle_gps_position.heading_offset,
-			.yaw_accuracy = vehicle_gps_position.heading_accuracy,
-			.fix_type = vehicle_gps_position.fix_type,
-			.eph = vehicle_gps_position.eph,
-			.epv = vehicle_gps_position.epv,
+		Vector3f vel_ned;
+
+		if (vehicle_gps_position.vel_ned_valid) {
+			vel_ned = Vector3f(vehicle_gps_position.vel_n_m_s,
+					   vehicle_gps_position.vel_e_m_s,
+					   vehicle_gps_position.vel_d_m_s);
+
+		} else {
+			return; //TODO: change and set to NAN
+		}
+
+		gnssSample gnss_sample{
+			.time_us = vehicle_gps_position.timestamp,
+			.lat = vehicle_gps_position.latitude_deg,
+			.lon = vehicle_gps_position.longitude_deg,
+			.alt = static_cast<float>(vehicle_gps_position.altitude_msl_m),
+			.vel = vel_ned,
+			.hacc = vehicle_gps_position.eph,
+			.vacc = vehicle_gps_position.epv,
 			.sacc = vehicle_gps_position.s_variance_m_s,
-			.vel_m_s = vehicle_gps_position.vel_m_s,
-			.vel_ned = Vector3f{
-				vehicle_gps_position.vel_n_m_s,
-				vehicle_gps_position.vel_e_m_s,
-				vehicle_gps_position.vel_d_m_s
-			},
-			.vel_ned_valid = vehicle_gps_position.vel_ned_valid,
+			.fix_type = vehicle_gps_position.fix_type,
 			.nsats = vehicle_gps_position.satellites_used,
 			.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
 				      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
+			.yaw = vehicle_gps_position.heading, //TODO: move to different message
+			.yaw_acc = vehicle_gps_position.heading_accuracy,
+			.yaw_offset = vehicle_gps_position.heading_offset,
 		};
-		_ekf.setGpsData(gps_msg);
 
-		_gps_time_usec = gps_msg.time_usec;
+		_ekf.setGpsData(gnss_sample);
+
+		_gps_time_usec = gnss_sample.time_us;
 		_gps_alttitude_ellipsoid = static_cast<int32_t>(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3));
 	}
 }

src/modules/ekf2/test/sensor_simulator/gps.cpp

@@ -15,9 +15,9 @@ Gps::~Gps()
 
 void Gps::send(const uint64_t time)
 {
-	const float dt = static_cast<float>(time - _gps_data.time_usec) * 1e-6f;
+	const float dt = static_cast<float>(time - _gps_data.time_us) * 1e-6f;
 
-	_gps_data.time_usec = time;
+	_gps_data.time_us = time;
 
 	if (fabsf(_gps_pos_rate(0)) > FLT_EPSILON || fabsf(_gps_pos_rate(1)) > FLT_EPSILON) {
 		stepHorizontalPositionByMeters(Vector2f(_gps_pos_rate) * dt);
@@ -30,29 +30,29 @@ void Gps::send(const uint64_t time)
 	_ekf->setGpsData(_gps_data);
 }
 
-void Gps::setData(const gpsMessage &gps)
+void Gps::setData(const gnssSample &gps)
 {
 	_gps_data = gps;
 }
 
-void Gps::setAltitude(const int32_t alt)
+void Gps::setAltitude(const float alt)
 {
 	_gps_data.alt = alt;
 }
 
-void Gps::setLatitude(const int32_t lat)
+void Gps::setLatitude(const double lat)
 {
 	_gps_data.lat = lat;
 }
 
-void Gps::setLongitude(const int32_t lon)
+void Gps::setLongitude(const double lon)
 {
 	_gps_data.lon = lon;
 }
 
 void Gps::setVelocity(const Vector3f &vel)
 {
-	_gps_data.vel_ned = vel;
+	_gps_data.vel = vel;
 }
 
 void Gps::setYaw(const float yaw)
@@ -87,7 +87,7 @@ void Gps::setPositionRateNED(const Vector3f &rate)
 
 void Gps::stepHeightByMeters(const float hgt_change)
 {
-	_gps_data.alt += hgt_change * 1e3f;
+	_gps_data.alt += hgt_change;
 }
 
 void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
@@ -98,32 +98,30 @@ void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
 	double lat_new {0.0};
 	double lon_new {0.0};
 
-	_ekf->global_origin().project(_gps_data.lat * 1e-7, _gps_data.lon * 1e-7, hposN_curr, hposE_curr);
+	_ekf->global_origin().project(_gps_data.lat, _gps_data.lon, hposN_curr, hposE_curr);
 
 	Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
 
 	_ekf->global_origin().reproject(hpos_new(0), hpos_new(1), lat_new, lon_new);
 
-	_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
-	_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
+	_gps_data.lon = lon_new;
+	_gps_data.lat = lat_new;
 }
 
-gpsMessage Gps::getDefaultGpsData()
+gnssSample Gps::getDefaultGpsData()
 {
-	gpsMessage gps_data{};
-	gps_data.time_usec = 0;
-	gps_data.lat = 473566094;
-	gps_data.lon = 85190237;
-	gps_data.alt = 422056;
+	gnssSample gps_data{};
+	gps_data.time_us = 0;
+	gps_data.lat = 47.3566094;
+	gps_data.lon = 8.5190237;
+	gps_data.alt = 422.056f;
 	gps_data.yaw = NAN;
 	gps_data.yaw_offset = 0.0f;
 	gps_data.fix_type = 3;
-	gps_data.eph = 0.5f;
-	gps_data.epv = 0.8f;
+	gps_data.hacc = 0.5f;
+	gps_data.vacc = 0.8f;
 	gps_data.sacc = 0.2f;
-	gps_data.vel_m_s = 0.0;
-	gps_data.vel_ned.setZero();
-	gps_data.vel_ned_valid = 1;
+	gps_data.vel.setZero();
 	gps_data.nsats = 16;
 	gps_data.pdop = 0.0f;
 

src/modules/ekf2/test/sensor_simulator/gps.h

@@ -51,13 +51,13 @@ public:
 	Gps(std::shared_ptr<Ekf> ekf);
 	~Gps();
 
-	void setData(const gpsMessage &gps);
+	void setData(const gnssSample &gps);
 	void stepHeightByMeters(const float hgt_change);
 	void stepHorizontalPositionByMeters(const Vector2f hpos_change);
 	void setPositionRateNED(const Vector3f &rate);
-	void setAltitude(const int32_t alt);
-	void setLatitude(const int32_t lat);
-	void setLongitude(const int32_t lon);
+	void setAltitude(const float alt);
+	void setLatitude(const double lat);
+	void setLongitude(const double lon);
 	void setVelocity(const Vector3f &vel);
 	void setYaw(const float yaw);
 	void setYawOffset(const float yaw);
@@ -65,12 +65,12 @@ public:
 	void setNumberOfSatellites(const int num_satellites);
 	void setPdop(const float pdop);
 
-	gpsMessage getDefaultGpsData();
+	gnssSample getDefaultGpsData();
 
 private:
 	void send(uint64_t time) override;
 
-	gpsMessage _gps_data{};
+	gnssSample _gps_data{};
 	Vector3f _gps_pos_rate{};
 };
 

src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp

@@ -395,20 +395,17 @@ void SensorSimulator::setSensorDataFromTrajectory()
 
 void SensorSimulator::setGpsLatitude(const double latitude)
 {
-	int32_t lat = static_cast<int32_t>(latitude * 1e7);
-	_gps.setLatitude(lat);
+	_gps.setLatitude(latitude);
 }
 
 void SensorSimulator::setGpsLongitude(const double longitude)
 {
-	int32_t lon = static_cast<int32_t>(longitude * 1e7);
-	_gps.setLongitude(lon);
+	_gps.setLongitude(longitude);
 }
 
 void SensorSimulator::setGpsAltitude(const float altitude)
 {
-	int32_t alt = static_cast<int32_t>(altitude * 1e3f);
-	_gps.setAltitude(alt);
+	_gps.setAltitude(altitude);
 }
 
 void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)

src/modules/ekf2/test/test_EKF_fusionLogic.cpp

@@ -195,15 +195,15 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow)
 
 	// WHEN: GPS data stops
 	_sensor_simulator.stopGps();
-	_sensor_simulator.runSeconds(2);
+	_sensor_simulator.runSeconds(10);
 
-	// THEN: immediately switch to flow only
+	// THEN: GNSS fusion stops after a timing out
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
 
-	// BUT WHEN: GPS starts again
+	// BUT WHEN: GPS starts after passing the checks again
 	_sensor_simulator.startGps();
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(6);
 
 	// THEN: use it again
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -84,28 +84,16 @@ TEST_F(EkfGpsTest, gpsTimeout)
 	// WHEN: the number of satellites drops below the minimum
 	_sensor_simulator._gps.setNumberOfSatellites(3);
 
-	// THEN: the GNSS fusion continues because it is the only source of position/velocity
-	_sensor_simulator.runSeconds(20);
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
-
-	// BUT: if we have another velocity aiding source
-	const float max_flow_rate = 5.f;
-	const float min_ground_distance = 0.f;
-	const float max_ground_distance = 50.f;
-	_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
-
-	_sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest());
-	_ekf_wrapper.enableFlowFusion();
-	_sensor_simulator.startFlow();
-
-	_sensor_simulator._rng.setData(0.2, 100);
-	_sensor_simulator._rng.setLimits(0.1f, 9.f);
-	_sensor_simulator.startRangeFinder();
-	_sensor_simulator.runSeconds(5);
-
-	// THEN: the GNSS fusion stops
-	EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
+	// THEN: the GNSS fusion stops after some time
+	_sensor_simulator.runSeconds(8);
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+
+	// BUT WHEN: the number of satellites is good again
+	_sensor_simulator._gps.setNumberOfSatellites(16);
+
+	// THEN: the GNSS fusion restarts
+	_sensor_simulator.runSeconds(6);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
 }
 
 TEST_F(EkfGpsTest, gpsFixLoss)
@@ -148,6 +136,7 @@ TEST_F(EkfGpsTest, resetToGpsVelocity)
 
 	_ekf->set_in_air_status(true);
 	_ekf->set_vehicle_at_rest(false);
+	_sensor_simulator.runSeconds(5.2); // required to pass the checks
 	_sensor_simulator.runMicroseconds(dt_us);
 
 	// THEN: a reset to GPS velocity should be done
@@ -178,7 +167,7 @@ TEST_F(EkfGpsTest, resetToGpsPosition)
 	const Vector3f simulated_position_change(2.0f, -1.0f, 0.f);
 	_sensor_simulator._gps.stepHorizontalPositionByMeters(
 		Vector2f(simulated_position_change));
-	_sensor_simulator.runMicroseconds(1e5);
+	_sensor_simulator.runSeconds(6);
 
 	// THEN: a reset to the new GPS position should be done
 	const Vector3f estimated_position = _ekf->getPosition();

src/modules/ekf2/test/test_EKF_terrain_estimator.cpp

@@ -81,8 +81,7 @@ public:
 		_ekf->set_vehicle_at_rest(true);
 
 		_ekf_wrapper.enableGpsFusion();
-		_sensor_simulator.runSeconds(2); // Run to pass the GPS checks
-		_sensor_simulator.runSeconds(3.5); // And a bit more to start the GPS fusion TODO: this shouldn't be necessary
+		_sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks
 		EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
 
 		const Vector3f simulated_velocity(0.5f, -1.0f, 0.f);