ekf2: migrate uorb events to events interface

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" \

msg/CMakeLists.txt

@@ -81,7 +81,6 @@ set(msg_files
 	EstimatorAidSource3d.msg
 	EstimatorBias.msg
 	EstimatorBias3d.msg
-	EstimatorEventFlags.msg
 	EstimatorGpsStatus.msg
 	EstimatorInnovations.msg
 	EstimatorSelectorStatus.msg

msg/EstimatorEventFlags.msg

@@ -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

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 &timestamp)
 	}
 
 	if (information_event_updated || warning_event_updated) {
-		estimator_event_flags_s event_flags{};
+		if (_ekf.information_event_flags().gps_checks_passed) {
-		event_flags.timestamp_sample = _ekf.time_delayed_us();
+			/* EVENT
+			 * @group ekf2
+			 * @description
+			 * The GNSS checks enabled by <param>EKF2_GPS_CHECK</param> have passed.
+			 */
+			events::send<int32_t>(events::ID("ekf2_gnss_checks_passed"), events::Log::Debug,
+					      "EKF2({1}): GNSS checks passed", _instance);
+		}
 
-		event_flags.information_event_changes           = _filter_information_event_changes;
+		if (_ekf.information_event_flags().reset_vel_to_gps) {
-		event_flags.gps_checks_passed                   = _ekf.information_event_flags().gps_checks_passed;
+			/* EVENT
-		event_flags.reset_vel_to_gps                    = _ekf.information_event_flags().reset_vel_to_gps;
+			 * @group ekf2
-		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;
+			events::send<int32_t>(events::ID("ekf2_reset_vel_to_gnss"), events::Log::Debug,
-		event_flags.reset_vel_to_zero                   = _ekf.information_event_flags().reset_vel_to_zero;
+					      "EKF2({1}): Reset velocity to GNSS", _instance);
-		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;
 
-		event_flags.warning_event_changes               = _filter_warning_event_changes;
+		if (_ekf.information_event_flags().reset_vel_to_flow) {
-		event_flags.gps_quality_poor                    = _ekf.warning_event_flags().gps_quality_poor;
+			/* EVENT
-		event_flags.gps_fusion_timout                   = _ekf.warning_event_flags().gps_fusion_timout;
+			 * @group ekf2
-		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;
+			events::send<int32_t>(events::ID("ekf2_reset_vel_to_flow"), events::Log::Debug,
-		event_flags.height_sensor_timeout               = _ekf.warning_event_flags().height_sensor_timeout;
+					      "EKF2({1}): Reset velocity to optical flow", _instance);
-		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();
+		if (_ekf.information_event_flags().reset_vel_to_vision) {
-		_estimator_event_flags_pub.update(event_flags);
+			/* EVENT
+			 * @group ekf2
+			 */
+			events::send<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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<int32_t>(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;
 	}
 }
 

src/modules/ekf2/EKF2.hpp

@@ -67,7 +67,6 @@
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/estimator_bias.h>
 #include <uORB/topics/estimator_bias3d.h>
-#include <uORB/topics/estimator_event_flags.h>
 #include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_states.h>
@@ -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_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
-	uORB::PublicationMultiData<estimator_event_flags_s>  _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovations_pub{ORB_ID(estimator_innovations)};

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");

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