ekf2: new kconfig to enable/disable GNSS (enabled by default)

boards/bitcraze/crazyflie/default.px4board

@@ -15,7 +15,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_GNSS is not set
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

boards/bitcraze/crazyflie21/default.px4board

@@ -14,7 +14,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_GNSS is not set
 # CONFIG_EKF2_MAGNETOMETER is not set
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y

src/modules/ekf2/CMakeLists.txt

@@ -124,9 +124,6 @@ list(APPEND EKF_SRCS
 	EKF/estimator_interface.cpp
 	EKF/fake_height_control.cpp
 	EKF/fake_pos_control.cpp
-	EKF/gnss_height_control.cpp
-	EKF/gps_checks.cpp
-	EKF/gps_control.cpp
 	EKF/gravity_fusion.cpp
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
@@ -166,6 +163,14 @@ if(CONFIG_EKF2_EXTERNAL_VISION)
 	)
 endif()
 
+if(CONFIG_EKF2_GNSS)
+	list(APPEND EKF_SRCS
+		EKF/gnss_height_control.cpp
+		EKF/gps_checks.cpp
+		EKF/gps_control.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -42,9 +42,6 @@ list(APPEND EKF_SRCS
 	estimator_interface.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
-	gnss_height_control.cpp
-	gps_checks.cpp
-	gps_control.cpp
 	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
@@ -84,6 +81,14 @@ if(CONFIG_EKF2_EXTERNAL_VISION)
 	)
 endif()
 
+if(CONFIG_EKF2_GNSS)
+	list(APPEND EKF_SRCS
+		gnss_height_control.cpp
+		gps_checks.cpp
+		gps_control.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()

src/modules/ekf2/EKF/common.h

@@ -280,15 +280,9 @@ struct parameters {
 	// measurement source control
 	int32_t height_sensor_ref{HeightSensor::BARO};
 	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
-	int32_t baro_ctrl{1};
-	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
 
 	int32_t sensor_interval_max_ms{10};     ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
 
-	// measurement time delays
-	float baro_delay_ms{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
-	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
-
 	// input noise
 	float gyro_noise{1.5e-2f};              ///< IMU angular rate noise used for covariance prediction (rad/sec)
 	float accel_noise{3.5e-1f};             ///< IMU acceleration noise use for covariance prediction (m/sec**2)
@@ -308,18 +302,66 @@ struct parameters {
 	float switch_on_accel_bias{0.2f};       ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
 	float initial_tilt_err{0.1f};           ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
 
+#if defined(CONFIG_EKF2_BAROMETER)
+	int32_t baro_ctrl{1};
+	float baro_delay_ms{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
+	float baro_noise{2.0f};                 ///< observation noise for barometric height fusion (m)
+	float baro_bias_nsd{0.13f};             ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
+	float baro_innov_gate{5.0f};            ///< barometric and GPS height innovation consistency gate size (STD)
+
+	float gnd_effect_deadzone{5.0f};        ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
+	float gnd_effect_max_hgt{0.5f};         ///< Height above ground at which baro ground effect becomes insignificant (m)
+
+# if defined(CONFIG_EKF2_BARO_COMPENSATION)
+	// static barometer pressure position error coefficient along body axes
+	float static_pressure_coef_xp{0.0f};    // (-)
+	float static_pressure_coef_xn{0.0f};    // (-)
+	float static_pressure_coef_yp{0.0f};    // (-)
+	float static_pressure_coef_yn{0.0f};    // (-)
+	float static_pressure_coef_z{0.0f};     // (-)
+
+	// upper limit on airspeed used for correction  (m/s**2)
+	float max_correction_airspeed{20.0f};
+# endif // CONFIG_EKF2_BARO_COMPENSATION
+#endif // CONFIG_EKF2_BAROMETER
+
+#if defined(CONFIG_EKF2_GNSS)
+	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
+	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
+
+	Vector3f gps_pos_body{};                ///< xyz position of the GPS antenna in body frame (m)
+
 	// position and velocity fusion
 	float gps_vel_noise{0.5f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
 	float gps_pos_noise{0.5f};              ///< minimum allowed observation noise for gps position fusion (m)
 	float gps_hgt_bias_nsd{0.13f};          ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
-	float pos_noaid_noise{10.0f};           ///< observation noise for non-aiding position fusion (m)
-	float baro_noise{2.0f};                 ///< observation noise for barometric height fusion (m)
-	float baro_bias_nsd{0.13f};             ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
-	float baro_innov_gate{5.0f};            ///< barometric and GPS height innovation consistency gate size (STD)
 	float gps_pos_innov_gate{5.0f};         ///< GPS horizontal position innovation consistency gate size (STD)
 	float gps_vel_innov_gate{5.0f};         ///< GPS velocity innovation consistency gate size (STD)
-	float gnd_effect_deadzone{5.0f};        ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
+
-	float gnd_effect_max_hgt{0.5f};         ///< Height above ground at which baro ground effect becomes insignificant (m)
+	// these parameters control the strictness of GPS quality checks used to determine if the GPS is
+	// good enough to set a local origin and commence aiding
+	int32_t gps_check_mask{21};             ///< bitmask used to control which GPS quality checks are used
+	float req_hacc{5.0f};                   ///< maximum acceptable horizontal position error (m)
+	float req_vacc{8.0f};                   ///< maximum acceptable vertical position error (m)
+	float req_sacc{1.0f};                   ///< maximum acceptable speed error (m/s)
+	int32_t req_nsats{6};                   ///< minimum acceptable satellite count
+	float req_pdop{2.0f};                   ///< maximum acceptable position dilution of precision
+	float req_hdrift{0.3f};                 ///< maximum acceptable horizontal drift speed (m/s)
+	float req_vdrift{0.5f};                 ///< maximum acceptable vertical drift speed (m/s)
+
+# if defined(CONFIG_EKF2_GNSS_YAW)
+	// GNSS heading fusion
+	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
+# endif // CONFIG_EKF2_GNSS_YAW
+
+	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
+	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
+	const unsigned EKFGSF_reset_delay{1000000};     ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
+	const float EKFGSF_yaw_err_max{0.262f};         ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
+
+#endif // CONFIG_EKF2_GNSS
+
+	float pos_noaid_noise{10.0f};           ///< observation noise for non-aiding position fusion (m)
 
 	float heading_innov_gate{2.6f};         ///< heading fusion innovation consistency gate size (STD)
 	float mag_heading_noise{3.0e-1f};       ///< measurement noise used for simple heading fusion (rad)
@@ -346,11 +388,6 @@ struct parameters {
 	float mag_check_inclination_tolerance_deg{20.f};
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-#if defined(CONFIG_EKF2_GNSS_YAW)
-	// GNSS heading fusion
-	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
-#endif // CONFIG_EKF2_GNSS_YAW
-
 #if defined(CONFIG_EKF2_AIRSPEED)
 	// airspeed fusion
 	float airspeed_delay_ms{100.0f};        ///< airspeed measurement delay relative to the IMU (mSec)
@@ -434,20 +471,8 @@ struct parameters {
 	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-	// these parameters control the strictness of GPS quality checks used to determine if the GPS is
-	// good enough to set a local origin and commence aiding
-	int32_t gps_check_mask{21};             ///< bitmask used to control which GPS quality checks are used
-	float req_hacc{5.0f};                   ///< maximum acceptable horizontal position error (m)
-	float req_vacc{8.0f};                   ///< maximum acceptable vertical position error (m)
-	float req_sacc{1.0f};                   ///< maximum acceptable speed error (m/s)
-	int32_t req_nsats{6};                   ///< minimum acceptable satellite count
-	float req_pdop{2.0f};                   ///< maximum acceptable position dilution of precision
-	float req_hdrift{0.3f};                 ///< maximum acceptable horizontal drift speed (m/s)
-	float req_vdrift{0.5f};                 ///< maximum acceptable vertical drift speed (m/s)
-
 	// XYZ offset of sensors in body axes (m)
 	Vector3f imu_pos_body{};                ///< xyz position of IMU in body frame (m)
-	Vector3f gps_pos_body{};                ///< xyz position of the GPS antenna in body frame (m)
 
 	// accel bias learning control
 	float acc_bias_lim{0.4f};               ///< maximum accel bias magnitude (m/sec**2)
@@ -463,18 +488,6 @@ struct parameters {
 
 	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
-	// static barometer pressure position error coefficient along body axes
-	float static_pressure_coef_xp{0.0f};    // (-)
-	float static_pressure_coef_xn{0.0f};    // (-)
-	float static_pressure_coef_yp{0.0f};    // (-)
-	float static_pressure_coef_yn{0.0f};    // (-)
-	float static_pressure_coef_z{0.0f};     // (-)
-
-	// upper limit on airspeed used for correction  (m/s**2)
-	float max_correction_airspeed {20.0f};
-#endif // CONFIG_EKF2_BARO_COMPENSATION
-
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	// multi-rotor drag specific force fusion
 	int32_t drag_ctrl{0};
@@ -496,10 +509,6 @@ struct parameters {
 	const float auxvel_gate{5.0f};          ///< velocity fusion innovation consistency gate size (STD)
 #endif // CONFIG_EKF2_AUXVEL
 
-	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
-	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
-	const unsigned EKFGSF_reset_delay{1000000};     ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
-	const float EKFGSF_yaw_err_max{0.262f};         ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
 };
 
 union fault_status_u {

src/modules/ekf2/EKF/control.cpp

@@ -92,7 +92,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 			}
 
 			ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
-					 (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
+				 (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
 
 			ECL_DEBUG("tilt aligned, roll: %.3f, pitch %.3f, yaw: %.3f",
 				  (double)matrix::Eulerf(_state.quat_nominal).phi(),
@@ -111,7 +111,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlOpticalFlowFusion(imu_delayed);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_GNSS)
 	controlGpsFusion(imu_delayed);
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	controlAirDataFusion(imu_delayed);

src/modules/ekf2/EKF/covariance.cpp

@@ -57,16 +57,24 @@ void Ekf::initialiseCovariance()
 	resetQuatCov();
 
 	// velocity
+#if defined(CONFIG_EKF2_GNSS)
 	const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
+#else
+	const float vel_var = sq(0.5f);
+#endif
 	P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
 
 	// position
-	const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
 	float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
+#if defined(CONFIG_EKF2_GNSS)
+	const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
 
 	if (_control_status.flags.gps_hgt) {
 		z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
 	}
+#else
+	const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f));
+#endif
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
@@ -404,8 +412,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
 		bool bad_acc_bias = false;
 		if (fabsf(down_dvel_bias) > dVel_bias_lim) {
-
+#if defined(CONFIG_EKF2_GNSS)
 			bool bad_vz_gps = _control_status.flags.gps    && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
+#else
+			bool bad_vz_gps = false;
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 			bool bad_vz_ev  = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
 #else
@@ -418,7 +429,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 #else
 				bool bad_z_baro = false;
 #endif
+#if defined(CONFIG_EKF2_GNSS)
 				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
+#else
+				bool bad_z_gps  = false;
+#endif
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _aid_src_rng_hgt.innovation  < 0.0f);

src/modules/ekf2/EKF/ekf.cpp

@@ -92,7 +92,11 @@ void Ekf::reset()
 	_prev_gyro_bias_var.zero();
 	_prev_accel_bias_var.zero();
 
+#if defined(CONFIG_EKF2_GNSS)
 	resetGpsDriftCheckFilters();
+	_gps_checks_passed = false;
+#endif // CONFIG_EKF2_GNSS
+	_gps_alt_ref = NAN;
 
 	_output_predictor.reset();
 
@@ -112,9 +116,6 @@ void Ekf::reset()
 
 	_time_acc_bias_check = 0;
 
-	_gps_checks_passed = false;
-	_gps_alt_ref = NAN;
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	_baro_counter = 0;
 #endif // CONFIG_EKF2_BAROMETER
@@ -147,13 +148,15 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_ev_yaw);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
+#if defined(CONFIG_EKF2_GNSS)
 	resetEstimatorAidStatus(_aid_src_gnss_hgt);
 	resetEstimatorAidStatus(_aid_src_gnss_pos);
 	resetEstimatorAidStatus(_aid_src_gnss_vel);
 
-#if defined(CONFIG_EKF2_GNSS_YAW)
+# if defined(CONFIG_EKF2_GNSS_YAW)
 	resetEstimatorAidStatus(_aid_src_gnss_yaw);
-#endif // CONFIG_EKF2_GNSS_YAW
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	resetEstimatorAidStatus(_aid_src_mag_heading);

src/modules/ekf2/EKF/ekf.h

@@ -80,10 +80,6 @@ public:
 
 	static uint8_t getNumberOfStates() { return State::size; }
 
-	void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
-	void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
-	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
@@ -343,19 +339,11 @@ public:
 	Vector3f getVelocityVariance() const { return getStateVariance<State::vel>(); };
 	Vector3f getPositionVariance() const { return getStateVariance<State::pos>(); }
 
-
-	// 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;
-
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
 	bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
 
-	float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
-	bool setEkfGlobalOriginAltitude(const float altitude);
-
-
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
 
@@ -373,14 +361,6 @@ public:
 	void resetGyroBias();
 	void resetAccelBias();
 
-	// First argument returns GPS drift  metrics in the following array locations
-	// 0 : Horizontal position drift rate (m/s)
-	// 1 : Vertical position drift rate (m/s)
-	// 2 : Filtered horizontal velocity (m/s)
-	// Second argument returns true when IMU movement is blocking the drift calculation
-	// Function returns true if the metrics have been updated and not returned previously by this function
-	bool get_gps_drift_metrics(float drift[3], bool *blocked);
-
 	// return true if the global position estimate is valid
 	// return true if the origin is set we are not doing unconstrained free inertial navigation
 	// and have not started using synthetic position observations to constrain drift
@@ -504,26 +484,11 @@ public:
 	Vector3f calcRotVecVariances() const;
 	float getYawVar() const;
 
-	// 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; }
-
-	const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; }
-	const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; }
-
-	bool gps_checks_passed() const { return _gps_checks_passed; };
-
-	// get solution data from the EKF-GSF emergency yaw estimator
-	// returns false when data is not available
-	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
-			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
-
 	// Returns true if the output of the yaw emergency estimator can be used for a reset
 	bool isYawEmergencyEstimateAvailable() const;
 
 	uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
 
-	const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
 
@@ -548,13 +513,37 @@ public:
 	const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
+#if defined(CONFIG_EKF2_GNSS)
+	void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
+	void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
+	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
+
+	// 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;
+
+	// 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; }
+
+	const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; }
+	const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; }
+
+	bool gps_checks_passed() const { return _gps_checks_passed; };
+
+	// get solution data from the EKF-GSF emergency yaw estimator
+	// returns false when data is not available
+	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
+			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
+
+	const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
+
 	const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
 	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
 
-#if defined(CONFIG_EKF2_GNSS_YAW)
+# if defined(CONFIG_EKF2_GNSS_YAW)
 	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
-#endif // CONFIG_EKF2_GNSS_YAW
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
@@ -606,9 +595,6 @@ private:
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
 
-	// 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
-
 	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};
@@ -710,20 +696,9 @@ private:
 	uint8_t _nb_ev_yaw_reset_available{0};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	estimator_aid_source1d_s _aid_src_gnss_hgt{};
+#if defined(CONFIG_EKF2_GNSS)
-	estimator_aid_source2d_s _aid_src_gnss_pos{};
+	// booleans true when fresh sensor data is available at the fusion time horizon
-	estimator_aid_source3d_s _aid_src_gnss_vel{};
+	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
-
-#if defined(CONFIG_EKF2_GNSS_YAW)
-	estimator_aid_source1d_s _aid_src_gnss_yaw{};
-	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
-#endif // CONFIG_EKF2_GNSS_YAW
-
-	estimator_aid_source3d_s _aid_src_gravity{};
-
-#if defined(CONFIG_EKF2_AUXVEL)
-	estimator_aid_source2d_s _aid_src_aux_vel{};
-#endif // CONFIG_EKF2_AUXVEL
 
 	// variables used for the GPS quality checks
 	Vector3f _gps_pos_deriv_filt{};	///< GPS NED position derivative (m/sec)
@@ -736,8 +711,27 @@ private:
 	uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
 	bool _gps_checks_passed{false};		///> true when all active GPS checks have passed
 
-	// Variables used to publish the WGS-84 location of the EKF local NED origin
+	gps_check_fail_status_u _gps_check_fail_status{};
-	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
+	// height sensor status
+	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
+
+	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
+
+	estimator_aid_source1d_s _aid_src_gnss_hgt{};
+	estimator_aid_source2d_s _aid_src_gnss_pos{};
+	estimator_aid_source3d_s _aid_src_gnss_vel{};
+
+# if defined(CONFIG_EKF2_GNSS_YAW)
+	estimator_aid_source1d_s _aid_src_gnss_yaw{};
+	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
+
+	estimator_aid_source3d_s _aid_src_gravity{};
+
+#if defined(CONFIG_EKF2_AUXVEL)
+	estimator_aid_source2d_s _aid_src_aux_vel{};
+#endif // CONFIG_EKF2_AUXVEL
 
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
@@ -785,8 +779,6 @@ private:
 	Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-	gps_check_fail_status_u _gps_check_fail_status{};
-
 	// variables used to inhibit accel bias learning
 	bool _accel_bias_inhibit[3] {};		///< true when the accel bias learning is being inhibited for the specified axis
 	bool _gyro_bias_inhibit[3] {};		///< true when the gyro bias learning is being inhibited for the specified axis
@@ -797,9 +789,6 @@ private:
 	Vector3f _prev_gyro_bias_var{};         ///< saved gyro XYZ bias variances
 	Vector3f _prev_accel_bias_var{};        ///< saved accel XYZ bias variances
 
-	// height sensor status
-	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
-
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
 	uint64_t _time_good_vert_accel{0};	///< last time a good vertical accel was detected (uSec)
@@ -818,7 +807,8 @@ private:
 	void predictCovariance(const imuSample &imu_delayed);
 
 	template <const IdxDof &S>
-	void resetStateCovariance(const matrix::SquareMatrix<float, S.dof> &cov) {
+	void resetStateCovariance(const matrix::SquareMatrix<float, S.dof> &cov)
+	{
 		P.uncorrelateCovarianceSetVariance<S.dof>(S.idx, 0.0f);
 		P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
 	}
@@ -828,21 +818,6 @@ private:
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
 
-#if defined(CONFIG_EKF2_GNSS_YAW)
-	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
-
-	// fuse the yaw angle obtained from a dual antenna GPS unit
-	void fuseGpsYaw();
-
-	// 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);
-
-	void updateGpsYaw(const gpsSample &gps_sample);
-
-#endif // CONFIG_EKF2_GNSS_YAW
-	void stopGpsYawFusion();
-
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// ekf sequential fusion of magnetometer measurements
 	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
@@ -1079,9 +1054,6 @@ private:
 	// calculate the earth rotation vector from a given latitude
 	Vector3f calcEarthRateNED(float lat_rad) const;
 
-	// return true id the GPS quality is good enough to set an origin and start aiding
-	bool gps_is_good(const gpsMessage &gps);
-
 	// Control the filter fusion modes
 	void controlFusionModes(const imuSample &imu_delayed);
 
@@ -1102,11 +1074,43 @@ private:
 	void stopEvYawFusion();
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
+#if defined(CONFIG_EKF2_GNSS)
 	// control fusion of GPS observations
 	void controlGpsFusion(const imuSample &imu_delayed);
+	void stopGpsFusion();
+
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;
 
+	// return true id the GPS quality is good enough to set an origin and start aiding
+	bool gps_is_good(const gpsMessage &gps);
+
+	void controlGnssHeightFusion(const gpsSample &gps_sample);
+	void stopGpsHgtFusion();
+
+	// 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
+	bool resetYawToEKFGSF();
+
+	void resetGpsDriftCheckFilters();
+
+# if defined(CONFIG_EKF2_GNSS_YAW)
+	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
+	void stopGpsYawFusion();
+
+	// fuse the yaw angle obtained from a dual antenna GPS unit
+	void fuseGpsYaw();
+
+	// 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);
+
+	void updateGpsYaw(const gpsSample &gps_sample);
+
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
 	void controlMagFusion();
@@ -1165,7 +1169,6 @@ private:
 	// control for combined height fusion mode (implemented for switching between baro and range height)
 	void controlHeightFusion(const imuSample &imu_delayed);
 	void checkHeightSensorRefFallback();
-	void controlGnssHeightFusion(const gpsSample &gps_sample);
 
 #if defined(CONFIG_EKF2_BAROMETER)
 	void controlBaroHeightFusion();
@@ -1174,8 +1177,6 @@ private:
 	void updateGroundEffect();
 #endif // CONFIG_EKF2_BAROMETER
 
-	void stopGpsHgtFusion();
-
 	// gravity fusion: heuristically enable / disable gravity fusion
 	void controlGravityFusion(const imuSample &imu_delayed);
 
@@ -1212,8 +1213,6 @@ private:
 		return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
 	}
 
-	void stopGpsFusion();
-
 	void resetFakePosFusion();
 	void stopFakePosFusion();
 
@@ -1232,8 +1231,6 @@ private:
 	uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
 	uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
 
-	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
@@ -1241,13 +1238,6 @@ private:
 	bool _ev_q_error_initialized{false};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	// 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
-	bool resetYawToEKFGSF();
-
-	void resetGpsDriftCheckFilters();
-
 	void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
 	{
 		// only bother resetting if timestamp_sample is set

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -206,7 +206,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
+#if defined(CONFIG_EKF2_GNSS)
 	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -293,6 +295,7 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 			-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
 }
 
+#if defined(CONFIG_EKF2_GNSS)
 void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2],  float &vpos) const
 {
 	hvel[0] = _aid_src_gnss_vel.innovation[0];
@@ -323,6 +326,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
 	hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
 	vpos = _aid_src_gnss_hgt.test_ratio;
 }
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
@@ -440,11 +444,15 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			// determine current z
 			float current_alt = -_state.pos(2) + gps_alt_ref_prev;
 
+#if defined(CONFIG_EKF2_GNSS)
 			const float gps_hgt_bias = _gps_hgt_b_est.getBias();
+#endif // CONFIG_EKF2_GNSS
 			resetVerticalPositionTo(_gps_alt_ref - current_alt);
 
+#if defined(CONFIG_EKF2_GNSS)
 			// preserve GPS height bias
 			_gps_hgt_b_est.setBias(gps_hgt_bias);
+#endif // CONFIG_EKF2_GNSS
 		}
 
 		return true;
@@ -465,9 +473,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_control_status.flags.inertial_dead_reckoning) {
+#if defined(CONFIG_EKF2_GNSS)
 		if (_control_status.flags.gps) {
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_pos) {
@@ -490,9 +500,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_horizontal_deadreckon_time_exceeded) {
+#if defined(CONFIG_EKF2_GNSS)
 		if (_control_status.flags.gps) {
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_pos) {
@@ -523,9 +535,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 		}
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_GNSS)
 		if (_control_status.flags.gps) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_pos) {
@@ -670,6 +684,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	vel = NAN;
 	pos = NAN;
 
+#if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps) {
 		float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
 		vel = math::max(gps_vel, FLT_MIN);
@@ -677,6 +692,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
 		pos = math::max(gps_pos, FLT_MIN);
 	}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_vel) {
@@ -708,10 +724,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	}
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps_hgt) {
 		hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
 		n_hgt_sources++;
 	}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
