ekf2: add kconfig option to enable/disable external vision fusion

src/modules/ekf2/EKF/CMakeLists.txt

@@ -41,11 +41,6 @@ list(APPEND EKF_SRCS
 	ekf_helper.cpp
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
-	ev_control.cpp
-	ev_height_control.cpp
-	ev_pos_control.cpp
-	ev_vel_control.cpp
-	ev_yaw_control.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp
@@ -80,6 +75,17 @@ if(CONFIG_EKF2_DRAG_FUSION)
 	list(APPEND EKF_SRCS drag_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_EXTERNAL_VISION)
+	list(APPEND EKF_SRCS
+		ev_control.cpp
+		ev_height_control.cpp
+		ev_pos_control.cpp
+		ev_vel_control.cpp
+		ev_yaw_control.cpp
+	)
+endif()
+
+
 if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()

src/modules/ekf2/EKF/common.h

@@ -238,6 +238,7 @@ struct flowSample {
 	uint8_t     quality{};     ///< quality indicator between 0 and 255
 };
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 struct extVisionSample {
 	uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
 	Vector3f    pos{};         ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
@@ -251,6 +252,7 @@ struct extVisionSample {
 	uint8_t     reset_counter{};
 	int8_t     quality{};     ///< quality indicator between 0 and 100
 };
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 struct dragSample {
@@ -299,7 +301,6 @@ struct parameters {
 	int32_t baro_ctrl{1};
 	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
 	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
-	int32_t ev_ctrl{0};
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
@@ -311,7 +312,6 @@ struct parameters {
 	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
 	float flow_delay_ms{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
-	float ev_delay_ms{175.0f};              ///< off-board vision 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)
@@ -395,7 +395,11 @@ struct parameters {
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// vision position fusion
+	int32_t ev_ctrl{0};
+	float ev_delay_ms{175.0f};              ///< off-board vision measurement delay relative to the IMU (mSec)
+
 	float ev_vel_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
 	float ev_pos_noise{0.1f};               ///< minimum allowed observation noise for EV position fusion (m)
 	float ev_att_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
@@ -403,6 +407,7 @@ struct parameters {
 	float ev_vel_innov_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	// gravity fusion
 	float gravity_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)

src/modules/ekf2/EKF/control.cpp

@@ -122,8 +122,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlHeightFusion(imu_delayed);
 	controlGravityFusion(imu_delayed);
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused

src/modules/ekf2/EKF/covariance.cpp

@@ -444,14 +444,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		if (fabsf(down_dvel_bias) > dVel_bias_lim) {
 
 			bool bad_vz_gps = _control_status.flags.gps    && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
+#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
+			bool bad_vz_ev  = false;
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 			if (bad_vz_gps || bad_vz_ev) {
 				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
 				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
 				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _aid_src_rng_hgt.innovation  < 0.0f);
-				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _aid_src_ev_hgt.innovation   < 0.0f);
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _aid_src_ev_hgt.innovation   < 0.0f);
+#else
+				bool bad_z_ev   = false;
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 				if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
 					bad_acc_bias = true;

src/modules/ekf2/EKF/ekf.cpp

@@ -126,10 +126,12 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_fake_pos);
 	resetEstimatorAidStatus(_aid_src_fake_hgt);
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	resetEstimatorAidStatus(_aid_src_ev_hgt);
 	resetEstimatorAidStatus(_aid_src_ev_pos);
 	resetEstimatorAidStatus(_aid_src_ev_vel);
 	resetEstimatorAidStatus(_aid_src_ev_yaw);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	resetEstimatorAidStatus(_aid_src_gnss_hgt);
 	resetEstimatorAidStatus(_aid_src_gnss_pos);

src/modules/ekf2/EKF/ekf.h

@@ -86,9 +86,11 @@ public:
 	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;
 	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
 	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
@@ -140,10 +142,12 @@ public:
 		}
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_yaw) {
 			heading_innov = _aid_src_ev_yaw.innovation;
 			return;
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
 	void getHeadingInnovVar(float &heading_innov_var) const
@@ -165,10 +169,12 @@ public:
 		}
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_yaw) {
 			heading_innov_var = _aid_src_ev_yaw.innovation_variance;
 			return;
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
 	void getHeadingInnovRatio(float &heading_innov_ratio) const
@@ -190,10 +196,12 @@ public:
 		}
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_yaw) {
 			heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
 			return;
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
 	void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
@@ -443,9 +451,11 @@ public:
 	const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 	const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
 	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
 
 	const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
@@ -461,10 +471,12 @@ public:
 	const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	const auto &aid_src_ev_hgt() const { return _aid_src_ev_hgt; }
 	const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; }
 	const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; }
 	const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
@@ -525,9 +537,6 @@ private:
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
 
