diff --git a/msg/EstimatorEventFlags.msg b/msg/EstimatorEventFlags.msg index 1a47e676a5a4..7d5cf7ca343c 100644 --- a/msg/EstimatorEventFlags.msg +++ b/msg/EstimatorEventFlags.msg @@ -20,18 +20,3 @@ bool reset_hgt_to_baro # 13 - true when the vertical position s bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement - -# warning events -uint32 warning_event_changes # number of warning event changes -bool gps_quality_poor # 0 - true when the gps is failing quality checks -bool gps_fusion_timout # 1 - true when the gps data has not been used to correct the state estimates for a significant time period -bool gps_data_stopped # 2 - true when the gps data has stopped for a significant time period -bool gps_data_stopped_using_alternate # 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 # 4 - true when the height sensor has not been used to correct the state estimates for a significant time period -bool stopping_navigation # 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 # 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 # 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course -bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data -bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period -bool emergency_yaw_reset_mag_stopped # 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 magnetometer data -bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 43f61a79d057..78cc2bd30a11 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -83,7 +83,6 @@ void Ekf::controlExternalVisionFusion() _ev_q_error_initialized = false; - _warning_events.flags.vision_data_stopped = true; ECL_WARN("vision data stopped"); } } diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index 1ba1dd309b02..3704434d5fde 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -109,7 +109,6 @@ void Ekf::controlFakePosFusion() if (_control_status.flags.tilt_align) { // The fake position fusion is not started for initial alignement - _warning_events.flags.stopping_navigation = true; ECL_WARN("stopping navigation"); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 0c8c98ab26b6..d5bca4f46ac3 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -78,7 +78,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) { stopGpsFusion(); - _warning_events.flags.gps_quality_poor = true; ECL_WARN("GPS quality poor - stopping use"); } } @@ -92,7 +91,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } else if (_control_status.flags.gps) { if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { stopGpsFusion(); - _warning_events.flags.gps_data_stopped = true; ECL_WARN("GPS data stopped"); } } @@ -276,14 +274,12 @@ bool Ekf::tryYawEmergencyReset() // stop using the magnetometer in the main EKF otherwise its 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; } #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { _control_status.flags.gps_yaw_fault = true; - _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; } #endif // CONFIG_EKF2_GNSS_YAW diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 7a008783fe1b..05610bca5858 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -666,34 +666,5 @@ bool reset_pos_to_ext_obs : 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 estimates and has reset the corresponding covariance matrix elements -bool bad_yaw_using_gps_course : - 1; ///< 7 - true when the filter has detected an invalid yaw estimate 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 magnetometer 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 magnetometer data -bool emergency_yaw_reset_gps_yaw_stopped: - 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data - } flags; - uint32_t value; -}; - } #endif // !EKF_COMMON_H diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e7bef6bb0931..4477f17b5164 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -293,10 +293,6 @@ class EstimatorInterface 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; } - void clear_warning_events() { _warning_events.value = 0; } - 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; } void clear_information_events() { _information_events.value = 0; } @@ -473,7 +469,6 @@ class EstimatorInterface // these are used to record single frame events for external monitoring and should NOT be used for // state logic becasue they will be cleared externally after being read. - warning_event_status_u _warning_events{}; information_event_status_u _information_events{}; unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index a7e0b00f541d..5ad5d7db34dd 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -216,7 +216,6 @@ void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed) _time_acc_bias_check = imu_delayed.time_us; _fault_status.flags.bad_acc_bias = false; - _warning_events.flags.invalid_accel_bias_cov_reset = true; ECL_WARN("invalid accel bias - covariance reset"); } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4fa40399b369..cee124db0c25 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1107,16 +1107,7 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) _filter_information_event_changes++; } - // warning events - uint32_t warning_events = _ekf.warning_event_status().value; - bool warning_event_updated = false; - - if (warning_events != 0) { - warning_event_updated = true; - _filter_warning_event_changes++; - } - - if (information_event_updated || warning_event_updated) { + if (information_event_updated) { estimator_event_flags_s event_flags{}; event_flags.timestamp_sample = _ekf.time_delayed_us(); @@ -1139,27 +1130,12 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) event_flags.reset_hgt_to_rng = _ekf.information_event_flags().reset_hgt_to_rng; event_flags.reset_hgt_to_ev = _ekf.information_event_flags().reset_hgt_to_ev; - event_flags.warning_event_changes = _filter_warning_event_changes; - event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor; - event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout; - event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped; - event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate; - event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout; - event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use; - event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset; - event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course; - event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use; - event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped; - event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped; - event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped; - event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_event_flags_pub.update(event_flags); _last_event_flags_publish = event_flags.timestamp; _ekf.clear_information_events(); - _ekf.clear_warning_events(); } else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) { // continue publishing periodically @@ -2676,8 +2652,7 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) && (_ekf.fault_status().value == 0) && !_ekf.fault_status_flags().bad_acc_bias && !_ekf.fault_status_flags().bad_acc_clipping - && !_ekf.fault_status_flags().bad_acc_vertical - && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset; + && !_ekf.fault_status_flags().bad_acc_vertical; const bool learning_valid = bias_valid && !_ekf.accel_bias_inhibited(); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 3bc2b99f953d..8b832d60ccd8 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -418,7 +418,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uint32_t _filter_control_status_changes{0}; uint32_t _filter_fault_status_changes{0}; uint32_t _innov_check_fail_status_changes{0}; - uint32_t _filter_warning_event_changes{0}; uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};