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

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{};
-	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
-
-	estimator_aid_source3d_s _aid_src_gravity{};
-
-#if defined(CONFIG_EKF2_AUXVEL)
-	estimator_aid_source2d_s _aid_src_aux_vel{};
-#endif // CONFIG_EKF2_AUXVEL
+#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
 	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
-	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
+	gps_check_fail_status_u _gps_check_fail_status{};
+	// 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);
 }
-
-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;
-}
+#endif // CONFIG_EKF2_GNSS
 
 #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
-				stopGpsYawFusion();
-				stopGpsFusion();
-
+				// turn on fusion of external vision yaw measurements
 				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)
-						  || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
+						  || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
+
+#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