From 1af2f121521da3d9a41dfbdf90492957c6d24c88 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 | 22 ---- src/modules/ekf2/EKF2.cpp | 168 ++++++++++++++++++++++----- src/modules/ekf2/EKF2.hpp | 5 +- src/modules/logger/logged_topics.cpp | 1 - src/modules/zenoh/Kconfig.topics | 5 - 7 files changed, 138 insertions(+), 66 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 c87e75fb69..f6bb265279 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 7d5cf7ca34..0000000000 --- a/msg/EstimatorEventFlags.msg +++ /dev/null @@ -1,22 +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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 93c9ed617a..115ce80b1f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -230,7 +230,6 @@ void EKF2::AdvertiseTopics() // advertise expected minimal topic set immediately for logging _attitude_pub.advertise(); _local_position_pub.advertise(); - _estimator_event_flags_pub.advertise(); _estimator_sensor_bias_pub.advertise(); _estimator_status_pub.advertise(); _estimator_status_flags_pub.advertise(); @@ -794,7 +793,7 @@ void EKF2::Run() #endif // CONFIG_EKF2_WIND // publish status/logging messages - PublishEventFlags(now); + SendEvents(); PublishStatus(now); PublishStatusFlags(now); @@ -1114,7 +1113,7 @@ estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status, return bias; } -void EKF2::PublishEventFlags(const hrt_abstime ×tamp) +void EKF2::SendEvents() { // information events uint32_t information_events = _ekf.information_event_status().value; @@ -1126,40 +1125,145 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) } if (information_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.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_event_flags_pub.update(event_flags); + 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); + } - _last_event_flags_publish = event_flags.timestamp; + 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); + } + + 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); + } _ekf.clear_information_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 0bec478e0d..8334bae9e2 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include #include @@ -183,7 +182,7 @@ private: #endif // CONFIG_EKF2_EXTERNAL_VISION estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, uint64_t timestamp, uint32_t device_id = 0); - void PublishEventFlags(const hrt_abstime ×tamp); + void SendEvents(); void PublishGlobalPosition(const hrt_abstime ×tamp); void PublishInnovations(const hrt_abstime ×tamp); void PublishInnovationTestRatios(const hrt_abstime ×tamp); @@ -408,7 +407,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}; @@ -421,7 +419,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 3ff1c321e7..bbc0fa5e8b 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -173,7 +173,6 @@ void LoggedTopics::add_default_topics() // important EKF topics (higher rate) add_optional_topic("estimator_selector_status", 10); - add_optional_topic_multi("estimator_event_flags", 10); add_optional_topic_multi("estimator_optical_flow_vel", 200); add_optional_topic_multi("estimator_sensor_bias", 1000); add_optional_topic_multi("estimator_status", 200); diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 585831cd2f..110e97c548 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 @@ -958,7 +954,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