@@ -795,10 +813,15 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
+#if defined(CONFIG_EKF2_GNSS)
 	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
 	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
 
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
+#else
+	(void)mag_innov_good;
+#endif // CONFIG_EKF2_GNSS
+
 	soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
 	*status = soln_status.value;
 }
@@ -995,6 +1018,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	_time_last_heading_fuse = _time_delayed_us;
 }
 
+#if defined(CONFIG_EKF2_GNSS)
 bool Ekf::resetYawToEKFGSF()
 {
 	if (!isYawEmergencyEstimateAvailable()) {
@@ -1019,9 +1043,11 @@ bool Ekf::resetYawToEKFGSF()
 
 	return true;
 }
+#endif // CONFIG_EKF2_GNSS
 
 bool Ekf::isYawEmergencyEstimateAvailable() const
 {
+#if defined(CONFIG_EKF2_GNSS)
 	// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
 	// data and the yaw estimate has converged
 	if (!_yawEstimator.isActive()) {
@@ -1029,23 +1055,18 @@ bool Ekf::isYawEmergencyEstimateAvailable() const
 	}
 
 	return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
+#else
+	return false;
+#endif
 }
 
+#if defined(CONFIG_EKF2_GNSS)
 bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
 			float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
 {
 	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
 }
-
+#endif // CONFIG_EKF2_GNSS
-void Ekf::resetGpsDriftCheckFilters()
-{
-	_gps_velNE_filt.setZero();
-	_gps_pos_deriv_filt.setZero();
-
-	_gps_horizontal_position_drift_rate_m_s = NAN;
-	_gps_vertical_position_drift_rate_m_s = NAN;
-	_gps_filtered_horizontal_velocity_m_s = NAN;
-}
 
 #if defined(CONFIG_EKF2_WIND)
 void Ekf::resetWind()

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -46,7 +46,9 @@
 
 EstimatorInterface::~EstimatorInterface()
 {
+#if defined(CONFIG_EKF2_GNSS)
 	delete _gps_buffer;
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	delete _mag_buffer;
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -144,6 +146,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
 
+#if defined(CONFIG_EKF2_GNSS)
 void EstimatorInterface::setGpsData(const gpsMessage &gps)
 {
 	if (!_initialised) {
@@ -222,6 +225,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_BAROMETER)
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
@@ -585,9 +589,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_GNSS)
 	if (_params.gnss_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (_params.flow_ctrl > 0) {
@@ -713,9 +719,11 @@ void EstimatorInterface::print_status()
 
 	printf("minimum observation interval %d us\n", _min_obs_interval_us);
 
+#if defined(CONFIG_EKF2_GNSS)
 	if (_gps_buffer) {
 		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_mag_buffer) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -81,17 +81,24 @@ using namespace estimator;
 class EstimatorInterface
 {
 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 setIMUData(const imuSample &imu_sample);
+	const gpsSample &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)
 	void setMagData(const magSample &mag_sample);
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-	void setGpsData(const gpsMessage &gps);
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	void setBaroData(const baroSample &baro_sample);
 #endif // CONFIG_EKF2_BAROMETER
@@ -299,17 +306,12 @@ public:
 	const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
 	const uint64_t &time_delayed_us() const { return _time_delayed_us; }
 
-	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
-
 	const bool &global_origin_valid() const { return _NED_origin_initialised; }
 	const MapProjection &global_origin() const { return _pos_ref; }
+	float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
 
 	void print_status();
 
-	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; }
-
 	OutputPredictor &output_predictor() { return _output_predictor; };
 
 protected:
@@ -345,10 +347,6 @@ protected:
 
 	OutputPredictor _output_predictor{};
 
-	// measurement samples capturing measurements on the delayed time horizon
-	gpsSample _gps_sample_delayed{};
-
-
 #if defined(CONFIG_EKF2_AIRSPEED)
 	airspeedSample _airspeed_sample_delayed{};
 #endif // CONFIG_EKF2_AIRSPEED
@@ -381,18 +379,33 @@ protected:
 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
 
+	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	bool _NED_origin_initialised{false};
+	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
+	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
 	float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin
 	float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
-	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
+
+#if defined(CONFIG_EKF2_GNSS)
+	RingBuffer<gpsSample> *_gps_buffer{nullptr};
+	uint64_t _time_last_gps_buffer_push{0};
+
+	gpsSample _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)
+	float _gps_filtered_horizontal_velocity_m_s{NAN};   // Filtered horizontal velocity (m/s)
+
 	MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
 	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
-#if defined(CONFIG_EKF2_GNSS_YAW)
+
+# 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};
-#endif // CONFIG_EKF2_GNSS_YAW
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	RingBuffer<dragSample> *_drag_buffer{nullptr};
@@ -405,10 +418,6 @@ protected:
 	bool _vertical_position_deadreckon_time_exceeded{true};
 	bool _vertical_velocity_deadreckon_time_exceeded{true};
 
-	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)
-	float _gps_filtered_horizontal_velocity_m_s{NAN};   // Filtered horizontal velocity (m/s)
-
 	uint64_t _time_last_on_ground_us{0};	///< last time we were on the ground (uSec)
 	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
 
@@ -416,8 +425,6 @@ protected:
 	static constexpr uint8_t kBufferLengthDefault = 12;
 	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
 
-	RingBuffer<gpsSample> *_gps_buffer{nullptr};
-
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	RingBuffer<magSample> *_mag_buffer{nullptr};
 	uint64_t _time_last_mag_buffer_push{0};
@@ -436,8 +443,6 @@ protected:
 #endif // CONFIG_EKF2_AUXVEL
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
-	uint64_t _time_last_gps_buffer_push{0};
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
 	uint64_t _time_last_baro_buffer_push{0};

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -76,10 +76,12 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 	const float measurement = pos(2) - pos_offset_earth(2);
 	float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
 
+#if defined(CONFIG_EKF2_GNSS)
 	// increase minimum variance if GPS active
 	if (_control_status.flags.gps_hgt) {
 		measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
 	}
+#endif // CONFIG_EKF2_GNSS
 
 	const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -125,12 +125,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 		break;
 	}
 
+#if defined(CONFIG_EKF2_GNSS)
 	// increase minimum variance if GPS active (position reference)
 	if (_control_status.flags.gps) {
 		for (int i = 0; i < 2; i++) {
 			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
 		}
 	}
+#endif // CONFIG_EKF2_GNSS
 
 	const Vector2f measurement{pos(0), pos(1)};
 

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -106,12 +106,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 		break;
 	}
 
+#if defined(CONFIG_EKF2_GNSS)
 	// increase minimum variance if GPS active (position reference)
 	if (_control_status.flags.gps) {
 		for (int i = 0; i < 2; i++) {
 			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
 		}
 	}
+#endif // CONFIG_EKF2_GNSS
 
 	const Vector3f measurement{vel};
 

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -155,10 +155,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 				_control_status.flags.ev_yaw = true;
 
 			} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
-				// turn on fusion of external vision yaw measurements and disable all other heading fusion
+				// turn on fusion of external vision yaw measurements
-				stopGpsYawFusion();
-				stopGpsFusion();
-
 				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
 
 				// reset yaw to EV

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -52,7 +52,7 @@ void Ekf::controlFakePosFusion()
 		Vector2f obs_var;
 
 		if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-			obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
+			obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, 1.f));
 
 		} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 			// Accelerate tilt fine alignment by fusing more

src/modules/ekf2/EKF/gps_checks.cpp

@@ -271,3 +271,13 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 	// 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()
+{
+	_gps_velNE_filt.setZero();
+	_gps_pos_deriv_filt.setZero();
+
+	_gps_horizontal_position_drift_rate_m_s = NAN;
+	_gps_vertical_position_drift_rate_m_s = NAN;
+	_gps_filtered_horizontal_velocity_m_s = NAN;
+}

src/modules/ekf2/EKF/gps_control.cpp

@@ -400,12 +400,9 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 		stopGpsYawFusion();
 	}
 }
-#endif // CONFIG_EKF2_GNSS_YAW
 
 void Ekf::stopGpsYawFusion()
 {
-#if defined(CONFIG_EKF2_GNSS_YAW)
-
 	if (_control_status.flags.gps_yaw) {
 
 		_control_status.flags.gps_yaw = false;
@@ -421,9 +418,8 @@ void Ekf::stopGpsYawFusion()
 			ECL_INFO("stopping GPS yaw fusion");
 		}
 	}
-
-#endif // CONFIG_EKF2_GNSS_YAW
 }
+#endif // CONFIG_EKF2_GNSS_YAW
 
 void Ekf::stopGpsFusion()
 {
@@ -436,5 +432,7 @@ void Ekf::stopGpsFusion()
 	}
 
 	stopGpsHgtFusion();
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	stopGpsYawFusion();
+#endif // CONFIG_EKF2_GNSS_YAW
 }

src/modules/ekf2/EKF/height_control.cpp

@@ -47,7 +47,9 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 	controlBaroHeightFusion();
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_GNSS)
 	controlGnssHeightFusion(_gps_sample_delayed);
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	controlRangeHeightFusion();
@@ -185,6 +187,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 	}
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps_hgt) {
 		checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance};
 	}
@@ -192,6 +195,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 	if (_control_status.flags.gps) {
 		checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]};
 	}
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -142,15 +142,25 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 	// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
 	if (_flow_data_ready) {
 
-		// Inhibit flow use if motion is un-suitable or we have good quality GPS
-		// Apply hysteresis to prevent rapid mode switching
-		const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
-
 		// Check if we are in-air and require optical flow to control position drift
-		const bool is_flow_required = _control_status.flags.in_air
+		bool is_flow_required = _control_status.flags.in_air
 					      && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
-						  || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
+						  || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
-						  || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
+
+#if defined(CONFIG_EKF2_GNSS)
+		// check if using GPS, but GPS is bad
+		if (_control_status.flags.gps) {
+			if (_control_status.flags.in_air && !is_flow_required) {
+				// Inhibit flow use if motion is un-suitable or we have good quality GPS
+				// Apply hysteresis to prevent rapid mode switching
+				const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
+
+				if (_gps_error_norm > gps_err_norm_lim) {
+					is_flow_required = true;
+				}
+			}
+		}
+#endif // CONFIG_EKF2_GNSS
 
 		// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
 		const bool preflight_motion_not_ok = !_control_status.flags.in_air

src/modules/ekf2/EKF2.cpp

@@ -62,7 +62,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_params(_ekf.getParamHandle()),
 	_param_ekf2_predict_us(_params->filter_update_interval_us),
 	_param_ekf2_imu_ctrl(_params->imu_ctrl),
-	_param_ekf2_gps_delay(_params->gps_delay_ms),
 #if defined(CONFIG_EKF2_AUXVEL)
 	_param_ekf2_avel_delay(_params->auxvel_delay_ms),
 #endif // CONFIG_EKF2_AUXVEL
@@ -73,9 +72,27 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 #if defined(CONFIG_EKF2_WIND)
 	_param_ekf2_wind_nsd(_params->wind_vel_nsd),
 #endif // CONFIG_EKF2_WIND
+	_param_ekf2_noaid_noise(_params->pos_noaid_noise),
+#if defined(CONFIG_EKF2_GNSS)
+	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
+	_param_ekf2_gps_delay(_params->gps_delay_ms),
+	_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
+	_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
+	_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
 	_param_ekf2_gps_v_noise(_params->gps_vel_noise),
 	_param_ekf2_gps_p_noise(_params->gps_pos_noise),
-	_param_ekf2_noaid_noise(_params->pos_noaid_noise),
+	_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
+	_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
+	_param_ekf2_gps_check(_params->gps_check_mask),
+	_param_ekf2_req_eph(_params->req_hacc),
+	_param_ekf2_req_epv(_params->req_vacc),
+	_param_ekf2_req_sacc(_params->req_sacc),
+	_param_ekf2_req_nsats(_params->req_nsats),
+	_param_ekf2_req_pdop(_params->req_pdop),
+	_param_ekf2_req_hdrift(_params->req_hdrift),
+	_param_ekf2_req_vdrift(_params->req_vdrift),
+	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default),
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_BAROMETER)
 	_param_ekf2_baro_ctrl(_params->baro_ctrl),
 	_param_ekf2_baro_delay(_params->baro_delay_ms),
@@ -92,8 +109,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
 # endif // CONFIG_EKF2_BARO_COMPENSATION
 #endif // CONFIG_EKF2_BAROMETER
-	_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
-	_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
 #if defined(CONFIG_EKF2_AIRSPEED)
 	_param_ekf2_asp_delay(_params->airspeed_delay_ms),
 	_param_ekf2_tas_gate(_params->tas_innov_gate),
@@ -123,16 +138,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
 	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
 #endif // CONFIG_EKF2_MAGNETOMETER
-	_param_ekf2_gps_check(_params->gps_check_mask),
-	_param_ekf2_req_eph(_params->req_hacc),
-	_param_ekf2_req_epv(_params->req_vacc),
-	_param_ekf2_req_sacc(_params->req_sacc),
-	_param_ekf2_req_nsats(_params->req_nsats),
-	_param_ekf2_req_pdop(_params->req_pdop),
-	_param_ekf2_req_hdrift(_params->req_hdrift),
-	_param_ekf2_req_vdrift(_params->req_vdrift),
 	_param_ekf2_hgt_ref(_params->height_sensor_ref),
-	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
 	_param_ekf2_noaid_tout(_params->valid_timeout_max),
 #if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
 	_param_ekf2_min_rng(_params->rng_gnd_clearance),
@@ -171,7 +177,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
 	_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-	_param_ekf2_grav_noise(_params->gravity_noise),
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	_param_ekf2_of_ctrl(_params->flow_ctrl),
 	_param_ekf2_of_delay(_params->flow_delay_ms),
@@ -187,9 +192,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
 	_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
 	_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
-	_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
-	_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
-	_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
 	_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
 	_param_ekf2_abias_init(_params->switch_on_accel_bias),
 	_param_ekf2_angerr_init(_params->initial_tilt_err),
@@ -205,7 +207,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_bcoef_y(_params->bcoef_y),
 	_param_ekf2_mcoef(_params->mcoef),
 #endif // CONFIG_EKF2_DRAG_FUSION
-	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
+	_param_ekf2_grav_noise(_params->gravity_noise)
+
 {
 	// advertise expected minimal topic set immediately to ensure logging
 	_attitude_pub.advertise();
@@ -233,7 +236,9 @@ EKF2::~EKF2()
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	perf_free(_msg_missed_distance_sensor_perf);
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_GNSS)
 	perf_free(_msg_missed_gps_perf);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_AUXVEL)
 	perf_free(_msg_missed_landing_target_pose_perf);
 #endif // CONFIG_EKF2_AUXVEL
@@ -262,7 +267,9 @@ bool EKF2::multi_init(int imu, int mag)
 	_estimator_states_pub.advertise();
 	_estimator_status_flags_pub.advertise();
 	_estimator_status_pub.advertise();
+#if defined(CONFIG_EKF2_GNSS)
 	_yaw_est_pub.advertise();
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_BAROMETER)
 
@@ -297,6 +304,8 @@ bool EKF2::multi_init(int imu, int mag)
 
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
+#if defined(CONFIG_EKF2_GNSS)
+
 	// GNSS advertise
 	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
 		_estimator_aid_src_gnss_hgt_pub.advertise();
@@ -312,13 +321,14 @@ bool EKF2::multi_init(int imu, int mag)
 		_estimator_aid_src_gnss_vel_pub.advertise();
 	}
 
-#if defined(CONFIG_EKF2_GNSS_YAW)
+# if defined(CONFIG_EKF2_GNSS_YAW)
 
 	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
 		_estimator_aid_src_gnss_yaw_pub.advertise();
 	}
 
-#endif // CONFIG_EKF2_GNSS_YAW
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 
@@ -395,7 +405,9 @@ int EKF2::print_status()
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	perf_print_counter(_msg_missed_distance_sensor_perf);
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_GNSS)
 	perf_print_counter(_msg_missed_gps_perf);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_AUXVEL)
 	perf_print_counter(_msg_missed_landing_target_pose_perf);
 #endif // CONFIG_EKF2_AUXVEL
@@ -434,7 +446,9 @@ void EKF2::Run()
 
 		VerifyParams();
 
+#if defined(CONFIG_EKF2_GNSS)
 		_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
+#endif // CONFIG_EKF2_GNSS
 
 		const matrix::Vector3f imu_pos_body(_param_ekf2_imu_pos_x.get(),
 						    _param_ekf2_imu_pos_y.get(),
@@ -721,7 +735,9 @@ void EKF2::Run()
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		UpdateFlowSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
+#if defined(CONFIG_EKF2_GNSS)
 		UpdateGpsSample(ekf2_timestamps);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 		UpdateMagSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -739,43 +755,49 @@ void EKF2::Run()
 			PublishLocalPosition(now);
 			PublishOdometry(now, imu_sample_new);
 			PublishGlobalPosition(now);
+			PublishSensorBias(now);
 
 #if defined(CONFIG_EKF2_WIND)
 			PublishWindEstimate(now);
 #endif // CONFIG_EKF2_WIND
 
 			// publish status/logging messages
-#if defined(CONFIG_EKF2_BAROMETER)
-			PublishBaroBias(now);
-#endif // CONFIG_EKF2_BAROMETER
-			PublishGnssHgtBias(now);
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-			PublishRngHgtBias(now);
-#endif // CONFIG_EKF2_RANGE_FINDER
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-			PublishEvPosBias(now);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
 			PublishEventFlags(now);
-			PublishGpsStatus(now);
 			PublishInnovations(now);
 			PublishInnovationTestRatios(now);
 			PublishInnovationVariances(now);
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-			PublishOpticalFlowVel(now);
-#endif // CONFIG_EKF2_OPTICAL_FLOW
 			PublishStates(now);
 			PublishStatus(now);
 			PublishStatusFlags(now);
+			PublishAidSourceStatus(now);
+
+#if defined(CONFIG_EKF2_BAROMETER)
+			PublishBaroBias(now);
+#endif // CONFIG_EKF2_BAROMETER
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+			PublishRngHgtBias(now);
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+			PublishEvPosBias(now);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+#if defined(CONFIG_EKF2_GNSS)
+			PublishGnssHgtBias(now);
+			PublishGpsStatus(now);
 			PublishYawEstimatorStatus(now);
+#endif // CONFIG_EKF2_GNSS
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+			PublishOpticalFlowVel(now);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 			UpdateAccelCalibration(now);
 			UpdateGyroCalibration(now);
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 			UpdateMagCalibration(now);
 #endif // CONFIG_EKF2_MAGNETOMETER
-			PublishSensorBias(now);
-
-			PublishAidSourceStatus(now);
 
 		} else {
 			// ekf no update
@@ -792,6 +814,8 @@ void EKF2::Run()
 
 void EKF2::VerifyParams()
 {
+#if defined(CONFIG_EKF2_GNSS)
+
 	if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS)
 	    || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) {
 		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS |
@@ -816,6 +840,8 @@ void EKF2::VerifyParams()
 				    "GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get());
 	}
 
+#endif // CONFIG_EKF2_GNSS
+
 #if defined(CONFIG_EKF2_BAROMETER)
 
 	if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) {
@@ -846,6 +872,8 @@ void EKF2::VerifyParams()
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_GNSS)
+
 	if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) {
 		_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL));
 		_param_ekf2_gps_ctrl.commit();
@@ -857,6 +885,8 @@ void EKF2::VerifyParams()
 				    "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get());
 	}
 
+#endif // CONFIG_EKF2_GNSS
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 
 	if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV)
@@ -1034,13 +1064,15 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
+#if defined(CONFIG_EKF2_GNSS)
 	// GNSS hgt/pos/vel/yaw
 	PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
-#if defined(CONFIG_EKF2_GNSS_YAW)
+# if defined(CONFIG_EKF2_GNSS_YAW)
 	PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
-#endif // CONFIG_EKF2_GNSS_YAW
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// mag heading
@@ -1115,6 +1147,7 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
 }
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_GNSS)
 void EKF2::PublishGnssHgtBias(const hrt_abstime &timestamp)
 {
 	if (_ekf.get_gps_sample_delayed().time_us != 0) {
@@ -1127,6 +1160,7 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime &timestamp)
 		}
 	}
 }
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 void EKF2::PublishRngHgtBias(const hrt_abstime &timestamp)
@@ -1280,7 +1314,11 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 		_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
 
 		global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
+#if defined(CONFIG_EKF2_GNSS)
 		global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
+#else
+		global_pos.alt_ellipsoid = global_pos.alt;
+#endif
 
 		// delta_alt, alt_reset_counter
 		//  global altitude has opposite sign of local down position
@@ -1319,6 +1357,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 	}
 }
 
+#if defined(CONFIG_EKF2_GNSS)
 void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
 {
 	const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us;
@@ -1353,13 +1392,16 @@ void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
 
 	_last_gps_status_published = timestamp_sample;
 }
+#endif // CONFIG_EKF2_GNSS
 
 void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 {
 	// publish estimator innovation data
 	estimator_innovations_s innovations{};
 	innovations.timestamp_sample = _ekf.time_delayed_us();
+#if defined(CONFIG_EKF2_GNSS)
 	_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -1449,8 +1491,10 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	// publish estimator innovation test ratio data
 	estimator_innovations_s test_ratios{};
 	test_ratios.timestamp_sample = _ekf.time_delayed_us();
+#if defined(CONFIG_EKF2_GNSS)
 	_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
 				    test_ratios.gps_vpos);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -1504,7 +1548,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	// publish estimator innovation variance data
 	estimator_innovations_s variances{};
 	variances.timestamp_sample = _ekf.time_delayed_us();
