From 80cefa3e0014075e6bb8c53a05be128a5993937d Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 23 Mar 2023 18:18:36 +0100 Subject: [PATCH] ekf2: migrate uorb events to events interface --- Tools/px4events/srcparser.py | 2 +- msg/CMakeLists.txt | 1 - msg/EstimatorEventFlags.msg | 37 ---- src/modules/ekf2/EKF2.cpp | 266 ++++++++++++++++++++++----- src/modules/ekf2/EKF2.hpp | 3 - src/modules/logger/logged_topics.cpp | 3 - src/modules/zenoh/Kconfig.topics | 5 - 7 files changed, 223 insertions(+), 94 deletions(-) delete mode 100644 msg/EstimatorEventFlags.msg diff --git a/Tools/px4events/srcparser.py b/Tools/px4events/srcparser.py index 290aded541..3be346207b 100644 --- a/Tools/px4events/srcparser.py +++ b/Tools/px4events/srcparser.py @@ -126,7 +126,7 @@ class SourceParser(object): descr = descr[:i] + ' ' + descr[i+1:] event.description = descr elif tag == "group": - known_groups = ["calibration", "health", "arming_check", "default"] + known_groups = ["calibration", "health", "arming_check", "default", "ekf2"] event.group = value.strip() if not event.group in known_groups: raise Exception("Unknown event group: '{}'\nKnown groups: {}\n" \ diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3b1e2647e3..b6f1dc3cae 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -81,7 +81,6 @@ set(msg_files EstimatorAidSource3d.msg EstimatorBias.msg EstimatorBias3d.msg - EstimatorEventFlags.msg EstimatorGpsStatus.msg EstimatorInnovations.msg EstimatorSelectorStatus.msg diff --git a/msg/EstimatorEventFlags.msg b/msg/EstimatorEventFlags.msg deleted file mode 100644 index 1a47e676a5..0000000000 --- a/msg/EstimatorEventFlags.msg +++ /dev/null @@ -1,37 +0,0 @@ -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 -bool reset_hgt_to_baro # 13 - true when the vertical position state is reset to the baro measurement -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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c75093711e..555f3650ec 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -217,7 +217,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _attitude_pub.advertise(); _local_position_pub.advertise(); - _estimator_event_flags_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); @@ -238,7 +237,6 @@ bool EKF2::multi_init(int imu, int mag) { // advertise all topics to ensure consistent uORB instance numbering _ekf2_timestamps_pub.advertise(); - _estimator_event_flags_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); @@ -1050,55 +1048,235 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) } if (information_event_updated || warning_event_updated) { - estimator_event_flags_s event_flags{}; - event_flags.timestamp_sample = _ekf.time_delayed_us(); + if (_ekf.information_event_flags().gps_checks_passed) { + /* EVENT + * @group ekf2 + * @description + * The GNSS checks enabled by EKF2_GPS_CHECK have passed. + */ + events::send(events::ID("ekf2_gnss_checks_passed"), events::Log::Debug, + "EKF2({1}): GNSS checks passed", _instance); + } - 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.reset_hgt_to_baro = _ekf.information_event_flags().reset_hgt_to_baro; - event_flags.reset_hgt_to_gps = _ekf.information_event_flags().reset_hgt_to_gps; - 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; + if (_ekf.information_event_flags().reset_vel_to_gps) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_vel_to_gnss"), events::Log::Debug, + "EKF2({1}): Reset velocity to GNSS", _instance); + } - 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; + if (_ekf.information_event_flags().reset_vel_to_flow) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_vel_to_flow"), events::Log::Debug, + "EKF2({1}): Reset velocity to optical flow", _instance); + } - event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_event_flags_pub.update(event_flags); + if (_ekf.information_event_flags().reset_vel_to_vision) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_vel_to_vision"), events::Log::Debug, + "EKF2({1}): Reset velocity vision", _instance); + } - _last_event_flags_publish = event_flags.timestamp; + if (_ekf.information_event_flags().reset_vel_to_zero) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_vel_to_zero"), events::Log::Debug, + "EKF2({1}): Reset velocity zero", _instance); + } + + if (_ekf.information_event_flags().reset_pos_to_last_known) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_pos_to_last_known"), events::Log::Debug, + "EKF2({1}): Reset position to last known", _instance); + } + + if (_ekf.information_event_flags().reset_pos_to_gps) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_pos_to_gnss"), events::Log::Debug, + "EKF2({1}): Reset position to GNSS", _instance); + } + + if (_ekf.information_event_flags().reset_pos_to_vision) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_pos_to_vision"), events::Log::Debug, + "EKF2({1}): Reset position to vision", _instance); + } + + if (_ekf.information_event_flags().starting_gps_fusion) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_starting_gnss_fusion"), events::Log::Debug, + "EKF2({1}): Starting GNSS fusion", _instance); + } + + if (_ekf.information_event_flags().starting_vision_pos_fusion) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_starting_vision_pos_fusion"), events::Log::Debug, + "EKF2({1}): Starting vision position fusion", _instance); + } + + if (_ekf.information_event_flags().starting_vision_vel_fusion) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_starting_vision_vel_fusion"), events::Log::Debug, + "EKF2({1}): Starting vision velocity fusion", _instance); + } + + if (_ekf.information_event_flags().starting_vision_yaw_fusion) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_starting_vision_yaw_fusion"), events::Log::Debug, + "EKF2({1}): Starting vision yaw fusion", _instance); + } + + if (_ekf.information_event_flags().yaw_aligned_to_imu_gps) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_yaw_aligned_to_imu_gnss"), events::Log::Debug, + "EKF2({1}): Yaw aligned using IMU and GNSS", _instance); + } + + if (_ekf.information_event_flags().reset_hgt_to_baro) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_hgt_to_baro"), events::Log::Debug, + "EKF2({1}): Reset height to baro", _instance); + } + + if (_ekf.information_event_flags().reset_hgt_to_gps) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_hgt_to_gnss"), events::Log::Debug, + "EKF2({1}): Reset height to GNSS", _instance); + } + + if (_ekf.information_event_flags().reset_hgt_to_rng) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_hgt_to_rng"), events::Log::Debug, + "EKF2({1}): Reset height to range finder", _instance); + } + + if (_ekf.information_event_flags().reset_hgt_to_ev) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_reset_hgt_to_ev"), events::Log::Debug, + "EKF2({1}): Reset height to vision", _instance); + } + + // Warning events + if (_ekf.warning_event_flags().gps_quality_poor) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_gnss_quality_poor"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Poor GNSS quality", _instance); + } + + if (_ekf.warning_event_flags().gps_fusion_timout) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_gnss_fusion_timout"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): GNSS fusion timeout", _instance); + } + + if (_ekf.warning_event_flags().gps_data_stopped) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_gnss_data_stopped"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): GNSS data stopped", _instance); + } + + if (_ekf.warning_event_flags().gps_data_stopped_using_alternate) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_gnss_data_stopped_using_alternate"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): GNSS data stopped, using alternate aiding source", _instance); + } + + if (_ekf.warning_event_flags().height_sensor_timeout) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_height_sensor_timeout"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Height sensor timeout", _instance); + } + + if (_ekf.warning_event_flags().stopping_mag_use) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_stopping_mag_use"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Stopping magnetometer use", _instance); + } + + if (_ekf.warning_event_flags().invalid_accel_bias_cov_reset) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_invalid_accel_bias_cov_reset"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Invalid accelerometer bias, resetting covariance", _instance); + } + + if (_ekf.warning_event_flags().bad_yaw_using_gps_course) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_bad_yaw_using_gnss_course"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Bad yaw, reset to GNSS COG", _instance); + } + + if (_ekf.warning_event_flags().vision_data_stopped) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_vision_data_stopped"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Vision data stopped", _instance); + } + + if (_ekf.warning_event_flags().emergency_yaw_reset_mag_stopped) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_emergency_yaw_reset_mag_stopped"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Emergency yaw reset, stopping magnetometer use", _instance); + } + + if (_ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped) { + /* EVENT + * @group ekf2 + */ + events::send(events::ID("ekf2_emergency_yaw_reset_gnss_yaw_stopped"), {events::Log::Debug, events::LogInternal::Warning}, + "EKF2({1}): Emergency yaw reset, stopping GNSS yaw use", _instance); + } _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 - _estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_event_flags_pub.update(); - _last_event_flags_publish = _estimator_event_flags_pub.get().timestamp; } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 1d54315b8c..186f9cac25 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include #include @@ -421,7 +420,6 @@ private: bool _callback_registered{false}; - hrt_abstime _last_event_flags_publish{0}; hrt_abstime _last_status_flags_publish{0}; uint64_t _filter_control_status{0}; @@ -435,7 +433,6 @@ private: uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 68b3d27b42..e995664bae 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -163,7 +163,6 @@ void LoggedTopics::add_default_topics() add_topic("estimator_gnss_hgt_bias", 500); add_topic("estimator_rng_hgt_bias", 500); add_topic("estimator_ev_pos_bias", 500); - add_topic("estimator_event_flags", 0); add_topic("estimator_gps_status", 1000); add_topic("estimator_innovation_test_ratios", 500); add_topic("estimator_innovation_variances", 500); @@ -179,7 +178,6 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); @@ -266,7 +264,6 @@ void LoggedTopics::add_default_topics() add_topic("estimator_gnss_hgt_bias"); add_topic("estimator_rng_hgt_bias"); add_topic("estimator_ev_pos_bias"); - add_topic("estimator_event_flags"); add_topic("estimator_gps_status"); add_topic("estimator_innovation_test_ratios"); add_topic("estimator_innovation_variances"); diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 14866f5ebf..71f693edb0 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -177,10 +177,6 @@ menu "Zenoh publishers/subscribers" bool "estimator_bias3d" default n - config ZENOH_PUBSUB_ESTIMATOR_EVENT_FLAGS - bool "estimator_event_flags" - default n - config ZENOH_PUBSUB_ESTIMATOR_GPS_STATUS bool "estimator_gps_status" default n @@ -874,7 +870,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE3D select ZENOH_PUBSUB_ESTIMATOR_BIAS select ZENOH_PUBSUB_ESTIMATOR_BIAS3D - select ZENOH_PUBSUB_ESTIMATOR_EVENT_FLAGS select ZENOH_PUBSUB_ESTIMATOR_GPS_STATUS select ZENOH_PUBSUB_ESTIMATOR_INNOVATIONS select ZENOH_PUBSUB_ESTIMATOR_SELECTOR_STATUS