ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -52,10 +52,6 @@ list(APPEND EKF_SRCS
 	mag_control.cpp
 	mag_fusion.cpp
 	output_predictor.cpp
-	range_finder_consistency_check.cpp
-	range_height_control.cpp
-	sensor_range_finder.cpp
-	terrain_estimator.cpp
 	vel_pos_fusion.cpp
 	zero_innovation_heading_update.cpp
 	zero_velocity_update.cpp
@@ -94,6 +90,15 @@ if(CONFIG_EKF2_OPTICAL_FLOW)
 	)
 endif()
 
+if(CONFIG_EKF2_RANGE_FINDER)
+	list(APPEND EKF_SRCS
+		range_finder_consistency_check.cpp
+		range_height_control.cpp
+		sensor_range_finder.cpp
+		terrain_estimator.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_SIDESLIP)
 	list(APPEND EKF_SRCS sideslip_fusion.cpp)
 endif()

src/modules/ekf2/EKF/common.h

@@ -108,10 +108,12 @@ enum MagFuseType : uint8_t {
 	NONE    = 5    	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
 };
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 enum TerrainFusionMask : uint8_t {
 	TerrainFuseRangeFinder = (1 << 0),
 	TerrainFuseOpticalFlow = (1 << 1)
 };
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 enum HeightSensor : uint8_t {
 	BARO  = 0,
@@ -300,9 +302,6 @@ struct parameters {
 	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 rng_ctrl{RngCtrl::CONDITIONAL};
-	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
-				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
 	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)
 
@@ -310,7 +309,6 @@ struct parameters {
 	float mag_delay_ms{0.0f};               ///< magnetometer measurement delay relative to the IMU (mSec)
 	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)
-	float range_delay_ms{5.0f};             ///< range finder 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)
@@ -324,10 +322,6 @@ struct parameters {
 	float wind_vel_nsd{1.0e-2f};        ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
 	const float wind_vel_nsd_scaler{0.5f};      ///< scaling of wind process noise with vertical velocity
 
-	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
-	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
-	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
-
 	// initialization errors
 	float switch_on_gyro_bias{0.1f};        ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
 	float switch_on_accel_bias{0.2f};       ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
@@ -379,7 +373,14 @@ struct parameters {
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
 #endif // CONFIG_EKF2_SIDESLIP
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range finder fusion
+	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
+
+	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
+				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
+
+	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
 	float rng_hgt_bias_nsd{0.13f};          ///< process noise for range height bias estimation (m/s/sqrt(Hz))
@@ -394,6 +395,14 @@ 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
 
+	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
+
+	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
+	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
+	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
+
+#endif // CONFIG_EKF2_RANGE_FINDER
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// vision position fusion
 	int32_t ev_ctrl{0};
@@ -406,6 +415,8 @@ 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))
+
+	Vector3f ev_pos_body{};                 ///< xyz position of VI-sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	// gravity fusion
@@ -419,6 +430,8 @@ struct parameters {
 	float flow_noise_qual_min{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
 	int32_t flow_qual_min{1};               ///< minimum acceptable quality integer from  the flow sensor
 	float flow_innov_gate{3.0f};            ///< optical flow fusion innovation consistency gate size (STD)
+
+	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
@@ -435,9 +448,6 @@ struct parameters {
 	// 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)
-	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
-	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)
-	Vector3f ev_pos_body{};                 ///< xyz position of VI-sensor focal point in body frame (m)
 
 	// accel bias learning control
 	float acc_bias_lim{0.4f};               ///< maximum accel bias magnitude (m/sec**2)
@@ -617,6 +627,7 @@ union ekf_solution_status_u {
 	uint16_t value;
 };
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 union terrain_fusion_status_u {
 	struct {
 		bool range_finder : 1;  ///< 0 - true if we are fusing range finder data
@@ -624,6 +635,7 @@ union terrain_fusion_status_u {
 	} flags;
 	uint8_t value;
 };
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 // define structure used to communicate information events
 union information_event_status_u {

src/modules/ekf2/EKF/covariance.cpp

@@ -69,16 +69,17 @@ void Ekf::initialiseCovariance()
 	// position
 	P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));
 	P(8,8) = P(7,7);
+	P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
 
+	if (_control_status.flags.gps_hgt) {
+		P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
+	}
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
 		P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
-
-	} else if (_control_status.flags.gps_hgt) {
-		P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
-
-	} else {
-		P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	// gyro bias
 	_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
@@ -453,7 +454,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 			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);
+
+#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);
+#else
+				bool bad_z_rng  = false;
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #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);

src/modules/ekf2/EKF/ekf.cpp

@@ -63,9 +63,11 @@ void Ekf::reset()
 	_state.wind_vel.setZero();
 	_state.quat_nominal.setIdentity();
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
@@ -94,17 +96,13 @@ void Ekf::reset()
 	_time_last_hor_vel_fuse = 0;
 	_time_last_ver_vel_fuse = 0;
 	_time_last_heading_fuse = 0;
-
-	_time_last_flow_terrain_fuse = 0;
 	_time_last_zero_velocity_fuse = 0;
-	_time_last_healthy_rng_data = 0;
 
 	_last_known_pos.setZero();
 
 	_time_acc_bias_check = 0;
 
 	_gps_checks_passed = false;
-
 	_gps_alt_ref = NAN;
 
 	_baro_counter = 0;
@@ -115,7 +113,6 @@ void Ekf::reset()
 	_clip_counter = 0;
 
 	resetEstimatorAidStatus(_aid_src_baro_hgt);
-	resetEstimatorAidStatus(_aid_src_rng_hgt);
 #if defined(CONFIG_EKF2_AIRSPEED)
 	resetEstimatorAidStatus(_aid_src_airspeed);
 #endif // CONFIG_EKF2_AIRSPEED
@@ -152,6 +149,10 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_optical_flow);
 	resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	resetEstimatorAidStatus(_aid_src_rng_hgt);
+#endif // CONFIG_EKF2_RANGE_FINDER
 }
 
 bool Ekf::update()
@@ -179,8 +180,10 @@ bool Ekf::update()
 		// control fusion of observation data
 		controlFusionModes(imu_sample_delayed);
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 		// run a separate filter for terrain estimation
 		runTerrainEstimator(imu_sample_delayed);
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
 							_state.quat_nominal, _state.vel, _state.pos);
@@ -218,8 +221,10 @@ bool Ekf::initialiseFilter()
 	// initialise the state covariance matrix now we have starting values for all the states
 	initialiseCovariance();
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Initialise the terrain estimator
 	initHagl();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	// reset the output predictor state history to match the EKF initial values
 	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);

src/modules/ekf2/EKF/ekf.h

@@ -95,17 +95,41 @@ public:
 	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
 	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// range height
+	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
+	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
+
 	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
 	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
 	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
 
-#if defined(CONFIG_EKF2_AUXVEL)
-	void getAuxVelInnov(float aux_vel_innov[2]) const;
-	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
-	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
-#endif // CONFIG_EKF2_AUXVEL
+	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
+	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
+	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
+
+	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
+	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
+	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
+
+	// terrain estimate
+	bool isTerrainEstimateValid() const;
+
+	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
+
+	// get the estimated terrain vertical position relative to the NED origin
+	float getTerrainVertPos() const { return _terrain_vpos; };
+
+	// get the number of times the vertical terrain position has been reset
+	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
+
+	// get the terrain variance
+	float get_terrain_var() const { return _terrain_var; }
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
+
 	void getFlowInnov(float flow_innov[2]) const;
 	void getFlowInnovVar(float flow_innov_var[2]) const;
 	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
@@ -123,10 +147,15 @@ public:
 	void getTerrainFlowInnovVar(float flow_innov_var[2]) const;
 	void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
 
-	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
 	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_AUXVEL)
+	void getAuxVelInnov(float aux_vel_innov[2]) const;
+	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
+	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
+#endif // CONFIG_EKF2_AUXVEL
+
 	void getHeadingInnov(float &heading_innov) const
 	{
 		if (_control_status.flags.mag_hdg) {
@@ -230,14 +259,6 @@ public:
 	void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
 #endif // CONFIG_EKF2_SIDESLIP
 
-	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
-	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
-	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
-
-	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
-	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
-	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
-
 	void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
 	void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
 	void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
@@ -331,8 +352,6 @@ public:
 		return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
 	}
 
-	bool isTerrainEstimateValid() const;
-
 	bool isYawFinalAlignComplete() const
 	{
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
@@ -343,17 +362,6 @@ public:
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
 	}
 
-	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
-
-	// get the estimated terrain vertical position relative to the NED origin
-	float getTerrainVertPos() const { return _terrain_vpos; };
-
-	// get the number of times the vertical terrain position has been reset
-	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
-
-	// get the terrain variance
-	float get_terrain_var() const { return _terrain_var; }
-
 	// gyro bias (states 10, 11, 12)
 	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
 	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
@@ -454,7 +462,7 @@ public:
 	uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
 	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(); }
 
@@ -470,7 +478,6 @@ public:
 #endif // CONFIG_EKF2_SIDESLIP
 
 	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