-	float _ev_yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
-	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
-
 	// 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
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
@@ -547,10 +556,6 @@ private:
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 	uint64_t _time_last_healthy_rng_data{0};
 
-	uint8_t _nb_ev_pos_reset_available{0};
-	uint8_t _nb_ev_vel_reset_available{0};
-	uint8_t _nb_ev_yaw_reset_available{0};
-
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
@@ -609,11 +614,20 @@ private:
 	estimator_aid_source2d_s _aid_src_fake_pos{};
 	estimator_aid_source1d_s _aid_src_fake_hgt{};
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	estimator_aid_source1d_s _aid_src_ev_hgt{};
 	estimator_aid_source2d_s _aid_src_ev_pos{};
 	estimator_aid_source3d_s _aid_src_ev_vel{};
 	estimator_aid_source1d_s _aid_src_ev_yaw{};
 
+	float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
+
+	uint8_t _nb_ev_pos_reset_available{0};
+	uint8_t _nb_ev_vel_reset_available{0};
+	uint8_t _nb_ev_yaw_reset_available{0};
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
+
 	estimator_aid_source1d_s _aid_src_gnss_hgt{};
 	estimator_aid_source2d_s _aid_src_gnss_pos{};
 	estimator_aid_source3d_s _aid_src_gnss_vel{};
@@ -946,18 +960,21 @@ private:
 	// Control the filter fusion modes
 	void controlFusionModes(const imuSample &imu_delayed);
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
-
 	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
-
 	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
+	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
+	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
+
 	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
 	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
 	void stopEvPosFusion();
-
-	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
-	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
+	void stopEvHgtFusion();
+	void stopEvVelFusion();
+	void stopEvYawFusion();
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion(const imuSample &imu_delayed);
@@ -1029,7 +1046,6 @@ private:
 	void stopBaroHgtFusion();
 	void stopGpsHgtFusion();
 	void stopRngHgtFusion();
-	void stopEvHgtFusion();
 
 	void updateGroundEffect();
 
@@ -1095,9 +1111,6 @@ private:
 	void stopGpsPosFusion();
 	void stopGpsVelFusion();
 
-	void stopEvVelFusion();
-	void stopEvYawFusion();
-
 	void stopFlowFusion();
 
 	void resetFakePosFusion();
@@ -1121,8 +1134,11 @@ private:
 	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
 	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
 	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _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};
+#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

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -150,7 +150,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 
 	_state_reset_status.reset_count.posNE++;
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
 
 	// Reset the timout timer
@@ -196,7 +198,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 	_state_reset_status.reset_count.posD++;
 
 	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
 	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
 
@@ -304,6 +308,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
 	vpos = _aid_src_gnss_hgt.test_ratio;
 }
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
 {
 	hvel[0] = _aid_src_ev_vel.innovation[0];
@@ -334,6 +339,7 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp
 	hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
 	vpos = _aid_src_ev_hgt.test_ratio;
 }
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
@@ -466,9 +472,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_pos) {
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
 	*ekf_eph = hpos_err;
@@ -489,9 +497,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_pos) {
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
 	*ekf_eph = hpos_err;
@@ -516,14 +526,17 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 
 		if (_control_status.flags.gps) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
+		}
 
-		} else if (_control_status.flags.ev_pos) {
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+		if (_control_status.flags.ev_pos) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}
 
 		if (_control_status.flags.ev_vel) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm());
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 		hvel_err = math::max(hvel_err, vel_err_conservative);
 	}
@@ -637,6 +650,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	}
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	if (_control_status.flags.ev_yaw) {
+		mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio));
+	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
 	// return the largest velocity and position innovation test ratio
 	vel = NAN;
 	pos = NAN;
@@ -649,6 +668,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		pos = math::max(gps_pos, FLT_MIN);
 	}
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_vel) {
 		float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max());
 		vel = math::max(vel, ev_vel, FLT_MIN);
@@ -658,6 +678,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max());
 		pos = math::max(pos, ev_pos, FLT_MIN);
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
 		float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
@@ -683,10 +704,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		n_hgt_sources++;
 	}
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_hgt) {
 		hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio);
 		n_hgt_sources++;
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	if (n_hgt_sources > 0) {
 		hgt = math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
@@ -1136,9 +1159,12 @@ bool Ekf::resetYawToEKFGSF()
 		_control_status.flags.gps_yaw_fault = true;
 		_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
 
-	} else if (_control_status.flags.ev_yaw) {
+	}
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	if (_control_status.flags.ev_yaw) {
 		_inhibit_ev_yaw_use = true;
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	return true;
 }

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -54,7 +54,9 @@ EstimatorInterface::~EstimatorInterface()
 	delete _airspeed_buffer;
 #endif // CONFIG_EKF2_AIRSPEED
 	delete _flow_buffer;
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	delete _ext_vision_buffer;
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	delete _drag_buffer;
 #endif // CONFIG_EKF2_DRAG_FUSION
