EKF: add structure to capture innovation test failures and state resets

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 {

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;
 }
 

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);