EKF: Add external monitoring of warning and information events

EKF/common.h

@@ -505,9 +505,48 @@ union ekf_solution_status {
 union terrain_fusion_status_u {
 	struct {
 		bool range_finder: 1;	///< 0 - true if we are fusing range finder data
-		bool flow: 1;			///< 1 - true if we are fusing flow data
+		bool flow: 1;		///< 1 - true if we are fusing flow data
 	} flags;
 	uint8_t value;
 };
 
+// define structure used to communicate information events
+union information_event_status_u {
+	struct {
+		bool ekf_tilt_aligned		: 1; ///< 0 - true when the tilt alignment using the sensed gravity vector is complete
+		bool reset_vel_to_gps		: 1; ///< 1 - true when the velocity states are reset to the gps measurement
+		bool reset_vel_to_flow		: 1; ///< 2 - true when the velocity states are reset using the optical flow measurement
+		bool reset_vel_to_vision	: 1; ///< 3 - true when the velocity states are reset to the vision system measurement
+		bool reset_vel_to_zero		: 1; ///<4   - true when the velocity states are reset to zero
+		bool reset_pos_to_last_known	: 1; ///< 5 - true when the position states are reset to the last known position
+		bool reset_pos_to_gps		: 1; ///< 6 - true when the position states are reset to the gps measurement
+		bool reset_pos_to_vision	: 1; ///< 7 - true when the position states are reset to the vision system measurement
+		bool starting_gps_fusion	: 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates
+		bool starting_vision_pos_fusion	: 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates
+		bool starting_vision_vel_fusion	: 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
+		bool starting_vision_yaw_fusion	: 1; ///< 11 - true when the filter starts using vision system yaw  measurements to correct the state estimates
+		bool yaw_aligned_to_imu_gps	: 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
+		bool gps_checks_passed		: 1; ///< 13 - true when gps quality checks are passing passed
+	} flags;
+	uint32_t value;
+};
+
+// define structure used to communicate information events
+union warning_event_status_u {
+	struct {
+		bool gps_quality_poor			: 1; ///< 0 - true when the gps is failing quality checks
+		bool gps_fusion_timout			: 1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period
+		bool gps_data_stopped			: 1; ///< 2 - true when the gps data has stopped for a significant time period
+		bool gps_data_stopped_using_alternate	: 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
+		bool height_sensor_timeout		: 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
+		bool stopping_navigation		: 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
+		bool invalid_accel_bias_cov_reset	: 1; ///< 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
+		bool bad_yaw_using_gps_course		: 1; ///< 7 - true when the fiter has detected an invalid yaw esitmate and has reset the yaw angle to the GPS ground course
+		bool stopping_mag_use			: 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
+		bool vision_data_stopped		: 1; ///< 9 - true when the vision system data has stopped for a significant time period
+		bool emergency_yaw_reset_mag_stopped	: 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
+	} flags;
+	uint32_t value;
+};
+
 }

EKF/control.cpp

@@ -78,6 +78,7 @@ void Ekf::controlFusionModes()
 
 			}
 			if (height_source){
+				_information_events.flags.ekf_tilt_aligned = true;
 				ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
 					(unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
 			}
@@ -336,6 +337,7 @@ void Ekf::controlExternalVisionFusion()
 
 		// Turn off EV fusion mode if no data has been received
 		stopEvFusion();
+		_warning_events.flags.vision_data_stopped = true;
 		ECL_WARN_TIMESTAMPED("vision data stopped");
 
 	}
@@ -552,6 +554,7 @@ void Ekf::controlGpsFusion()
 			if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
 				resetHorizontalPosition();
 			}
+			_warning_events.flags.gps_quality_poor = true;
 			ECL_WARN_TIMESTAMPED("GPS quality poor - stopping use");
 		}
 
@@ -659,6 +662,7 @@ void Ekf::controlGpsFusion()
 				resetVelocity();
 				resetHorizontalPosition();
 				_velpos_reset_request = false;
+				_warning_events.flags.gps_fusion_timout = true;
 				ECL_WARN_TIMESTAMPED("GPS fusion timeout - reset to GPS");
 
 				// Reset the timeout counters
@@ -719,11 +723,13 @@ void Ekf::controlGpsFusion()
 
 	} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
 		stopGpsFusion();
+		_warning_events.flags.gps_data_stopped = true;
 		ECL_WARN_TIMESTAMPED("GPS data stopped");
 	}  else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 		// Handle the case where we are fusing another position source along GPS,
 		// stop waiting for GPS after 1 s of lost signal
 		stopGpsFusion();
+		_warning_events.flags.gps_data_stopped_using_alternate = true;
 		ECL_WARN_TIMESTAMPED("GPS data stopped, using only EV, OF or air data" );
 	}
 }
@@ -879,6 +885,7 @@ void Ekf::controlHeightSensorTimeouts()
 		}
 
 		if (failing_height_source && new_height_source) {
+			_warning_events.flags.height_sensor_timeout = true;
 			ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source);
 		}
 
@@ -1293,6 +1300,7 @@ void Ekf::controlFakePosFusion()
 				_fuse_hpos_as_odom = false;
 
 				if (_time_last_fake_pos != 0) {
+					_warning_events.flags.stopping_navigation = true;
 					ECL_WARN_TIMESTAMPED("stopping navigation");
 				}
 

EKF/covariance.cpp

@@ -971,6 +971,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 			_time_acc_bias_check = _time_last_imu;
 			_fault_status.flags.bad_acc_bias = false;