-	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
 
 	const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
@@ -540,8 +547,6 @@ private:
 
 	// 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
-	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
 	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};
@@ -552,10 +557,7 @@ private:
 	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
 	uint64_t _time_last_heading_fuse{0};
-
-	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
-	uint64_t _time_last_healthy_rng_data{0};
 
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
@@ -589,8 +591,30 @@ private:
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
 #endif // CONFIG_EKF2_DRAG_FUSION
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	estimator_aid_source1d_s _aid_src_rng_hgt{};
+
+	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
+
 	float _hagl_innov{0.0f};		///< innovation of the last height above terrain measurement (m)
 	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
+	float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
+
+	uint64_t _time_last_healthy_rng_data{0};
+
+	// Terrain height state estimation
+	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
+	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
+	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
+	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
+	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
+
+	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	estimator_aid_source2d_s _aid_src_optical_flow{};
+	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
 
 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
@@ -603,8 +627,11 @@ private:
 	uint64_t _time_good_motion_us{0};	///< last system time that on-ground motion was within limits (uSec)
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
+	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
+	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	estimator_aid_source1d_s _aid_src_baro_hgt{};
-	estimator_aid_source1d_s _aid_src_rng_hgt{};
 #if defined(CONFIG_EKF2_AIRSPEED)
 	estimator_aid_source1d_s _aid_src_airspeed{};
 #endif // CONFIG_EKF2_AIRSPEED
@@ -647,11 +674,6 @@ private:
 	estimator_aid_source2d_s _aid_src_aux_vel{};
 #endif // CONFIG_EKF2_AUXVEL
 
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	estimator_aid_source2d_s _aid_src_optical_flow{};
-	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
 	// variables used for the GPS quality checks
 	Vector3f _gps_pos_deriv_filt{};	///< GPS NED position derivative (m/sec)
 	Vector2f _gps_velNE_filt{};	///< filtered GPS North and East velocity (m/sec)
@@ -678,7 +700,6 @@ private:
 	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)
 
 	// Variables used to control activation of post takeoff functionality
-	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
@@ -696,13 +717,6 @@ private:
 	Vector3f _prev_delta_ang_bias_var{};	///< saved delta angle XYZ bias variances (rad/sec)
 	Vector3f _prev_dvel_bias_var{};		///< saved delta velocity XYZ bias variances (m/sec)**2
 
-	// Terrain height state estimation
-	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
-	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
-	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
-	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
-	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
-
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
 	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
@@ -829,16 +843,19 @@ private:
 	void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
 	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
 
-	// calculate optical flow body angular rate compensation
-	// returns false if bias corrected body rate data is unavailable
-	bool calcOptFlowBodyRateComp();
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// range height
+	void controlRangeHeightFusion();
+	bool isConditionalRangeAidSuitable();
+	void stopRngHgtFusion();
 
-	// initialise the terrain vertical position estimator
+	// terrain vertical position estimator
 	void initHagl();
-
 	void runTerrainEstimator(const imuSample &imu_delayed);
 	void predictHagl(const imuSample &imu_delayed);
 
+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
+
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	void controlHaglRngFusion();
 	void fuseHaglRng();
@@ -848,22 +865,37 @@ private:
 	void stopHaglRngFusion();
 	float getRngVar();
 
+	void controlHaglFakeFusion();
+#endif // CONFIG_EKF2_RANGE_FINDER
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// update the terrain vertical position estimate using an optical flow measurement
-	void controlHaglFlowFusion();
-	void startHaglFlowFusion();
-	void resetHaglFlow();
-	void stopHaglFlowFusion();
-	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
+	// control fusion of optical flow observations
+	void controlOpticalFlowFusion(const imuSample &imu_delayed);
+	void stopFlowFusion();
+
+	void updateOnGroundMotionForOpticalFlowChecks();
+	void resetOnGroundMotionForOpticalFlowChecks();
+
+	// calculate the measurement variance for the optical flow sensor
+	float calcOptFlowMeasVar(const flowSample &flow_sample);
+
+	// calculate optical flow body angular rate compensation
+	// returns false if bias corrected body rate data is unavailable
+	bool calcOptFlowBodyRateComp();
 
 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
 	void fuseOptFlow();
 	float predictFlowRange();
 	Vector2f predictFlowVelBody();
-#endif // CONFIG_EKF2_OPTICAL_FLOW
 
