PreFlightChecker: use setter for the innovations to check instead of …

…sending booleans in the update function

This makes it more scalable as more checks will be added

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -38,61 +38,50 @@
 
 #include "PreFlightChecker.hpp"
 
-void PreFlightChecker::update(const float dt,
-			      const bool is_ne_aiding,
-			      const bool is_flow_aiding,
-			      const vehicle_status_s &vehicle_status,
-			      const ekf2_innovations_s &innov)
+void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov)
 {
 	const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv);
 
-	_has_heading_failed = preFlightCheckHeadingFailed(is_ne_aiding, vehicle_status, innov, alpha);
-	_has_horiz_vel_failed = preFlightCheckHorizVelFailed(is_ne_aiding, is_flow_aiding, innov, alpha);
+	_has_heading_failed = preFlightCheckHeadingFailed(innov, alpha);
+	_has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha);
 	_has_down_vel_failed = preFlightCheckDownVelFailed(innov, alpha);
 	_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
 }
 
-bool PreFlightChecker::preFlightCheckHeadingFailed(const bool is_ne_aiding,
-		const vehicle_status_s &vehicle_status,
-		const ekf2_innovations_s &innov,
-		const float alpha)
+bool PreFlightChecker::preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, const float alpha)
 {
-	const float heading_test_limit = selectHeadingTestLimit(is_ne_aiding, vehicle_status);
+	const float heading_test_limit = selectHeadingTestLimit();
 	const float heading_innov_spike_lim = 2.0f * heading_test_limit;
 
 	const float heading_innov_lpf = _filter_heading_innov.update(innov.heading_innov, alpha, heading_innov_spike_lim);
 
 	return checkInnovFailed(innov.heading_innov, heading_innov_lpf, heading_test_limit);
 }
 
-float PreFlightChecker::selectHeadingTestLimit(const bool is_ne_aiding,
-		const vehicle_status_s &vehicle_status)
+float PreFlightChecker::selectHeadingTestLimit()
 {
 	// Select the max allowed heading innovaton depending on whether we are not aiding navigation using
 	// observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion).
-	const bool cannot_realign_in_flight = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+	const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding;
 
-	return (is_ne_aiding && cannot_realign_in_flight)
-	       ? _nav_heading_innov_test_lim
-	       : _heading_innov_test_lim;
+	return (is_ne_aiding && !_can_observe_heading_in_flight)
+	       ? _nav_heading_innov_test_lim // more restrictive test limit
+	       : _heading_innov_test_lim; // less restrictive test limit
 }
 
-bool PreFlightChecker::preFlightCheckHorizVelFailed(const bool is_ne_aiding,
-		const bool is_flow_aiding,
-		const ekf2_innovations_s &innov,
-		const float alpha)
+bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, const float alpha)
 {
 	bool has_failed = false;
 
-	if (is_ne_aiding) {
+	if (_is_using_gps_aiding || _is_using_ev_pos_aiding) {
 		const Vector2f vel_ne_innov = Vector2f(innov.vel_pos_innov);
 		Vector2f vel_ne_innov_lpf;
 		vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
 		vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
 		has_failed |= checkInnov2DFailed(vel_ne_innov, vel_ne_innov_lpf, _vel_innov_test_lim);
 	}
 
-	if (is_flow_aiding) {
+	if (_is_using_flow_aiding) {
 		const Vector2f flow_innov = Vector2f(innov.flow_innov);
 		Vector2f flow_innov_lpf;
 		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
@@ -136,6 +125,9 @@ uint8_t PreFlightChecker::prefltFailBoolToBitMask(const bool heading_failed, con
 
 void PreFlightChecker::reset()
 {
+	_is_using_gps_aiding = false;
+	_is_using_flow_aiding = false;
+	_is_using_ev_pos_aiding = false;
 	_has_heading_failed = false;
 	_has_horiz_vel_failed = false;
 	_has_down_vel_failed = false;

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -64,13 +64,18 @@ class PreFlightChecker
 	/*
 	 * Update the internal states
 	 * @param dt the sampling time
-	 * @param is_ne_aiding true if the EKF is currently fusing NE vel and pos
-	 * @param is_flow_aiding true if the EKF is currently fusing optical flow
-	 * @param vehicle_status the vehicle_status_s struct containing the status flags
 	 * @param innov the ekf2_innovation_s struct containing the current innovations
 	 */
-	void update(float dt, bool is_ne_aiding, bool is_flow_aiding, const vehicle_status_s &vehicle_status,
-		    const ekf2_innovations_s &innov);
+	void update(float dt, const ekf2_innovations_s &innov);
+
+	/*
+	 * If set to true, the checker will use a less conservative heading innovation check
+	 */
+	void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; }
+
+	void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
+	void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
+	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
 
 	bool hasHeadingFailed() const { return _has_heading_failed; }
 	bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
@@ -141,23 +146,13 @@ class PreFlightChecker
 	static uint8_t prefltFailBoolToBitMask(bool heading_failed, bool horiz_vel_failed, bool down_vel_failed,
 					       bool height_failed);
 
-	/*
-	 * Select the appropriate heading test limit depending on the type of aiding and
-	 * the ability of the vehicle to observe heading in flight
-	 * @param is_ne_aiding true if the vehicle is relying on heading for vel/pos fusion
-	 * @param vehicle_status the vehicle_status_s containing the status flags
-	 * @return the appropriate heading test limit
-	 */
-	static float selectHeadingTestLimit(bool is_ne_aiding, const vehicle_status_s &vehicle_status);
 	static constexpr float sq(float var) { return var * var; }
 
 private:
-	bool preFlightCheckHeadingFailed(bool is_ne_aiding,
-					 const vehicle_status_s &vehicle_status,
-					 const ekf2_innovations_s &innov,
-					 float alpha);
-	bool preFlightCheckHorizVelFailed(bool is_ne_aiding, bool is_flow_aiding, const ekf2_innovations_s &innov,
-					  float alpha);
+	bool preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, float alpha);
+	float selectHeadingTestLimit();
+
+	bool preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, float alpha);
 	bool preFlightCheckDownVelFailed(const ekf2_innovations_s &innov, float alpha);
 	bool preFlightCheckHeightFailed(const ekf2_innovations_s &innov, float alpha);
 
@@ -168,6 +163,11 @@ class PreFlightChecker
 	bool _has_down_vel_failed{};
 	bool _has_height_failed{};
 
+	bool _can_observe_heading_in_flight{};
+	bool _is_using_gps_aiding{};
+	bool _is_using_flow_aiding{};
+	bool _is_using_ev_pos_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)

src/modules/ekf2/ekf2_main.cpp

@@ -1691,9 +1691,14 @@ void Ekf2::runPreFlightChecks(const float dt,
 			      const vehicle_status_s &vehicle_status,
 			      const ekf2_innovations_s &innov)
 {
-	const bool is_ne_aiding = control_status.flags.gps ||  control_status.flags.ev_pos;
-	const bool is_flow_aiding = control_status.flags.opt_flow;
-	_preflt_checker.update(dt, is_ne_aiding, is_flow_aiding, vehicle_status, innov);
+	const bool can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+
+	_preflt_checker.setVehicleCanObserveHeadingInFlight(can_observe_heading_in_flight);
+	_preflt_checker.setUsingGpsAiding(control_status.flags.gps);
+	_preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow);
+	_preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos);
+
+	_preflt_checker.update(dt, innov);
 }
 
 void Ekf2::resetPreFlightChecks()