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