+			_warning_events.flags.invalid_accel_bias_cov_reset = true;
 			ECL_WARN_TIMESTAMPED("invalid accel bias - covariance reset");
 
 		} else if (force_symmetry) {

EKF/ekf_helper.cpp

@@ -65,6 +65,7 @@ void Ekf::resetVelocity()
 
 void Ekf::resetVelocityToGps()
 {
+	_information_events.flags.reset_vel_to_gps = true;
 	ECL_INFO_TIMESTAMPED("reset velocity to GPS");
 	resetVelocityTo(_gps_sample_delayed.vel);
 	P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
@@ -72,6 +73,7 @@ void Ekf::resetVelocityToGps()
 
 void Ekf::resetHorizontalVelocityToOpticalFlow()
 {
+	_information_events.flags.reset_vel_to_flow = true;
 	ECL_INFO_TIMESTAMPED("reset velocity to flow");
 	// constrain height above ground to be above minimum possible
 	const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
@@ -102,6 +104,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
 
 void Ekf::resetVelocityToVision()
 {
+	_information_events.flags.reset_vel_to_vision = true;
 	ECL_INFO_TIMESTAMPED("reset to vision velocity");
 	resetVelocityTo(getVisionVelocityInEkfFrame());
 	P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame());
@@ -109,6 +112,7 @@ void Ekf::resetVelocityToVision()
 
 void Ekf::resetHorizontalVelocityToZero()
 {
+	_information_events.flags.reset_vel_to_zero = true;
 	ECL_INFO_TIMESTAMPED("reset velocity to zero");
 	// Used when falling back to non-aiding mode of operation
 	resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
@@ -167,6 +171,7 @@ void Ekf::resetHorizontalPosition()
 		resetHorizontalPositionToVision();
 
 	} else if (_control_status.flags.opt_flow) {
+		_information_events.flags.reset_pos_to_last_known = true;
 		ECL_INFO_TIMESTAMPED("reset position to last known position");
 
 		if (!_control_status.flags.in_air) {
@@ -181,6 +186,7 @@ void Ekf::resetHorizontalPosition()
 		P.uncorrelateCovarianceSetVariance<2>(7, 0.0f);
 
 	} else {
+		_information_events.flags.reset_pos_to_last_known = true;
 		ECL_INFO_TIMESTAMPED("reset position to last known position");
 		// Used when falling back to non-aiding mode of operation
 		resetHorizontalPositionTo(_last_known_posNE);
@@ -190,6 +196,7 @@ void Ekf::resetHorizontalPosition()
 
 void Ekf::resetHorizontalPositionToGps()
 {
+	_information_events.flags.reset_pos_to_gps = true;
 	ECL_INFO_TIMESTAMPED("reset position to GPS");
 	resetHorizontalPositionTo(_gps_sample_delayed.pos);
 	P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc));
@@ -197,6 +204,7 @@ void Ekf::resetHorizontalPositionToGps()
 
 void Ekf::resetHorizontalPositionToVision()
 {
+	_information_events.flags.reset_pos_to_vision = true;
 	ECL_INFO_TIMESTAMPED("reset position to ev position");
 	Vector3f _ev_pos = _ev_sample_delayed.pos;
 
@@ -386,11 +394,13 @@ bool Ekf::realignYawGPS()
 
 	// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
 	if (badMagYaw || !_control_status.flags.yaw_align) {
+		_warning_events.flags.bad_yaw_using_gps_course = true;
 		ECL_WARN_TIMESTAMPED("bad yaw, using GPS course");
 
 		// declare the magnetometer as failed if a bad yaw has occurred more than once
 		if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2)
 		    && !_control_status.flags.mag_fault) {
+			_warning_events.flags.stopping_mag_use = true;
 			ECL_WARN_TIMESTAMPED("stopping mag use");
 			_control_status.flags.mag_fault = true;
 		}
@@ -1413,6 +1423,7 @@ void Ekf::startGpsFusion()
 		resetVelocityToGps();
 	}
 
+	_information_events.flags.starting_gps_fusion = true;
 	ECL_INFO_TIMESTAMPED("starting GPS fusion");
 	_control_status.flags.gps = true;
 }
@@ -1458,6 +1469,7 @@ void Ekf::startEvPosFusion()
 {
 	_control_status.flags.ev_pos = true;
 	resetHorizontalPosition();
+	_information_events.flags.starting_vision_pos_fusion = true;
 	ECL_INFO_TIMESTAMPED("starting vision pos fusion");
 }
 
@@ -1465,6 +1477,7 @@ void Ekf::startEvVelFusion()
 {
 	_control_status.flags.ev_vel = true;
 	resetVelocity();
+	_information_events.flags.starting_vision_vel_fusion = true;
 	ECL_INFO_TIMESTAMPED("starting vision vel fusion");
 }
 
@@ -1486,6 +1499,7 @@ void Ekf::startEvYawFusion()
 	stopMagHdgFusion();
 	stopMag3DFusion();
 
+	_information_events.flags.starting_vision_yaw_fusion = true;
 	ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
 }
 
@@ -1605,12 +1619,14 @@ bool Ekf::resetYawToEKFGSF()
 	_control_status.flags.yaw_align = true;
 
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
+		_information_events.flags.yaw_aligned_to_imu_gps = true;
 		ECL_INFO_TIMESTAMPED("Yaw aligned using IMU and GPS");
 
 	} else {
 		// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
 		// and cause another navigation failure
 		_control_status.flags.mag_fault = true;
+		_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
 		ECL_WARN_TIMESTAMPED("Emergency yaw reset - mag use stopped");
 	}
 

EKF/estimator_interface.h

@@ -218,6 +218,12 @@ public:
 	const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; }
 	const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; }
 
+	const warning_event_status_u &warning_event_status() const { return _warning_events; }
+	const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; }
+
+	const information_event_status_u &information_event_status() const { return _information_events; }
+	const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; }
+
 	bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; }
 
 	// Getter for the average imu update period in s
@@ -381,6 +387,9 @@ protected:
 
 	virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0;
 
+	warning_event_status_u _warning_events{};
+	information_event_status_u _information_events{};
+
 private:
 
 	inline void setDragData(const imuSample &imu);

EKF/gps_checks.cpp

@@ -112,6 +112,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 			startGpsHgtFusion();
 		}
 
+		_information_events.flags.gps_checks_passed = true;
 		ECL_INFO_TIMESTAMPED("GPS checks passed");
 
 	} else if (!_NED_origin_initialised) {