From 89ab6a5dbfc6740631894b6a05802ff439a2c35f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 1 Nov 2020 09:23:19 -0500
Subject: [PATCH] ekf2: preflight checks only reset on STANDBY change

 - avoid storing unnecessary state and call setVehicleCanObserveHeadingInFlight() directly
---
 src/modules/ekf2/EKF2.cpp | 47 ++++++++++++++++-----------------------
 src/modules/ekf2/EKF2.hpp |  8 ++-----
 2 files changed, 21 insertions(+), 34 deletions(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 79ef1eb705..c0dabf9a38 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -350,9 +350,7 @@ void EKF2::Run()
 			vehicle_status_s vehicle_status;
 
 			if (_status_sub.copy(&vehicle_status)) {
-
 				const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-				_can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
 
 				// only fuse synthetic sideslip measurements if conditions are met
 				_ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
@@ -360,8 +358,20 @@ void EKF2::Run()
 				// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
 				_ekf.set_is_fixed_wing(is_fixed_wing);
 
+				_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
+						vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+
 				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-				_standby = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
+
+				// update standby (arming state) flag
+				const bool standby = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
+
+				if (_standby != standby) {
+					_standby = standby;
+
+					// reset preflight checks if transitioning in or out of standby arming state
+					_preflt_checker.reset();
+				}
 			}
 		}
 
@@ -370,7 +380,6 @@ void EKF2::Run()
 
 			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
 				_ekf.set_in_air_status(!vehicle_land_detected.landed);
-				_landed = vehicle_land_detected.landed;
 				_in_ground_effect = vehicle_land_detected.in_ground_effect;
 			}
 		}
@@ -450,25 +459,6 @@ void EKF2::Run()
 	}
 }
 
-void EKF2::runPreFlightChecks(const float dt,
-			      const filter_control_status_u &control_status,
-			      const estimator_innovations_s &innov,
-			      const bool can_observe_heading_in_flight)
-{
-	_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.setUsingEvVelAiding(control_status.flags.ev_vel);
-
-	_preflt_checker.update(dt, innov);
-}
-
-void EKF2::resetPreFlightChecks()
-{
-	_preflt_checker.reset();
-}
-
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 {
 	if (_ekf.attitude_valid()) {
@@ -590,15 +580,16 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu
 
 	// calculate noise filtered velocity innovations which are used for pre-flight checking
 	if (_standby) {
-
+		// TODO: move to run before publications
 		filter_control_status_u control_status;
 		_ekf.get_control_mode(&control_status.value);
 
-		float dt_seconds = imu.delta_ang_dt;
-		runPreFlightChecks(dt_seconds, control_status, innovations, _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.setUsingEvVelAiding(control_status.flags.ev_vel);
 
-	} else {
-		resetPreFlightChecks();
+		_preflt_checker.update(imu.delta_ang_dt, innovations);
 	}
 }
 
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index ef56a8d1d9..c6a1fe1bc7 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -120,11 +120,6 @@ public:
 private:
 	void Run() override;
 
-	PreFlightChecker _preflt_checker;
-	void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
-				const estimator_innovations_s &innov, const bool can_observe_heading_in_flight);
-	void resetPreFlightChecks();
-
 	template<typename Param>
 	void update_mag_bias(Param &mag_bias_param, int axis_index);
 
@@ -234,7 +229,6 @@ private:
 	bool _standby{false}; // standby arming state
 	bool _landed{true};
 	bool _in_ground_effect{false};
-	bool _can_observe_heading_in_flight{false};
 
 	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::PublicationMulti<ekf_gps_drift_s>              _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
@@ -255,6 +249,8 @@ private:
 	uORB::PublicationMulti<vehicle_global_position_s>    _global_position_pub;
 	uORB::PublicationMulti<vehicle_odometry_s>           _odometry_pub;
 
+	PreFlightChecker _preflt_checker;
+
 	Ekf _ekf;
 
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)