-	void controlHaglFakeFusion();
+	// update the terrain vertical position estimate using an optical flow measurement
+	void controlHaglFlowFusion();
+	void startHaglFlowFusion();
+	void resetHaglFlow();
+	void stopHaglFlowFusion();
+	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
@@ -981,11 +1013,6 @@ private:
 	void stopEvYawFusion();
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	// control fusion of optical flow observations
-	void controlOpticalFlowFusion(const imuSample &imu_delayed);
-	void updateOnGroundMotionForOpticalFlowChecks();
-	void resetOnGroundMotionForOpticalFlowChecks();
-
 	// control fusion of GPS observations
 	void controlGpsFusion(const imuSample &imu_delayed);
 	bool shouldResetGpsFusion() const;
@@ -994,8 +1021,6 @@ private:
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 
-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
-
 	bool magReset();
 	bool haglYawResetReq();
 
@@ -1038,9 +1063,6 @@ private:
 	void checkHeightSensorRefFallback();
 	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
-	void controlRangeHeightFusion();
-
-	bool isConditionalRangeAidSuitable();
 
 	void stopMagFusion();
 	void stopMag3DFusion();
@@ -1050,16 +1072,12 @@ private:
 
 	void stopBaroHgtFusion();
 	void stopGpsHgtFusion();
-	void stopRngHgtFusion();
 
 	void updateGroundEffect();
 
 	// gravity fusion: heuristically enable / disable gravity fusion
 	void controlGravityFusion(const imuSample &imu_delayed);
 
-	// calculate the measurement variance for the optical flow sensor
-	float calcOptFlowMeasVar(const flowSample &flow_sample);
-
 	// initialise the quaternion covariances using rotation vector variances
 	// do not call before quaternion states are initialised
 	void initialiseQuatCovariances(Vector3f &rot_vec_var);
@@ -1075,9 +1093,6 @@ private:
 	void resetWind();
 	void resetWindToZero();
 
-	// check that the range finder data is continuous
-	void updateRangeDataContinuity();
-
 	// Increase the yaw error variance of the quaternions
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
@@ -1116,8 +1131,6 @@ private:
 	void stopGpsPosFusion();
 	void stopGpsVelFusion();
 
-	void stopFlowFusion();
-
 	void resetFakePosFusion();
 	void stopFakePosFusion();
 
@@ -1138,7 +1151,6 @@ 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};

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -202,7 +202,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 	_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);
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;
@@ -564,6 +566,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 	*hagl_min = NAN;
 	*hagl_max = NAN;
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Calculate range finder limits
 	const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
 
@@ -578,6 +581,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		*hagl_min = rangefinder_hagl_min;
 		*hagl_max = rangefinder_hagl_max;
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// Keep within flow AND range sensor limits when exclusively using optical flow
@@ -708,10 +712,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		n_hgt_sources++;
 	}
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
 		hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio);
 		n_hgt_sources++;
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_hgt) {
@@ -732,8 +738,10 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	tas = sqrtf(_aid_src_airspeed.test_ratio);
 #endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// return the terrain height innovation test ratio
 	hagl = sqrtf(_hagl_test_ratio);
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_SIDESLIP)
 	// return the synthetic sideslip innovation test ratio
@@ -744,7 +752,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 // return a bitmask integer that describes which state estimates are valid
 void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
-	ekf_solution_status_u soln_status;
+	ekf_solution_status_u soln_status{};
 	// TODO: Is this accurate enough?
 	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
 	soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
@@ -752,7 +760,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
 	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
+#endif // CONFIG_EKF2_RANGE_FINDER
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
@@ -1016,12 +1026,15 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
 			float height = _terrain_vpos - _state.pos(2);
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
 
