diff --git a/EKF/common.h b/EKF/common.h index bb3b8953b8..5bd50a2fbf 100644 --- a/EKF/common.h +++ b/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; +}; + } diff --git a/EKF/control.cpp b/EKF/control.cpp index 4d84a44fbd..6cdb93d227 100644 --- a/EKF/control.cpp +++ b/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"); } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 1e0111611e..5097730393 100644 --- a/EKF/covariance.cpp +++ b/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) { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 352907e2b7..ff43d0b263 100644 --- a/EKF/ekf_helper.cpp +++ b/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"); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 893590fd90..7144acfb0e 100644 --- a/EKF/estimator_interface.h +++ b/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); diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 6a99179618..1527e4d59d 100644 --- a/EKF/gps_checks.cpp +++ b/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) {