ekf2 preflight: only check innovation of active height sources

src/modules/ekf2/EKF2.cpp

@@ -877,6 +877,11 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 		_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
 		_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
 
+		_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
+		_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
+		_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
+		_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
+
 		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
 
 	} else if (_ekf.control_status_flags().in_air != _ekf.control_status_prev_flags().in_air) {

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -102,10 +102,29 @@ bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s
 
 bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
 {
-	const float hgt_innov = fmaxf(fabsf(innov.gps_vpos), fmaxf(fabs(innov.ev_vpos),
-				      fabs(innov.rng_vpos)));    // only temporary solution
-	const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
-	return checkInnovFailed(hgt_innov_lpf, hgt_innov, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	bool has_failed = false;
+
+	if (_is_using_baro_hgt_aiding) {
+		const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	if (_is_using_gps_hgt_aiding) {
+		const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	if (_is_using_rng_hgt_aiding) {
+		const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	if (_is_using_ev_hgt_aiding) {
+		const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	return has_failed;
 }
 
 bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
@@ -127,6 +146,10 @@ void PreFlightChecker::reset()
 	_is_using_flow_aiding = false;
 	_is_using_ev_pos_aiding = false;
 	_is_using_ev_vel_aiding = false;
+	_is_using_baro_hgt_aiding = false;
+	_is_using_gps_hgt_aiding = false;
+	_is_using_rng_hgt_aiding = false;
+	_is_using_ev_hgt_aiding = false;
 	_has_heading_failed = false;
 	_has_horiz_vel_failed = false;
 	_has_vert_vel_failed = false;
@@ -134,7 +157,10 @@ void PreFlightChecker::reset()
 	_filter_vel_n_innov.reset();
 	_filter_vel_e_innov.reset();
 	_filter_vel_d_innov.reset();
-	_filter_hgt_innov.reset();
+	_filter_baro_hgt_innov.reset();
+	_filter_gps_hgt_innov.reset();
+	_filter_rng_hgt_innov.reset();
+	_filter_ev_hgt_innov.reset();
 	_filter_heading_innov.reset();
 	_filter_flow_x_innov.reset();
 	_filter_flow_y_innov.reset();

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -79,6 +79,11 @@ public:
 	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
 	void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
 
+	void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
+	void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
+	void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
+	void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
+
 	bool hasHeadingFailed() const { return _has_heading_failed; }
 	bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
 	bool hasVertVelFailed() const { return _has_vert_vel_failed; }
@@ -149,15 +154,25 @@ private:
 	bool _is_using_ev_pos_aiding{};
 	bool _is_using_ev_vel_aiding{};
 
+	bool _is_using_baro_hgt_aiding{};
+	bool _is_using_gps_hgt_aiding{};
+	bool _is_using_rng_hgt_aiding{};
+	bool _is_using_ev_hgt_aiding{};
+
 	// Low-pass filters for innovation pre-flight checks
 	InnovationLpf _filter_vel_n_innov;	///< Preflight low pass filter N axis velocity innovations (m/sec)
 	InnovationLpf _filter_vel_e_innov;	///< Preflight low pass filter E axis velocity innovations (m/sec)
 	InnovationLpf _filter_vel_d_innov;	///< Preflight low pass filter D axis velocity innovations (m/sec)
-	InnovationLpf _filter_hgt_innov;	///< Preflight low pass filter height innovation (m)
 	InnovationLpf _filter_heading_innov;	///< Preflight low pass filter heading innovation magntitude (rad)
 	InnovationLpf _filter_flow_x_innov;	///< Preflight low pass filter optical flow innovation (rad)
 	InnovationLpf _filter_flow_y_innov;	///< Preflight low pass filter optical flow innovation (rad)
 
+	// Preflight low pass filter height innovation (m)
+	InnovationLpf _filter_baro_hgt_innov;
+	InnovationLpf _filter_gps_hgt_innov;
+	InnovationLpf _filter_rng_hgt_innov;
+	InnovationLpf _filter_ev_hgt_innov;
+
 	// Preflight low pass filter time constant inverse (1/sec)
 	static constexpr float _innov_lpf_tau_inv = 0.2f;
 	// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)