-		} else if (_control_status.flags.gnd_effect) {
+		} else
+#endif // CONFIG_EKF2_RANGE_FINDER
+		if (_control_status.flags.gnd_effect) {
 			// Turn off ground effect compensation if it times out
 			if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
 				_control_status.flags.gnd_effect = false;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -49,13 +49,15 @@ EstimatorInterface::~EstimatorInterface()
 	delete _gps_buffer;
 	delete _mag_buffer;
 	delete _baro_buffer;
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	delete _range_buffer;
+#endif // CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_AIRSPEED)
 	delete _airspeed_buffer;
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	delete _flow_buffer;
-#endif // _flow_buffer
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	delete _ext_vision_buffer;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -288,6 +290,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 }
 #endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 {
 	if (!_initialised) {
@@ -323,6 +326,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 		ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
@@ -555,10 +559,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
 	}
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// using range finder
 	if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
 		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	if (_params.gnss_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
@@ -700,9 +706,11 @@ void EstimatorInterface::print_status()
 		printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
 	}
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_range_buffer) {
 		printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	if (_airspeed_buffer) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -64,11 +64,14 @@
 #include "common.h"
 #include "RingBuffer.h"
 #include "imu_down_sampler.hpp"
-#include "range_finder_consistency_check.hpp"
-#include "sensor_range_finder.hpp"
 #include "utils.hpp"
 #include "output_predictor.h"
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+# include "range_finder_consistency_check.hpp"
+# include "sensor_range_finder.hpp"
+#endif // CONFIG_EKF2_RANGE_FINDER
+
 #include <lib/geo/geo.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -94,8 +97,18 @@ public:
 	void setAirspeedData(const airspeedSample &airspeed_sample);
 #endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	void setRangeData(const rangeSample &range_sample);
 
+	// set sensor limitations reported by the rangefinder
+	void set_rangefinder_limits(float min_distance, float max_distance)
+	{
+		_range_sensor.setLimits(min_distance, max_distance);
+	}
+
+	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
+#endif // CONFIG_EKF2_RANGE_FINDER
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// 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);
@@ -163,12 +176,6 @@ public:
 	// set air density used by the multi-rotor specific drag force fusion
 	void set_air_density(float air_density) { _air_density = air_density; }
 
-	// set sensor limitations reported by the rangefinder
-	void set_rangefinder_limits(float min_distance, float max_distance)
-	{
-		_range_sensor.setLimits(min_distance, max_distance);
-	}
-
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos
 	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
 
@@ -250,7 +257,6 @@ public:
 	const uint64_t &time_delayed_us() const { return _time_delayed_us; }
 
 	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
-	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 
 	const bool &global_origin_valid() const { return _NED_origin_initialised; }
 	const MapProjection &global_origin() const { return _pos_ref; }
@@ -298,7 +304,7 @@ 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{};
@@ -308,9 +314,13 @@ protected:
 	extVisionSample _ev_sample_prev{};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	RangeFinderConsistencyCheck _rng_consistency_check;
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	RingBuffer<rangeSample> *_range_buffer{nullptr};
+	uint64_t _time_last_range_buffer_push{0};
 
-	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
+	sensor::SensorRangeFinder _range_sensor{};
+	RangeFinderConsistencyCheck _rng_consistency_check;
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
@@ -323,6 +333,8 @@ protected:
 	float _flow_max_distance{10.f};	///< maximum distance that the optical flow sensor can operate at (m)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
+
 	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
 
@@ -339,8 +351,6 @@ protected:
 	uint64_t _time_last_gps_yaw_buffer_push{0};
 #endif // CONFIG_EKF2_GNSS_YAW
 
-	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio
-
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	RingBuffer<dragSample> *_drag_buffer{nullptr};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
@@ -367,7 +377,7 @@ protected:
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
-	RingBuffer<rangeSample> *_range_buffer{nullptr};
+
 #if defined(CONFIG_EKF2_AIRSPEED)
 	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
 #endif // CONFIG_EKF2_AIRSPEED
@@ -384,7 +394,6 @@ protected:
 	uint64_t _time_last_gps_buffer_push{0};
 	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_gnd_effect_on{0};
 

src/modules/ekf2/EKF/height_control.cpp

@@ -45,7 +45,9 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 
 	controlBaroHeightFusion();
 	controlGnssHeightFusion(_gps_sample_delayed);
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	controlRangeHeightFusion();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	checkHeightSensorRefFallback();
 }
@@ -185,9 +187,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 		checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]};
 	}
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
 		checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_hgt) {

src/modules/ekf2/EKF/mag_control.cpp

@@ -238,9 +238,13 @@ bool Ekf::haglYawResetReq()
 	if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
 		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
 		return above_mag_anomalies;
+#else
+		return true;
+#endif // CONFIG_EKF2_RANGE_FINDER
 	}
 
 	return false;

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -199,6 +199,7 @@ void Ekf::resetHaglRng()
 	_terrain_var = getRngVar();
 	_terrain_vpos_reset_counter++;
 	_time_last_hagl_fuse = _time_delayed_us;
+	_time_last_healthy_rng_data = 0;
 }
 
 void Ekf::stopHaglRngFusion()
@@ -212,6 +213,8 @@ void Ekf::stopHaglRngFusion()
 
 		_hagl_sensor_status.flags.range_finder = false;
 	}
+
+	_time_last_healthy_rng_data = 0;
 }
 
 void Ekf::fuseHaglRng()
@@ -417,11 +420,13 @@ bool Ekf::isTerrainEstimateValid() const
 		return true;
 	}
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
 	// this can only be the case if the main filter does not fuse optical flow
 	if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) {
 		return true;
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	return false;
 }