+#if defined(CONFIG_EKF2_GNSS)
 	_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -1787,9 +1833,11 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 
 	_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
 
+#if defined(CONFIG_EKF2_GNSS)
 	// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
 	// the GPS Fix bit, which is always checked)
 	status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1);
+#endif // CONFIG_EKF2_GNSS
 
 	status.control_mode_flags = _ekf.control_status().value;
 	status.filter_fault_flags = _ekf.fault_status().value;
@@ -1948,6 +1996,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 	}
 }
 
+#if defined(CONFIG_EKF2_GNSS)
 void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 {
 	static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
@@ -1967,6 +2016,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 		_yaw_est_pub.publish(yaw_est_test_data);
 	}
 }
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_WIND)
 void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
@@ -2029,6 +2079,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_GNSS)
 float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 {
 	float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
@@ -2049,6 +2100,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 
 	return amsl_hgt + _wgs84_hgt_offset;
 }
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
@@ -2412,6 +2464,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_GNSS)
 void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF GPS message
@@ -2455,6 +2508,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 		_gps_alttitude_ellipsoid = static_cast<int32_t>(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3));
 	}
 }
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)

src/modules/ekf2/EKF2.hpp

@@ -68,7 +68,6 @@
 #include <uORB/topics/estimator_bias.h>
 #include <uORB/topics/estimator_bias3d.h>
 #include <uORB/topics/estimator_event_flags.h>
-#include <uORB/topics/estimator_gps_status.h>
 #include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_states.h>
@@ -80,7 +79,6 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -101,6 +99,11 @@
 # include <uORB/topics/vehicle_air_data.h>
 #endif // CONFIG_EKF2_BAROMETER
 
+#if defined(CONFIG_EKF2_GNSS)
+# include <uORB/topics/estimator_gps_status.h>
+# include <uORB/topics/sensor_gps.h>
+#endif // CONFIG_EKF2_GNSS
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 # include <uORB/topics/vehicle_magnetometer.h>
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -180,7 +183,6 @@ private:
 #if defined(CONFIG_EKF2_BAROMETER)
 	void PublishBaroBias(const hrt_abstime &timestamp);
 #endif // CONFIG_EKF2_BAROMETER
-	void PublishGnssHgtBias(const hrt_abstime &timestamp);
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	void PublishRngHgtBias(const hrt_abstime &timestamp);
@@ -193,15 +195,11 @@ private:
 					      uint64_t timestamp, uint32_t device_id = 0);
 	void PublishEventFlags(const hrt_abstime &timestamp);
 	void PublishGlobalPosition(const hrt_abstime &timestamp);
-	void PublishGpsStatus(const hrt_abstime &timestamp);
 	void PublishInnovations(const hrt_abstime &timestamp);
 	void PublishInnovationTestRatios(const hrt_abstime &timestamp);
 	void PublishInnovationVariances(const hrt_abstime &timestamp);
 	void PublishLocalPosition(const hrt_abstime &timestamp);
 	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample);
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	void PublishOpticalFlowVel(const hrt_abstime &timestamp);
-#endif // CONFIG_EKF2_OPTICAL_FLOW
 	void PublishSensorBias(const hrt_abstime &timestamp);
 	void PublishStates(const hrt_abstime &timestamp);
 	void PublishStatus(const hrt_abstime &timestamp);
@@ -209,7 +207,6 @@ private:
 #if defined(CONFIG_EKF2_WIND)
 	void PublishWindEstimate(const hrt_abstime &timestamp);
 #endif // CONFIG_EKF2_WIND
-	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -223,16 +220,28 @@ private:
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
+#if defined(CONFIG_EKF2_GNSS)
+	/*
+	 * Calculate filtered WGS84 height from estimated AMSL height
+	 */
+	float filter_altitude_ellipsoid(float amsl_hgt);
+
+	void PublishGpsStatus(const hrt_abstime &timestamp);
+	void PublishGnssHgtBias(const hrt_abstime &timestamp);
+	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
+	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
+#endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
+	void PublishOpticalFlowVel(const hrt_abstime &timestamp);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_RANGE_FINDER
+
 	void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps);
 
 	// Used to check, save and use learned accel/gyro/mag biases
@@ -267,11 +276,6 @@ private:
 		}
 	}
 
-	/*
-	 * Calculate filtered WGS84 height from estimated AMSL height
-	 */
-	float filter_altitude_ellipsoid(float amsl_hgt);
-
 	static constexpr float sq(float x) { return x * x; };
 
 	const bool _replay_mode{false};			///< true when we use replay data from a log
@@ -288,17 +292,11 @@ private:
 	perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
 	perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
 	perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
-	perf_counter_t _msg_missed_gps_perf{nullptr};
 	perf_counter_t _msg_missed_odometry_perf{nullptr};
 
 	InFlightCalibration _accel_cal{};
 	InFlightCalibration _gyro_cal{};
 
-	uint64_t _gps_time_usec{0};
-	int32_t _gps_alttitude_ellipsoid{0};			///< altitude in 1E-3 meters (millimeters) above ellipsoid
-	uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
-	float   _wgs84_hgt_offset = 0;  ///< height offset between AMSL and WGS84
-
 	uint8_t _accel_calibration_count{0};
 	uint8_t _gyro_calibration_count{0};
 
@@ -309,7 +307,6 @@ private:
 	Vector3f _last_gyro_bias_published{};
 
 	hrt_abstime _last_sensor_bias_published{0};
-	hrt_abstime _last_gps_status_published{0};
 
 	hrt_abstime _status_fake_hgt_pub_last{0};
 	hrt_abstime _status_fake_pos_pub_last{0};
@@ -352,13 +349,6 @@ private:
 	uORB::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	hrt_abstime _status_gnss_hgt_pub_last{0};
-	hrt_abstime _status_gnss_pos_pub_last{0};
-	hrt_abstime _status_gnss_vel_pub_last{0};
-#if defined(CONFIG_EKF2_GNSS_YAW)
-	hrt_abstime _status_gnss_yaw_pub_last {0};
-#endif // CONFIG_EKF2_GNSS_YAW
-
 	hrt_abstime _status_gravity_pub_last{0};
 
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -413,8 +403,6 @@ private:
 	hrt_abstime _status_drag_pub_last{0};
 #endif // CONFIG_EKF2_DRAG_FUSION
 
-	float _last_gnss_hgt_bias_published{};
-
 #if defined(CONFIG_EKF2_AIRSPEED)
 	uORB::Subscription _airspeed_sub {ORB_ID(airspeed)};
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
@@ -441,7 +429,6 @@ private:
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
@@ -477,9 +464,7 @@ private:
 	uint32_t _filter_information_event_changes{0};
 
 	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
-	uORB::PublicationMulti<estimator_bias_s>             _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
 	uORB::PublicationMultiData<estimator_event_flags_s>  _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
-	uORB::PublicationMulti<estimator_gps_status_s>       _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovations_pub{ORB_ID(estimator_innovations)};
@@ -487,18 +472,10 @@ private:
 	uORB::PublicationMulti<estimator_states_s>           _estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::PublicationMulti<estimator_status_flags_s>     _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
 	uORB::PublicationMulti<estimator_status_s>           _estimator_status_pub{ORB_ID(estimator_status)};
-	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
 
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
 
-	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
-	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
-	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
-#if defined(CONFIG_EKF2_GNSS_YAW)
-	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
-#endif // CONFIG_EKF2_GNSS_YAW
-
 	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
 
 	// publications with topic dependent on multi-mode
