From 4465c4fbf6379745af58de328d8f4bdd11347a11 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Mar 2021 08:16:48 +1100 Subject: [PATCH] ekf2: Publish and log EKF warning and information events (NEW msg estimator_event_flags) * msg: Add estinator information and warning events message (estimator_event_flags) * ekf2: publish information and warning events * logger: log estimator_event_flags * update ecl submodule to latest Co-authored-by: Daniel Agar --- .ci/Jenkinsfile-hardware | 1 + msg/CMakeLists.txt | 1 + msg/estimator_event_flags.msg | 32 +++++++++++++++ msg/tools/uorb_rtps_message_ids.yaml | 2 + src/lib/ecl | 2 +- src/modules/ekf2/EKF2.cpp | 61 ++++++++++++++++++++++++++++ src/modules/ekf2/EKF2.hpp | 5 +++ src/modules/logger/logged_topics.cpp | 3 +- 8 files changed, 105 insertions(+), 2 deletions(-) create mode 100644 msg/estimator_event_flags.msg diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 4980bc24bc..509b3a01ab 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -1004,6 +1004,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias"' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 388b8ea70e..b66c6e41b5 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -57,6 +57,7 @@ set(msg_files ekf_gps_drift.msg esc_report.msg esc_status.msg + estimator_event_flags.msg estimator_innovations.msg estimator_optical_flow_vel.msg estimator_selector_status.msg diff --git a/msg/estimator_event_flags.msg b/msg/estimator_event_flags.msg new file mode 100644 index 0000000000..677fd9e60c --- /dev/null +++ b/msg/estimator_event_flags.msg @@ -0,0 +1,32 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +# information events +uint32 information_event_changes # number of information event changes +bool gps_checks_passed # 0 - true when gps quality checks are passing passed +bool reset_vel_to_gps # 1 - true when the velocity states are reset to the gps measurement +bool reset_vel_to_flow # 2 - true when the velocity states are reset using the optical flow measurement +bool reset_vel_to_vision # 3 - true when the velocity states are reset to the vision system measurement +bool reset_vel_to_zero # 4 - true when the velocity states are reset to zero +bool reset_pos_to_last_known # 5 - true when the position states are reset to the last known position +bool reset_pos_to_gps # 6 - true when the position states are reset to the gps measurement +bool reset_pos_to_vision # 7 - true when the position states are reset to the vision system measurement +bool starting_gps_fusion # 8 - true when the filter starts using gps measurements to correct the state estimates +bool starting_vision_pos_fusion # 9 - true when the filter starts using vision system position measurements to correct the state estimates +bool starting_vision_vel_fusion # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates +bool starting_vision_yaw_fusion # 11 - true when the filter starts using vision system yaw measurements to correct the state estimates +bool yaw_aligned_to_imu_gps # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data + +# 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 fiter has detected an invalid yaw esitmate 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 magnetomer 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 magnetomer data diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 94655e8b2b..2a1783dcf1 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -331,6 +331,8 @@ rtps: id: 157 - msg: airspeed_wind id: 158 + - msg: estimator_event_flags + id: 159 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 170 diff --git a/src/lib/ecl b/src/lib/ecl index 098d5ce5c0..4bad2a272c 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 098d5ce5c034f630b1b440a02ba78a13d092b8c7 +Subproject commit 4bad2a272cfbcfd2ef2c77bb4af788f5b569aaaa diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 0eaebc4255..96617e2003 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -472,6 +472,7 @@ void EKF2::Run() // publish status/logging messages PublishEkfDriftMetrics(now); + PublishEventFlags(now); PublishStates(now); PublishStatus(now); PublishStatusFlags(now); @@ -540,6 +541,66 @@ void EKF2::PublishEkfDriftMetrics(const hrt_abstime ×tamp) } } +void EKF2::PublishEventFlags(const hrt_abstime ×tamp) +{ + // information events + uint32_t information_events = _ekf.information_event_status().value; + bool information_event_updated = false; + + if (information_events != 0) { + information_event_updated = true; + _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) { + estimator_event_flags_s event_flags{}; + event_flags.timestamp_sample = timestamp; + + event_flags.information_event_changes = _filter_information_event_changes; + event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed; + event_flags.reset_vel_to_gps = _ekf.information_event_flags().reset_vel_to_gps; + event_flags.reset_vel_to_flow = _ekf.information_event_flags().reset_vel_to_flow; + event_flags.reset_vel_to_vision = _ekf.information_event_flags().reset_vel_to_vision; + event_flags.reset_vel_to_zero = _ekf.information_event_flags().reset_vel_to_zero; + event_flags.reset_pos_to_last_known = _ekf.information_event_flags().reset_pos_to_last_known; + event_flags.reset_pos_to_gps = _ekf.information_event_flags().reset_pos_to_gps; + event_flags.reset_pos_to_vision = _ekf.information_event_flags().reset_pos_to_vision; + event_flags.starting_gps_fusion = _ekf.information_event_flags().starting_gps_fusion; + event_flags.starting_vision_pos_fusion = _ekf.information_event_flags().starting_vision_pos_fusion; + event_flags.starting_vision_vel_fusion = _ekf.information_event_flags().starting_vision_vel_fusion; + event_flags.starting_vision_yaw_fusion = _ekf.information_event_flags().starting_vision_yaw_fusion; + event_flags.yaw_aligned_to_imu_gps = _ekf.information_event_flags().yaw_aligned_to_imu_gps; + + 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.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_event_flags_pub.publish(event_flags); + } + + _ekf.clear_information_events(); + _ekf.clear_warning_events(); +} + void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) { if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index bbf6a49059..81045774db 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -128,6 +129,7 @@ private: void PublishAttitude(const hrt_abstime ×tamp); void PublishEkfDriftMetrics(const hrt_abstime ×tamp); + void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); void PublishInnovations(const hrt_abstime ×tamp, const imuSample &imu); void PublishInnovationTestRatios(const hrt_abstime ×tamp); @@ -243,6 +245,8 @@ private: 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)}; uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; @@ -254,6 +258,7 @@ private: uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; + uORB::PublicationMulti _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 29fe3e37ac..541794752c 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -140,12 +140,13 @@ void LoggedTopics::add_default_topics() add_topic("estimator_selector_status"); add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES);