From 106482b078ac87ccf9e44330269ac8e02d994e2b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 23 May 2016 14:08:03 +1000
Subject: [PATCH] EKF: add structure to capture innovation test failures and
 state resets

---
 EKF/common.h              | 24 ++++++++++++++++++++++++
 EKF/ekf.cpp               |  2 ++
 EKF/estimator_interface.h |  1 +
 3 files changed, 27 insertions(+)

diff --git a/EKF/common.h b/EKF/common.h
index cad93dc713..d6eb0e9555 100644
--- a/EKF/common.h
+++ b/EKF/common.h
@@ -388,6 +388,30 @@ union fault_status_u {
 
 };
 
+// define structure used to communicate innovation test failures and state resets
+union sensor_fault_status_u {
+	struct {
+		bool reject_vel_NED: 1;		// 0 - true if velocity observations have been rejected
+		bool reject_pos_NE: 1;		// 1 - true if horizontal position observations have been rejected
+		bool reject_pos_D: 1;		// 2 - true if true if vertical position observations have been rejected
+		bool reject_mag_x: 1;		// 3 - true if the X magnetometer observation has been rejected
+		bool reject_mag_y: 1;		// 4 - true if the Y magnetometer observation has been rejected
+		bool reject_mag_z: 1;		// 5 - true if the Z magnetometer observation has been rejected
+		bool reject_yaw: 1;		// 6 - true if the yaw observation has been rejected
+		bool reject_airspeed: 1;	// 7 - true if the airspeed observation has been rejected
+		bool reject_hagl: 1;		// 8 - true if the height above ground observation has been rejected
+		bool reject_optflow_X: 1;	// 9 - true if the X optical flow observation has been rejected
+		bool reject_optflow_Y: 1;	// 10 - true if the Y optical flow observation has been rejected
+		bool reset_pos_NE: 1;		// 11 - true if the horizontal positoin has been reset
+		bool reset_pos_D: 1;		// 12 - true if the vertical position  has been reset
+		bool reset_vel_NE: 1;		// 13 - true if the horizontal velocity has been reset
+		bool reset_vel_D: 1;		// 14 - true if the vertical velocity has been reset
+		bool reset_yaw: 1;		// 15 - true if teh yaw angle has been reset
+	} flags;
+	uint32_t value;
+
+};
+
 // publish the status of various GPS quality checks
 union gps_check_fail_status_u {
 	struct {
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index d98a5aec2d..762c71d175 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -180,6 +180,8 @@ bool Ekf::init(uint64_t timestamp)
 
 	_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
 
+	_sensor_fault_status.value = 0;
+
 	return ret;
 }
 
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index 364eb15882..0babecb623 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -287,6 +287,7 @@ protected:
 	uint64_t _time_last_optflow;
 
 	fault_status_u _fault_status;
+	sensor_fault_status_u _sensor_fault_status;
 
 	// allocate data buffers and intialise interface variables
 	bool initialise_interface(uint64_t timestamp);