@@ -511,6 +488,36 @@ private:
 	uORB::PublicationMulti<wind_s>              _wind_pub;
 #endif // CONFIG_EKF2_WIND
 
+#if defined(CONFIG_EKF2_GNSS)
+	perf_counter_t _msg_missed_gps_perf {nullptr};
+
+	uint64_t _gps_time_usec{0};
+	int32_t _gps_alttitude_ellipsoid{0};			///< altitude in 1E-3 meters (millimeters) above ellipsoid
+	uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
+	float   _wgs84_hgt_offset = 0;  ///< height offset between AMSL and WGS84
+
+	hrt_abstime _last_gps_status_published{0};
+
+	hrt_abstime _status_gnss_hgt_pub_last{0};
+	hrt_abstime _status_gnss_pos_pub_last{0};
+	hrt_abstime _status_gnss_vel_pub_last{0};
+
+	float _last_gnss_hgt_bias_published{};
+
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+
+	uORB::PublicationMulti<estimator_bias_s> _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
+	uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
+	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
+	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
+	uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
+# if defined(CONFIG_EKF2_GNSS_YAW)
+	hrt_abstime _status_gnss_yaw_pub_last {0};
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
+# endif // CONFIG_EKF2_GNSS_YAW
+#endif // CONFIG_EKF2_GNSS
+
 	PreFlightChecker _preflt_checker;
 
 	Ekf _ekf;
@@ -521,9 +528,6 @@ private:
 		(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
 		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
 
-		(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
-		_param_ekf2_gps_delay,	///< GPS measurement delay relative to the IMU (mSec)
-
 #if defined(CONFIG_EKF2_AUXVEL)
 		(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
 		_param_ekf2_avel_delay,	///< auxiliary velocity measurement delay relative to the IMU (mSec)
@@ -544,12 +548,35 @@ private:
 		(ParamExtFloat<px4::params::EKF2_WIND_NSD>) _param_ekf2_wind_nsd,
 #endif // CONFIG_EKF2_WIND
 
-		(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
+		(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>) _param_ekf2_noaid_noise,
-		_param_ekf2_gps_v_noise,	///< minimum allowed observation noise for gps velocity fusion (m/sec)
+
-		(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>)
+#if defined(CONFIG_EKF2_GNSS)
-		_param_ekf2_gps_p_noise,	///< minimum allowed observation noise for gps position fusion (m)
+		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
-		(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>)
+		(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _param_ekf2_gps_delay,
-		_param_ekf2_noaid_noise,	///< observation noise for non-aiding position fusion (m)
+
+		(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,
+		(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y,
+		(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z,
+
+		(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>) _param_ekf2_gps_v_noise,
+		(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,
+
+		(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>) _param_ekf2_gps_p_gate,
+		(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>) _param_ekf2_gps_v_gate,
+
+		(ParamExtInt<px4::params::EKF2_GPS_CHECK>) _param_ekf2_gps_check,
+		(ParamExtFloat<px4::params::EKF2_REQ_EPH>)    _param_ekf2_req_eph,
+		(ParamExtFloat<px4::params::EKF2_REQ_EPV>)    _param_ekf2_req_epv,
+		(ParamExtFloat<px4::params::EKF2_REQ_SACC>)   _param_ekf2_req_sacc,
+		(ParamExtInt<px4::params::EKF2_REQ_NSATS>)    _param_ekf2_req_nsats,
+		(ParamExtFloat<px4::params::EKF2_REQ_PDOP>)   _param_ekf2_req_pdop,
+		(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>) _param_ekf2_req_hdrift,
+		(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,
+		(ParamFloat<px4::params::EKF2_REQ_GPS_H>)     _param_ekf2_req_gps_h,
+
+		// Used by EKF-GSF experimental yaw estimator
+		(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas_default,
+#endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_BAROMETER)
 		(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
@@ -570,11 +597,6 @@ private:
 # endif // CONFIG_EKF2_BARO_COMPENSATION
 #endif // CONFIG_EKF2_BAROMETER
 
-		(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
-		_param_ekf2_gps_p_gate,	///< GPS horizontal position innovation consistency gate size (STD)
-		(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>)
-		_param_ekf2_gps_v_gate,	///< GPS velocity innovation consistency gate size (STD)
-
 #if defined(CONFIG_EKF2_AIRSPEED)
 		(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
 		_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
@@ -616,23 +638,10 @@ private:
 		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synthetic_mag_z,
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-		(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
-		_param_ekf2_gps_check,	///< bitmask used to control which GPS quality checks are used
-		(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph,	///< maximum acceptable horiz position error (m)
-		(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv,	///< maximum acceptable vert position error (m)
-		(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc,	///< maximum acceptable speed error (m/s)
-		(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats,	///< minimum acceptable satellite count
-		(ParamExtFloat<px4::params::EKF2_REQ_PDOP>)
-		_param_ekf2_req_pdop,	///< maximum acceptable position dilution of precision
-		(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
-		_param_ekf2_req_hdrift,	///< maximum acceptable horizontal drift speed (m/s)
-		(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,	///< maximum acceptable vertical drift speed (m/s)
-
 		// measurement source control
 		(ParamInt<px4::params::EKF2_AID_MASK>)
 		_param_ekf2_aid_mask,		///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
 		(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref,    ///< selects the primary source for height data
-		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,  ///< GPS control selection
 
 		(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
 		_param_ekf2_noaid_tout,	///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
@@ -689,10 +698,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
 		_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-
-		(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>)
-		_param_ekf2_grav_noise,	///< default accelerometer noise for gravity fusion measurements (m/s**2)
-
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		// optical flow fusion
 		(ParamExtInt<px4::params::EKF2_OF_CTRL>)
@@ -721,15 +726,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x,		///< X position of IMU in body frame (m)
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y,		///< Y position of IMU in body frame (m)
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z,		///< Z position of IMU in body frame (m)
-		(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,		///< X position of GPS antenna in body frame (m)
-		(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y,		///< Y position of GPS antenna in body frame (m)
-		(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z,		///< Z position of GPS antenna in body frame (m)
-
-		// output predictor filter time constants
-		(ParamFloat<px4::params::EKF2_TAU_VEL>)
-		_param_ekf2_tau_vel,		///< time constant used by the output velocity complementary filter (sec)
-		(ParamFloat<px4::params::EKF2_TAU_POS>)
-		_param_ekf2_tau_pos,		///< time constant used by the output position complementary filter (sec)
 
 		// IMU switch on bias parameters
 		(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
@@ -760,12 +756,11 @@ private:
 		(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef,		///< propeller momentum drag coefficient (1/s)
 #endif // CONFIG_EKF2_DRAG_FUSION
 
-		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
+		// output predictor filter time constants
-
+		(ParamFloat<px4::params::EKF2_TAU_VEL>) _param_ekf2_tau_vel,
-		// Used by EKF-GSF experimental yaw estimator
+		(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos,
-		(ParamExtFloat<px4::params::EKF2_GSF_TAS>)
-		_param_ekf2_gsf_tas_default	///< default value of true airspeed assumed during fixed wing operation
 
+		(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise
 	)
 };
 #endif // !EKF2_HPP

src/modules/ekf2/Kconfig

@@ -60,10 +60,18 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 external vision (EV) fusion support.
 
+menuconfig EKF2_GNSS
+depends on MODULES_EKF2
+	bool "GNSS fusion support"
+	default y
+	---help---
+		EKF2 GNSS fusion support.
+
 menuconfig EKF2_GNSS_YAW
 depends on MODULES_EKF2
         bool "GNSS yaw fusion support"
         default y
+	depends on EKF2_GNSS
 	---help---
 		EKF2 GNSS yaw fusion support.