EKF: publish ekf solution status summary data

EKF/common.h

@@ -445,4 +445,21 @@ union filter_control_status_u {
 	uint16_t value;
 };
 
+union ekf_solution_status {
+    struct {
+	uint16_t attitude           : 1; // 0 - True if the attitude estimate is good
+	uint16_t velocity_horiz     : 1; // 1 - True if the horizontal velocity estimate is good
+	uint16_t velocity_vert      : 1; // 2 - True if the vertical velocity estimate is good
+	uint16_t pos_horiz_rel      : 1; // 3 - True if the horizontal position (relative) estimate is good
+	uint16_t pos_horiz_abs      : 1; // 4 - True if the horizontal position (absolute) estimate is good
+	uint16_t pos_vert_abs       : 1; // 5 - True if the vertical position (absolute) estimate is good
+	uint16_t pos_vert_agl       : 1; // 6 - True if the vertical position (above ground) estimate is good
+	uint16_t const_pos_mode     : 1; // 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
+	uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
+	uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
+	uint16_t gps_glitch         : 1; // 10 - True if the EKF has detected a GPS glitch
+    } flags;
+    uint16_t value;
+};
+
 }

EKF/ekf.h

@@ -169,6 +169,9 @@ public:
 	// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
 	void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl);
 
+	// return a bitmask integer that describes which state estimates can be used for flight control
+	void get_ekf_soln_status(uint16_t *status);
+
 private:
 
 	static const uint8_t _k_num_states = 24;

EKF/ekf_helper.cpp

@@ -731,6 +731,28 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f
 	*hagl = sqrt(_terr_test_ratio);
 }
 
+// return a bitmask integer that describes which state estimates are valid
+void Ekf::get_ekf_soln_status(uint16_t *status)
+{
+	ekf_solution_status soln_status;
+	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
+	soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
+	soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
+	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
+	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
+	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
+	float dummy_var;
+	soln_status.flags.pos_vert_agl = get_terrain_vert_pos(&dummy_var);
+	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
+	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
+	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_vert_abs;
+	bool gps_vel_innov_bad = (_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f);
+	bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f);
+	bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
+	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
+	*status = soln_status.value;
+}
+
 // fuse measurement
 void Ekf::fuse(float *K, float innovation)
 {

EKF/estimator_interface.h

@@ -249,6 +249,8 @@ public:
 	// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
 	virtual void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl) = 0;
 
+	// return a bitmask integer that describes which state estimates can be used for flight control
+	virtual void get_ekf_soln_status(uint16_t *status) = 0;
 
 protected: