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