Merge branch 'main' into pr-ekf2-events

Update mangled symbol from PublishEventFlags to SendEvents in
ITCM function include linker scripts for fmu-v6xrt, mr-tropic,
and tropic-community boards.

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

boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld

@@ -405,7 +405,7 @@
 *(.text._ZN3Ekf33updateVerticalDeadReckoningStatusEv)
 *(.text._ZN33FlightTaskManualAltitudeSmoothVelC1Ev)
 *(.text.powf)
-*(.text._ZN4EKF217PublishEventFlagsERKy)
+*(.text._ZN4EKF210SendEventsEv)
 *(.text._ZN17FlightTaskDescend6updateEv)
 *(.text.imxrt_iomux_configure)
 *(.text.hrt_store_absolute_time)

boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld

@@ -405,7 +405,7 @@
 *(.text._ZN3Ekf33updateVerticalDeadReckoningStatusEv)
 *(.text._ZN33FlightTaskManualAltitudeSmoothVelC1Ev)
 *(.text.powf)
-*(.text._ZN4EKF217PublishEventFlagsERKy)
+*(.text._ZN4EKF210SendEventsEv)
 *(.text._ZN17FlightTaskDescend6updateEv)
 *(.text.imxrt_iomux_configure)
 *(.text.hrt_store_absolute_time)

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -413,7 +413,7 @@
 *(.text._ZN3Ekf33updateVerticalDeadReckoningStatusEv)
 *(.text._ZN33FlightTaskManualAltitudeSmoothVelC1Ev)
 *(.text.powf)
-*(.text._ZN4EKF217PublishEventFlagsERKy)
+*(.text._ZN4EKF210SendEventsEv)
 *(.text._ZN17FlightTaskDescend6updateEv)
 *(.text.imxrt_iomux_configure)
 *(.text.hrt_store_absolute_time)

msg/CMakeLists.txt

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

msg/EstimatorEventFlags.msg

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

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();
@@ -812,7 +811,7 @@ void EKF2::Run()
 #endif // CONFIG_EKF2_WIND
 
 			// publish status/logging messages
-			PublishEventFlags(now);
+			SendEvents();
 			PublishStatus(now);
 			PublishStatusFlags(now);
 
@@ -1136,7 +1135,7 @@ estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status,
 	return bias;
 }
 
-void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
+void EKF2::SendEvents()
 {
 	// information events
 	uint32_t information_events = _ekf.information_event_status().value;
@@ -1148,40 +1147,153 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 	}
 
 	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 <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;
-		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<int32_t>(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<int32_t>(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<int32_t>(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<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);
+		}
 
 		_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;
+	if (_ekf.control_status_flags().yaw_align && !_ekf.control_status_prev_flags().yaw_align) {
+		/* EVENT
+		 * @group ekf2
+		 */
+		events::send<int32_t>(events::ID("ekf2_yaw_align"), events::Log::Debug,
+				      "EKF2({1}): yaw aligned", _instance);
 	}
 }
 

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>
@@ -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 &timestamp);
+	void SendEvents();
 	void PublishGlobalPosition(const hrt_abstime &timestamp);
 	void PublishInnovations(const hrt_abstime &timestamp);
 	void PublishInnovationTestRatios(const hrt_abstime &timestamp);
@@ -409,7 +408,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};
@@ -420,7 +418,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

@@ -187,7 +187,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);

src/modules/zenoh/Kconfig.topics

@@ -181,10 +181,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
@@ -995,7 +991,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