@@ -355,7 +357,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 	}
 }
 
-// set attitude and position data derived from an external vision system
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 {
 	if (!_initialised) {
@@ -392,6 +394,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 		ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
@@ -561,9 +564,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_params.ev_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
 
@@ -703,9 +708,11 @@ void EstimatorInterface::print_status()
 		printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
 	}
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_ext_vision_buffer) {
 		printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	if (_drag_buffer) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -99,8 +99,10 @@ public:
 	// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
 	void setOpticalFlowData(const flowSample &flow);
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// set external vision position and attitude data
 	void setExtVisionData(const extVisionSample &evdata);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	void setAuxVelData(const auxVelSample &auxvel_sample);
@@ -295,11 +297,17 @@ protected:
 	// measurement samples capturing measurements on the delayed time horizon
 	gpsSample _gps_sample_delayed{};
 	sensor::SensorRangeFinder _range_sensor{};
+
 #if defined(CONFIG_EKF2_AIRSPEED)
 	airspeedSample _airspeed_sample_delayed{};
 #endif // CONFIG_EKF2_AIRSPEED
+
 	flowSample _flow_sample_delayed{};
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	extVisionSample _ev_sample_prev{};
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
 	RangeFinderConsistencyCheck _rng_consistency_check;
 
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
@@ -358,7 +366,11 @@ protected:
 	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
 #endif // CONFIG_EKF2_AIRSPEED
 	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
+	uint64_t _time_last_ext_vision_buffer_push{0};
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_AUXVEL)
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
 #endif // CONFIG_EKF2_AUXVEL
@@ -368,7 +380,6 @@ protected:
 	uint64_t _time_last_mag_buffer_push{0};
 	uint64_t _time_last_baro_buffer_push{0};
 	uint64_t _time_last_range_buffer_push{0};
-	uint64_t _time_last_ext_vision_buffer_push{0};
 
 	uint64_t _time_last_gnd_effect_on{0};
 

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -178,9 +178,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 
 void Ekf::stopEvYawFusion()
 {
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_yaw) {
 		resetEstimatorAidStatus(_aid_src_ev_yaw);
 
 		_control_status.flags.ev_yaw = false;
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 }

src/modules/ekf2/EKF/gps_control.cpp

@@ -119,6 +119,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 						     _aid_src_gnss_pos);
 		_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start
 		if (_control_status.flags.tilt_align && _NED_origin_initialised
 		    && gps_checks_passing && !gps_checks_failing) {
@@ -139,7 +140,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				}
 			}
 		}
-
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
@@ -202,6 +203,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 		} else {
 			if (starting_conditions_passing) {
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 				// Do not use external vision for yaw if using GPS because yaw needs to be
 				// defined relative to an NED reference frame
 				if (_control_status.flags.ev_yaw) {
@@ -209,6 +211,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 					stopEvYawFusion();
 					_inhibit_ev_yaw_use = true;
 				}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 				ECL_INFO("starting GPS fusion");
 				_information_events.flags.starting_gps_fusion = true;
@@ -396,7 +399,7 @@ void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
 		ECL_INFO("starting GPS yaw fusion");
 		_control_status.flags.yaw_align = true;
 		_control_status.flags.mag_dec = false;
-		stopEvYawFusion();
+
 		stopMagHdgFusion();
 		stopMag3DFusion();
 		_control_status.flags.gps_yaw = true;

src/modules/ekf2/EKF/height_control.cpp

@@ -166,11 +166,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 	enum class ReferenceType { PRESSURE, GNSS, GROUND };
 
 	struct {
-		ReferenceType ref_type;
-		float innov;
-		float innov_var;
-		bool failed_min;
-		bool failed_lim;
+		ReferenceType ref_type{};
+		float innov{0.f};
+		float innov_var{0.f};
+		bool failed_min{false};
+		bool failed_lim{false};
 	} checks[6] {};
 
 	if (_control_status.flags.baro_hgt) {
@@ -189,6 +189,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 		checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
 	}
 
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_hgt) {
 		checks[4] = {ReferenceType::GROUND, _aid_src_ev_hgt.innovation, _aid_src_ev_hgt.innovation_variance};
 	}
@@ -196,6 +197,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 	if (_control_status.flags.ev_vel) {
 		checks[5] = {ReferenceType::GROUND, _aid_src_ev_vel.innovation[2], _aid_src_ev_vel.innovation_variance[2]};
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	// Compute the check based on innovation ratio for all the sources
 	for (unsigned i = 0; i < 6; i++) {