Merge pull request #98 from priseborough/pr-ekf2StatusReporting

EKF: improve reporting of filter status

EKF/ekf.h

@@ -123,6 +123,9 @@ public:
 
 	void get_accel_bias(float *bias) {*bias = _state.accel_z_bias;}
 
+	// get GPS check status
+	void get_gps_check_status(uint16_t *_gps_check_fail_status);
+
 private:
 
 	static const uint8_t _k_num_states = 24;

EKF/ekf_helper.cpp

@@ -316,6 +316,12 @@ void Ekf::get_heading_innov_var(float *heading_innov_var)
 	memcpy(heading_innov_var, &_heading_innov_var, sizeof(float));
 }
 
+// get GPS check status
+void Ekf::get_gps_check_status(uint16_t *val)
+{
+	*val = _gps_check_fail_status.value;
+}
+
 // get the state vector at the delayed time horizon
 void Ekf::get_state_delayed(float *state)
 {

EKF/estimator_interface.cpp

@@ -366,5 +366,5 @@ void EstimatorInterface::unallocate_buffers()
 bool EstimatorInterface::local_position_is_valid()
 {
 	// return true if the position estimate is valid
-	return ((_time_last_imu - _time_last_optflow) < 5e6) || global_position_is_valid();
+	return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) || global_position_is_valid();
 }

EKF/estimator_interface.h

@@ -211,6 +211,15 @@ public:
 
 	virtual void get_accel_bias(float *bias) = 0;
 
+	// get EKF mode status
+	void get_control_mode(uint16_t *val)
+	{
+		*val = _control_status.value;
+	}
+
+	// get GPS check status
+	virtual void get_gps_check_status(uint16_t *val) = 0;
+
 protected:
 
 	parameters _params;		// filter parameters