WIP: multi-EKF replay hacks and replay overhaul

.ci/Jenkinsfile-hardware

@@ -898,7 +898,6 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"'
@@ -936,7 +935,6 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo"'

Makefile

@@ -122,13 +122,6 @@ endif
 
 SRC_DIR := $(shell dirname "$(realpath $(lastword $(MAKEFILE_LIST)))")
 
-# check if replay env variable is set & set build dir accordingly
-ifdef replay
-	BUILD_DIR_SUFFIX := _replay
-else
-	BUILD_DIR_SUFFIX :=
-endif
-
 # additional config parameters passed to cmake
 ifdef EXTERNAL_MODULES_LOCATION
 	CMAKE_ARGS += -DEXTERNAL_MODULES_LOCATION:STRING=$(EXTERNAL_MODULES_LOCATION)

ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt

@@ -38,6 +38,14 @@ px4_add_romfs_files(
 	px4-rc.params
 	px4-rc.rtps
 	px4-rc.simulator
-	rc.replay
+
+	rc.replay_controller_mc_attitude
+	rc.replay_controller_mc_position
+	rc.replay_controller_mc_rate
+	rc.replay_ekf2
+	rc.replay_mc_hover_thrust_estimator
+	rc.replay_mixing
+	rc.replay_sensor_calibration
+
 	rcS
 )

ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_attitude

@@ -10,20 +10,20 @@ fi
 
 if [ ! -f replay_params.txt ]; then
 	echo "Creating $(pwd)/replay_params.txt"
-	ulog_params -i "${replay}" -d ' ' | grep -e '^EKF2' > replay_params.txt
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
 fi
 
-publisher_rules_file="orb_publisher.rules"
-cat <<EOF > "$publisher_rules_file"
-restrict_topics: sensor_combined, vehicle_gps_position, vehicle_land_detected
-module: replay
-ignore_others: false
-EOF
-
 param set SDLOG_DIRS_MAX 7
 
+# TODO:
+# input: vehicle_attitude, vehicle_attitude_setpoint
+# output: vehicle_rates_setpoint
+
 # apply all params before ekf starts, as some params cannot be changed after startup
-replay tryapplyparams
-ekf2 start -r
+#replay tryapplyparams
+
+mc_att_control start
+mc_rate_control start
+
 logger start -f -t -b 1000 -p vehicle_attitude
 replay start

ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_position

@@ -0,0 +1,30 @@
+#!/bin/sh
+
+# EKF2 replay script
+
+# shellcheck disable=SC2154
+if [ ! -f ${replay} ]; then
+	echo "Invalid replay log file ${replay}"
+	exit 1
+fi
+
+if [ ! -f replay_params.txt ]; then
+	echo "Creating $(pwd)/replay_params.txt"
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+fi
+
+param set SDLOG_DIRS_MAX 7
+
+# TODO:
+# input: trajector_setpoint
+# output: vehicle_local_position_setpoint vehicle_attitude_setpoint
+
+# apply all params before ekf starts, as some params cannot be changed after startup
+#replay tryapplyparams
+
+mc_pos_control start
+mc_att_control start
+mc_rate_control start
+
+logger start -f -t -b 1000 -p vehicle_attitude
+replay start

ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_rate

@@ -0,0 +1,28 @@
+#!/bin/sh
+
+# EKF2 replay script
+
+# shellcheck disable=SC2154
+if [ ! -f ${replay} ]; then
+	echo "Invalid replay log file ${replay}"
+	exit 1
+fi
+
+if [ ! -f replay_params.txt ]; then
+	echo "Creating $(pwd)/replay_params.txt"
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+fi
+
+param set SDLOG_DIRS_MAX 7
+
+# TODO:
+# input: vehicle_angular_velocity
+# output: actuator_controls
+
+# apply all params before ekf starts, as some params cannot be changed after startup
+#replay tryapplyparams
+
+mc_rate_control start
+
+logger start -f -t -b 1000 -p vehicle_attitude
+replay start

ROMFS/px4fmu_common/init.d-posix/rc.replay_ekf2

@@ -0,0 +1,41 @@
+#!/bin/sh
+
+# EKF2 replay script
+
+# shellcheck disable=SC2154
+if [ ! -f ${replay} ]; then
+	echo "Invalid replay log file ${replay}"
+	exit 1
+fi
+
+if [ ! -f replay_params.txt ]; then
+	echo "Creating $(pwd)/replay_params.txt"
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+fi
+
+param set SDLOG_DIRS_MAX 7
+
+# TODO:
+# input:  vehicle_imu, vehicle_magnetometer, distance_sensor
+# output: estimator_status
+
+# apply all params before ekf starts, as some params cannot be changed after startup
+#replay tryapplyparams
+ekf2 start
+ekf2 status
+logger start -f -t -b 1000 -p vehicle_attitude
+replay start
+
+
+
+# TODO: easy debug from gdb (and vscode)
+# TODO
+# loop through parameter changes? (noise, sensor delays, etc)
+#        - create log files with appropriate names (logger custom logfile name?)
+
+# TODO
+#  - work queue lockstep handling
+#  - parameter defaults
+#  - progress (percentage, time in seconds, etc)
+#  - realtime vs execute as fast as possible
+#  - controls (pause, reset, etc)

ROMFS/px4fmu_common/init.d-posix/rc.replay_mc_hover_thrust_estimator

@@ -0,0 +1,26 @@
+#!/bin/sh
+
+# EKF2 replay script
+
+# shellcheck disable=SC2154
+if [ ! -f ${replay} ]; then
+	echo "Invalid replay log file ${replay}"
+	exit 1
+fi
+
+if [ ! -f replay_params.txt ]; then
+	echo "Creating $(pwd)/replay_params.txt"
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+fi
+
+param set SDLOG_DIRS_MAX 7
+
+# TODO:
+# input:  vehicle_local_position, vehicle_local_position_setpoint
+# output: hover_thrust_estimate
+
+# apply all params before ekf starts, as some params cannot be changed after startup
+#replay tryapplyparams
+mc_hover_thrust_estimator start
+logger start -f -t -b 1000 -p vehicle_attitude
+replay start

ROMFS/px4fmu_common/init.d-posix/rc.replay_mixing

@@ -0,0 +1,29 @@
+#!/bin/sh
+
+# EKF2 replay script
+
+# shellcheck disable=SC2154
+if [ ! -f ${replay} ]; then
+	echo "Invalid replay log file ${replay}"
+	exit 1
+fi
+
+if [ ! -f replay_params.txt ]; then
+	echo "Creating $(pwd)/replay_params.txt"
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+fi
+
+param set SDLOG_DIRS_MAX 7
+
+# TODO:
+# input:  actuator_controls_0
+# output:
+
+# apply all params before ekf starts, as some params cannot be changed after startup
+#replay tryapplyparams
+
+pwm_out_sim start
+mixer load /dev/pwm_output0 etc/mixers/quad_x.main.mix
+
+logger start -f -t -b 1000 -p vehicle_attitude
+replay start

ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_calibration

@@ -0,0 +1,30 @@
+#!/bin/sh
+
+# EKF2 replay script
+
+# shellcheck disable=SC2154
+if [ ! -f ${replay} ]; then
+	echo "Invalid replay log file ${replay}"
+	exit 1
+fi
+
+if [ ! -f replay_params.txt ]; then
+	echo "Creating $(pwd)/replay_params.txt"
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+fi
+
+param set SDLOG_DIRS_MAX 7
+
+# TODO:
+# input: sensor_accel, sensor_gyro, sensor_mag, ?
+# output: mag_worker_data
+
+# apply all params before ekf starts, as some params cannot be changed after startup
+#replay tryapplyparams
+
+commander start
+gyro_calibration start
+temperature_compensatoin start
+
+logger start -f -t -b 1000 -p vehicle_attitude
+replay start

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -12,11 +12,10 @@ PATH="$PATH:${R}etc/init.d-posix"
 # Main SITL startup script
 #
 
-# check for ekf2 replay
-# shellcheck disable=SC2154
-if [ "$replay_mode" = "ekf2" ]
-then
-	. ${R}etc/init.d-posix/rc.replay
+# check for replay
+if [ ! -z ${replay_mode+x} ]; then
+	echo "Replay Mode: ${replay_mode}"
+	. ${R}etc/init.d-posix/rc.replay_${replay_mode}
 	exit 0
 fi
 
@@ -265,12 +264,7 @@ fi
 [ -e "$autostart_file".post ] && . "$autostart_file".post
 
 # Run script to start logging
-if param compare SYS_MC_EST_GROUP 2
-then
-	set LOGGER_ARGS "-p ekf2_timestamps"
-else
-	set LOGGER_ARGS "-p vehicle_attitude"
-fi
+set LOGGER_ARGS "-p vehicle_attitude"
 . ${R}etc/init.d/rc.logging
 
 mavlink boot_complete

boards/px4/sitl/ctrlalloc.cmake

@@ -107,13 +107,4 @@ set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb")
 
 # If the environment variable 'replay' is defined, we are building with replay
 # support. In this case, we enable the orb publisher rules.
-set(REPLAY_FILE "$ENV{replay}")
-if(REPLAY_FILE)
-	message(STATUS "Building with uorb publisher rules support")
-	add_definitions(-DORB_USE_PUBLISHER_RULES)
-
-	message(STATUS "Building without lockstep for replay")
-	set(ENABLE_LOCKSTEP_SCHEDULER no)
-else()
-	set(ENABLE_LOCKSTEP_SCHEDULER yes)
-endif()
+set(ENABLE_LOCKSTEP_SCHEDULER yes)

boards/px4/sitl/default.cmake

@@ -104,15 +104,4 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none")
 set(config_sitl_debugger disable CACHE STRING "debugger for sitl")
 set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb")
 
-# If the environment variable 'replay' is defined, we are building with replay
-# support. In this case, we enable the orb publisher rules.
-set(REPLAY_FILE "$ENV{replay}")
-if(REPLAY_FILE)
-	message(STATUS "Building with uorb publisher rules support")
-	add_definitions(-DORB_USE_PUBLISHER_RULES)
-
-	message(STATUS "Building without lockstep for replay")
-	set(ENABLE_LOCKSTEP_SCHEDULER no)
-else()
-	set(ENABLE_LOCKSTEP_SCHEDULER yes)
-endif()
+set(ENABLE_LOCKSTEP_SCHEDULER yes)

boards/px4/sitl/replay.cmake

@@ -15,7 +15,4 @@ px4_add_board(
 		work_queue
 	)
 
-message(STATUS "Building with uorb publisher rules support")
-add_definitions(-DORB_USE_PUBLISHER_RULES)
-
 set(ENABLE_LOCKSTEP_SCHEDULER yes)

boards/px4/sitl/rtps.cmake

@@ -102,15 +102,4 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none")
 set(config_sitl_debugger disable CACHE STRING "debugger for sitl")
 set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb")
 
-# If the environment variable 'replay' is defined, we are building with replay
-# support. In this case, we enable the orb publisher rules.
-set(REPLAY_FILE "$ENV{replay}")
-if(REPLAY_FILE)
-	message(STATUS "Building with uorb publisher rules support")
-	add_definitions(-DORB_USE_PUBLISHER_RULES)
-
-	message(STATUS "Building without lockstep for replay")
-	set(ENABLE_LOCKSTEP_SCHEDULER no)
-else()
-	set(ENABLE_LOCKSTEP_SCHEDULER yes)
-endif()
+set(ENABLE_LOCKSTEP_SCHEDULER yes)

boards/px4/sitl/test.cmake

@@ -102,13 +102,5 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none")
 set(config_sitl_debugger disable CACHE STRING "debugger for sitl")
 set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb")
 
-# If the environment variable 'replay' is defined, we are building with replay
-# support. In this case, we enable the orb publisher rules.
-set(REPLAY_FILE "$ENV{replay}")
-if(REPLAY_FILE)
-	message(STATUS "Building with uorb publisher rules support")
-	add_definitions(-DORB_USE_PUBLISHER_RULES)
-endif()
-
 message(STATUS "Building without lockstep for test")
 set(ENABLE_LOCKSTEP_SCHEDULER no)

msg/CMakeLists.txt

@@ -56,7 +56,6 @@ set(msg_files
 	cpuload.msg
 	differential_pressure.msg
 	distance_sensor.msg
-	ekf2_timestamps.msg
 	ekf_gps_drift.msg
 	esc_report.msg
 	esc_status.msg
@@ -130,7 +129,6 @@ set(msg_files
 	sensor_accel.msg
 	sensor_accel_fifo.msg
 	sensor_baro.msg
-	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gps.msg
 	sensor_gyro.msg

msg/ekf2_timestamps.msg

@@ -1,23 +0,0 @@
-# this message contains the (relative) timestamps of the sensor inputs used by EKF2.
-# It can be used for reproducible replay.
-
-# the timestamp field is the ekf2 reference time and matches the timestamp of
-# the sensor_combined topic.
-
-uint64 timestamp			 # time since system start (microseconds)
-
-int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative timestamps
-                                         # is set to this value, it means the associated sensor values did not update
-
-# timestamps are relative to the main timestamp and are in 0.1 ms (timestamp +
-# *_timestamp_rel = absolute timestamp). For int16, this allows a maximum
-# difference of +-3.2s to the sensor_combined topic.
-
-int16 airspeed_timestamp_rel
-int16 distance_sensor_timestamp_rel
-int16 optical_flow_timestamp_rel
-int16 vehicle_air_data_timestamp_rel
-int16 vehicle_magnetometer_timestamp_rel
-int16 visual_odometry_timestamp_rel
-
-# Note: this is a high-rate logged topic, so it needs to be as small as possible

msg/sensor_combined.msg

@@ -1,20 +0,0 @@
-# Sensor readings in SI-unit form.
-# These fields are scaled and offset-compensated where possible and do not
-# change with board revisions and sensor updates.
-
-uint64 timestamp				# time since system start (microseconds)
-
-int32 RELATIVE_TIMESTAMP_INVALID = 2147483647	# (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
-
-# gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_rad			# average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
-uint32 gyro_integral_dt		# gyro measurement sampling period in microseconds
-
-int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_m_s2		# average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
-uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in microseconds
-
-uint8 CLIPPING_X = 1
-uint8 CLIPPING_Y = 2
-uint8 CLIPPING_Z = 4
-uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period

msg/sensor_selection.msg

@@ -1,7 +1,8 @@
 #
-# Sensor ID's for the voted sensors output on the sensor_combined topic.
+# Sensor ID's for the primary IMU.
 # Will be updated on startup of the sensor module and when sensor selection changes
 #
 uint64 timestamp		# time since system start (microseconds)
+
 uint32 accel_device_id		# unique device ID for the selected accelerometers
 uint32 gyro_device_id		# unique device ID for the selected rate gyros

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -111,6 +111,7 @@ void WorkQueue::Add(WorkItem *item)
 
 	if (_lockstep_component == -1) {
 		_lockstep_component = px4_lockstep_register_component();
+		//fprintf(stderr, "WQ %s px4_lockstep_register_component:%d\n", item->ItemName(), _lockstep_component);
 	}
 
 #endif // ENABLE_LOCKSTEP_SCHEDULER
@@ -170,6 +171,10 @@ void WorkQueue::Run()
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 
 		if (_q.empty()) {
+			if (_lockstep_component > 0) {
+				//fprintf(stderr, "WQ px4_lockstep_unregister_component:%d\n", _lockstep_component);
+			}
+
 			px4_lockstep_unregister_component(_lockstep_component);
 			_lockstep_component = -1;
 		}

platforms/common/uORB/uORBManager.cpp

@@ -66,29 +66,6 @@ bool uORB::Manager::terminate()
 	return false;
 }
 
-uORB::Manager::Manager()
-{
-#ifdef ORB_USE_PUBLISHER_RULES
-	const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules";
-	int ret = readPublisherRulesFromFile(file_name, _publisher_rule);
-
-	if (ret == PX4_OK) {
-		_has_publisher_rules = true;
-		PX4_INFO("Using orb rules from %s", file_name);
-
-	} else {
-		PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret));
-	}
-
-#endif /* ORB_USE_PUBLISHER_RULES */
-
-}
-
-uORB::Manager::~Manager()
-{
-	delete _device_master;
-}
-
 uORB::DeviceMaster *uORB::Manager::get_device_master()
 {
 	if (!_device_master) {
@@ -170,30 +147,6 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
 		unsigned int queue_size)
 {
-#ifdef ORB_USE_PUBLISHER_RULES
-
-	// check publisher rule
-	if (_has_publisher_rules) {
-		const char *prog_name = px4_get_taskname();
-
-		if (strcmp(_publisher_rule.module_name, prog_name) == 0) {
-			if (_publisher_rule.ignore_other_topics) {
-				if (!findTopic(_publisher_rule, meta->o_name)) {
-					PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
-					return (orb_advert_t)_Instance;
-				}
-			}
-
-		} else {
-			if (findTopic(_publisher_rule, meta->o_name)) {
-				PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
-				return (orb_advert_t)_Instance;
-			}
-		}
-	}
-
-#endif /* ORB_USE_PUBLISHER_RULES */
-
 	/* open the node as an advertiser */
 	int fd = node_open(meta, true, instance);
 
@@ -242,14 +195,6 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 
 int uORB::Manager::orb_unadvertise(orb_advert_t handle)
 {
-#ifdef ORB_USE_PUBLISHER_RULES
-
-	if (handle == _Instance) {
-		return PX4_OK; //pretend success
-	}
-
-#endif /* ORB_USE_PUBLISHER_RULES */
-
 	return uORB::DeviceNode::unadvertise(handle);
 }
 
@@ -264,54 +209,11 @@ int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned
 	return node_open(meta, false, &inst);
 }
 
-int uORB::Manager::orb_unsubscribe(int fd)
-{
-	return px4_close(fd);
-}
-
 int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
 {
-#ifdef ORB_USE_PUBLISHER_RULES
-
-	if (handle == _Instance) {
-		return PX4_OK; //pretend success
-	}
-
-#endif /* ORB_USE_PUBLISHER_RULES */
-
 	return uORB::DeviceNode::publish(meta, handle, data);
 }
 
-int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
-{
-	int ret;
-
-	ret = px4_read(handle, buffer, meta->o_size);
-
-	if (ret < 0) {
-		return PX4_ERROR;
-	}
-
-	if (ret != (int)meta->o_size) {
-		errno = EIO;
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
-int uORB::Manager::orb_check(int handle, bool *updated)
-{
-	/* Set to false here so that if `px4_ioctl` fails to false. */
-	*updated = false;
-	return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
-}
-
-int uORB::Manager::orb_set_interval(int handle, unsigned interval)
-{
-	return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
-}
-
 int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
 {
 	int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval);
@@ -508,142 +410,3 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
 #endif
 }
 #endif /* ORB_COMMUNICATOR */
-
-#ifdef ORB_USE_PUBLISHER_RULES
-
-bool uORB::Manager::startsWith(const char *pre, const char *str)
-{
-	size_t lenpre = strlen(pre),
-	       lenstr = strlen(str);
-	return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0;
-}
-
-bool uORB::Manager::findTopic(const PublisherRule &rule, const char *topic_name)
-{
-	const char **topics_ptr = rule.topics;
-
-	while (*topics_ptr) {
-		if (strcmp(*topics_ptr, topic_name) == 0) {
-			return true;
-		}
-
-		++topics_ptr;
-	}
-
-	return false;
-}
-
-void uORB::Manager::strTrim(const char **str)
-{
-	while (**str == ' ' || **str == '\t') { ++(*str); }
-}
-
-int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
-{
-	FILE *fp;
-	static const int line_len = 1024;
-	int ret = PX4_OK;
-	char *line = new char[line_len];
-
-	if (!line) {
-		return -ENOMEM;
-	}
-
-	fp = fopen(file_name, "r");
-
-	if (fp == NULL) {
-		delete[](line);
-		return -errno;
-	}
-
-	const char *restrict_topics_str = "restrict_topics:";
-	const char *module_str = "module:";
-	const char *ignore_others = "ignore_others:";
-
-	rule.ignore_other_topics = false;
-	rule.module_name = nullptr;
-	rule.topics = nullptr;
-
-	while (fgets(line, line_len, fp) && ret == PX4_OK) {
-
-		if (strlen(line) < 2 || line[0] == '#') {
-			continue;
-		}
-
-		if (startsWith(restrict_topics_str, line)) {
-			//read topics list
-			char *start = line + strlen(restrict_topics_str);
-			strTrim((const char **)&start);
-			char *topics = strdup(start);
-			int topic_len = 0, num_topics = 0;
-
-			for (int i = 0; topics[i]; ++i) {
-				if (topics[i] == ',' || topics[i] == '\n') {
-					if (topic_len > 0) {
-						topics[i] = 0;
-						++num_topics;
-					}
-
-					topic_len = 0;
-
-				} else {
-					++topic_len;
-				}
-			}
-
-			if (num_topics > 0) {
-				rule.topics = new const char *[num_topics + 1];
-				int topic = 0;
-				strTrim((const char **)&topics);
-				rule.topics[topic++] = topics;
-
-				while (topic < num_topics) {
-					if (*topics == 0) {
-						++topics;
-						strTrim((const char **)&topics);
-						rule.topics[topic++] = topics;
-
-					} else {
-						++topics;
-					}
-				}
-
-				rule.topics[num_topics] = nullptr;
-			}
-
-		} else if (startsWith(module_str, line)) {
-			//read module name
-			char *start = line + strlen(module_str);
-			strTrim((const char **)&start);
-			int len = strlen(start);
-
-			if (len > 0 && start[len - 1] == '\n') {
-				start[len - 1] = 0;
-			}
-
-			rule.module_name = strdup(start);
-
-		} else if (startsWith(ignore_others, line)) {
-			const char *start = line + strlen(ignore_others);
-			strTrim(&start);
-
-			if (startsWith("true", start)) {
-				rule.ignore_other_topics = true;
-			}
-
-		} else {
-			PX4_ERR("orb rules file: wrong format: %s", line);
-			ret = -EINVAL;
-		}
-	}
-
-	if (ret == PX4_OK && (!rule.module_name || !rule.topics)) {
-		PX4_ERR("Wrong format in orb publisher rules file");
-		ret = -EINVAL;
-	}
-
-	delete[](line);
-	fclose(fp);
-	return ret;
-}
-#endif /* ORB_USE_PUBLISHER_RULES */

platforms/common/uORB/uORBManager.hpp

@@ -256,7 +256,7 @@ public:
 	 * @param handle  A handle returned from orb_subscribe.
 	 * @return    OK on success, PX4_ERROR otherwise with errno set accordingly.
 	 */
-	int  orb_unsubscribe(int handle);
+	int orb_unsubscribe(int handle) { return px4_close(handle); }
 
 	/**
 	 * Fetch data from a topic.
@@ -274,7 +274,21 @@ public:
 	 *      using the data.
 	 * @return    OK on success, PX4_ERROR otherwise with errno set accordingly.
 	 */
-	int  orb_copy(const struct orb_metadata *meta, int handle, void *buffer);
+	int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
+	{
+		int ret = px4_read(handle, buffer, meta->o_size);
+
+		if (ret < 0) {
+			return PX4_ERROR;
+		}
+
+		if (ret != (int)meta->o_size) {
+			errno = EIO;
+			return PX4_ERROR;
+		}
+
+		return PX4_OK;
+	}
 
 	/**
 	 * Check whether a topic has been published to since the last orb_copy.
@@ -292,7 +306,12 @@ public:
 	 * @return    OK if the check was successful, PX4_ERROR otherwise with
 	 *      errno set accordingly.
 	 */
-	int  orb_check(int handle, bool *updated);
+	int orb_check(int handle, bool *updated)
+	{
+		/* Set to false here so that if `px4_ioctl` fails to false. */
+		*updated = false;
+		return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
+	}
 
 	/**
 	 * Check if a topic has already been created and published (advertised)
@@ -321,8 +340,7 @@ public:
 	 * @param interval  An interval period in milliseconds.
 	 * @return    OK on success, PX4_ERROR otherwise with ERRNO set accordingly.
 	 */
-	int  orb_set_interval(int handle, unsigned interval);
-
+	int orb_set_interval(int handle, unsigned interval) { return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); }
 
 	/**
 	 * Get the minimum interval between which updates are seen for a subscription.
@@ -381,9 +399,12 @@ private: // data members
 
 	DeviceMaster *_device_master{nullptr};
 
-private: //class methods
-	Manager();
-	virtual ~Manager();
+private: // class methods
+	Manager() = default;
+	virtual ~Manager()
+	{
+		delete _device_master;
+	}
 
 #ifdef ORB_COMMUNICATOR
 	/**
@@ -442,46 +463,6 @@ private: //class methods
 	 */
 	virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data);
 #endif /* ORB_COMMUNICATOR */
-
-#ifdef ORB_USE_PUBLISHER_RULES
-
-	struct PublisherRule {
-		const char **topics; //null-terminated list of topic names
-		const char *module_name; //only this module is allowed to publish one of the topics
-		bool ignore_other_topics;
-	};
-
-	/**
-	 * test if str starts with pre
-	 */
-	bool startsWith(const char *pre, const char *str);
-
-	/**
-	 * find a topic in a rule
-	 */
-	bool findTopic(const PublisherRule &rule, const char *topic_name);
-
-	/**
-	 * trim whitespace from the beginning of a string
-	 */
-	void strTrim(const char **str);
-
-	/**
-	 * Read publisher rules from a file. It has the format:
-	 *
-	 * restrict_topics: <topic1>, <topic2>, <topic3>
-	 * module: <module_name>
-	 * [ignore_others:true]
-	 *
-	 * @return 0 on success, <0 otherwise
-	 */
-	int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
-
-	PublisherRule _publisher_rule;
-	bool _has_publisher_rules = false;
-
-#endif /* ORB_USE_PUBLISHER_RULES */
-
 };
 
 #endif /* _uORBManager_hpp_ */

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -77,7 +77,7 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
 static px4_sem_t 	_hrt_lock;
 static struct work_s	_hrt_work;
 
-static hrt_abstime px4_timestart_monotonic = 0;
+static hrt_abstime px4_timestart_monotonic = UINT64_MAX;
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 static LockstepScheduler *lockstep_scheduler = new LockstepScheduler();
@@ -90,7 +90,7 @@ static void hrt_call_invoke();
 
 hrt_abstime hrt_absolute_time_offset()
 {
-	return px4_timestart_monotonic;
+	return (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
 }
 
 static void hrt_lock()
@@ -136,7 +136,9 @@ hrt_abstime hrt_absolute_time()
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	// optimized case (avoid ts_to_abstime) if lockstep scheduler is used
 	const uint64_t abstime = lockstep_scheduler->get_absolute_time();
-	return abstime - px4_timestart_monotonic;
+	uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
+	return abstime - timestart_monotonic;
+
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	struct timespec ts;
 	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
@@ -149,9 +151,7 @@ hrt_abstime hrt_absolute_time()
  */
 hrt_abstime ts_to_abstime(const struct timespec *ts)
 {
-	hrt_abstime	result;
-
-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	hrt_abstime result = (hrt_abstime)(ts->tv_sec) * 1000000;
 	result += ts->tv_nsec / 1000;
 
 	return result;
@@ -510,7 +510,8 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 	if (clk_id == CLOCK_MONOTONIC) {
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 		const uint64_t abstime = lockstep_scheduler->get_absolute_time();
-		abstime_to_ts(tp, abstime - px4_timestart_monotonic);
+		uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
+		abstime_to_ts(tp, abstime - timestart_monotonic);
 		return 0;
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 #if defined(__PX4_DARWIN)
@@ -536,7 +537,7 @@ int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
 	} else {
 		const uint64_t time_us = ts_to_abstime(ts);
 
-		if (px4_timestart_monotonic == 0) {
+		if (px4_timestart_monotonic == UINT64_MAX) {
 			px4_timestart_monotonic = time_us;
 		}
 
@@ -545,10 +546,9 @@ int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
 	}
 }
 
-
 int px4_usleep(useconds_t usec)
 {
-	if (px4_timestart_monotonic == 0) {
+	if (px4_timestart_monotonic == UINT64_MAX) {
 		// Until the time is set by the simulator, we fallback to the normal
 		// usleep;
 		return system_usleep(usec);
@@ -561,7 +561,7 @@ int px4_usleep(useconds_t usec)
 
 unsigned int px4_sleep(unsigned int seconds)
 {
-	if (px4_timestart_monotonic == 0) {
+	if (px4_timestart_monotonic == UINT64_MAX) {
 		// Until the time is set by the simulator, we fallback to the normal
 		// sleep;
 		return system_sleep(seconds);
@@ -577,8 +577,10 @@ int px4_pthread_cond_timedwait(pthread_cond_t *cond,
 			       pthread_mutex_t *mutex,
 			       const struct timespec *ts)
 {
+	uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
+
 	const uint64_t time_us = ts_to_abstime(ts);
-	const uint64_t scheduled = time_us + px4_timestart_monotonic;
+	const uint64_t scheduled = time_us + timestart_monotonic;
 	return lockstep_scheduler->cond_timedwait(cond, mutex, scheduled);
 }
 

src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp

@@ -49,10 +49,10 @@ MagnetometerBiasEstimator::MagnetometerBiasEstimator(const matrix::Dcmf &board_r
 	_board_rotation(board_rotation)
 {}
 
-void MagnetometerBiasEstimator::extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw)
+void MagnetometerBiasEstimator::extractBias(vehicle_magnetometer_s &mag_raw, const vehicle_angular_velocity_s &gyro)
 {
 	// fill in vectors
-	Vector3f gyro(gyro_raw.gyro_rad);
+	Vector3f gyro(gyro.xyz);
 	Vector3f mag(mag_raw.magnetometer_ga);
 
 	// prepare delta time in seconds

src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp

@@ -47,9 +47,9 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_magnetometer.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_magnetometer.h>
+#include <uORB/topics/vehicle_angular_velocity_s.h>
 
 #include <FieldSensorBiasEstimator.hpp>
 
@@ -64,7 +64,7 @@ public:
 	 * @param mag_raw struct containing the magnetometer data to operate on (gets adjusted with current estimate)
 	 * @param raw struct containing the gyroscope data to use
 	 */
-	void extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw);
+	void extractBias(vehicle_magnetometer_s &mag_raw, const vehicle_angular_velocity_s &gyro_raw);
 
 private:
 	void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &mag, const float dt);

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -58,12 +58,11 @@ bool TerrainEstimator::is_distance_valid(float distance)
 	return (distance < 40.0f && distance > 0.00001f);
 }
 
-void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude,
-			       const struct sensor_combined_s *sensor,
-			       const struct distance_sensor_s *distance)
+void TerrainEstimator::predict(float dt, const vehicle_attitude_s *attitude, const vehicle_imu_s *imu,
+			       const distance_sensor_s *distance)
 {
 	matrix::Dcmf R_att = matrix::Quatf(attitude->q);
-	matrix::Vector3f a{sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2]};
+	matrix::Vector3f a{Vector3f{imu->delta_velocity} * 1.e6f / (float)imu->delta_velocity_dt};
 	matrix::Vector<float, 3> u;
 	u = R_att * a;
 	_u_z = u(2) + CONSTANTS_ONE_G; // compensate for gravity
@@ -97,9 +96,8 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 	       B * R * B.transpose() + Q) * dt;
 }
 
-void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
-		const struct distance_sensor_s *distance,
-		const struct vehicle_attitude_s *attitude)
+void TerrainEstimator::measurement_update(uint64_t time_ref, const vehicle_gps_position_s *gps,
+		const distance_sensor_s *distance, const vehicle_attitude_s *attitude)
 {
 	// terrain estimate is invalid if we have range sensor timeout
 	if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) {

src/lib/terrain_estimation/terrain_estimator.h

@@ -39,11 +39,10 @@
 
 #include <lib/mathlib/mathlib.h>
 #include <matrix/math.hpp>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/distance_sensor.h>
-
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_imu.h>
 
 /*
 * This class can be used to estimate distance to the ground using a laser range finder.
@@ -66,11 +65,10 @@ public:
 	float get_distance_to_ground() {return -_x(0);}
 	float get_velocity() {return _x(1);}
 
-	void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor,
-		     const struct distance_sensor_s *distance);
-	void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
-				const struct distance_sensor_s *distance,
-				const struct vehicle_attitude_s *attitude);
+	void predict(float dt, const vehicle_attitude_s *attitude, const vehicle_imu_s *sensor,
+		     const distance_sensor_s *distance);
+	void measurement_update(uint64_t time_ref, const vehicle_gps_position_s *gps, const distance_sensor_s *distance,
+				const vehicle_attitude_s *attitude);
 
 private:
 	enum {n_x = 3};

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -55,9 +55,9 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
@@ -106,7 +106,7 @@ private:
 	const float _dt_min = 0.00001f;
 	const float _dt_max = 0.02f;
 
-	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
+	uORB::SubscriptionCallbackWorkItem _imu_sub{this, ORB_ID(vehicle_imu)};
 
 	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription		_gps_sub{ORB_ID(vehicle_gps_position)};
@@ -180,7 +180,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 bool
 AttitudeEstimatorQ::init()
 {
-	if (!_sensors_sub.registerCallback()) {
+	if (!_imu_sub.registerCallback()) {
 		PX4_ERR("sensor combined callback registration failed!");
 		return false;
 	}
@@ -192,33 +192,26 @@ void
 AttitudeEstimatorQ::Run()
 {
 	if (should_exit()) {
-		_sensors_sub.unregisterCallback();
+		_imu_sub.unregisterCallback();
 		exit_and_cleanup();
 		return;
 	}
 
-	sensor_combined_s sensors;
+	vehicle_imu_s imu;
 
-	if (_sensors_sub.update(&sensors)) {
+	if (_imu_sub.update(&imu)) {
 
 		update_parameters();
 
-		// Feed validator with recent sensor data
-		if (sensors.timestamp > 0) {
-			_gyro(0) = sensors.gyro_rad[0];
-			_gyro(1) = sensors.gyro_rad[1];
-			_gyro(2) = sensors.gyro_rad[2];
-		}
+		const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
+		_accel = Vector3f{imu.delta_velocity} * accel_dt_inv;
 
-		if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
-			_accel(0) = sensors.accelerometer_m_s2[0];
-			_accel(1) = sensors.accelerometer_m_s2[1];
-			_accel(2) = sensors.accelerometer_m_s2[2];
+		const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
+		_gyro = Vector3f{imu.delta_angle} * gyro_dt_inv;
 
-			if (_accel.length() < 0.01f) {
-				PX4_ERR("degenerate accel!");
-				return;
-			}
+		if (_accel.length() < 0.01f) {
+			PX4_ERR("degenerate accel!");
+			return;
 		}
 
 		// Update magnetometer
@@ -348,8 +341,8 @@ AttitudeEstimatorQ::Run()
 		_last_time = now;
 
 		if (update(dt)) {
-			vehicle_attitude_s att = {};
-			att.timestamp_sample = sensors.timestamp;
+			vehicle_attitude_s att{};
+			att.timestamp_sample = imu.timestamp;
 			_q.copyTo(att.q);
 
 			/* the instance count is not used here */

src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m

@@ -19,11 +19,11 @@ end
 clear imu_data;
 n_samples = length(timestamp);
 imu_data.time_us = timestamp + accelerometer_timestamp_relative;
-imu_data.gyro_dt = gyro_integral_dt ./ 1e6;
-imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gyro_rad2.*imu_data.gyro_dt];
+imu_data.gyro_dt = delta_angle_dt ./ 1e6;
+imu_data.del_ang = [delta_angle0, delta_angle1, delta_angle2];
 
-imu_data.accel_dt = accelerometer_integral_dt ./ 1e6;
-imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt];
+imu_data.accel_dt = delta_velocity_dt ./ 1e6;
+imu_data.del_vel = [delta_velocity0, delta_velocity1, delta_velocity2];
 
 %% convert magnetometer data
 clear mag_data;

src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m

@@ -38,7 +38,7 @@ opts.DataLines = [2, Inf];
 opts.Delimiter = ",";
 
 % Specify column names and types
-opts.VariableNames = ["timestamp", "gyro_rad0", "gyro_rad1", "gyro_rad2", "gyro_integral_dt", "accelerometer_timestamp_relative", "accelerometer_m_s20", "accelerometer_m_s21", "accelerometer_m_s22", "accelerometer_integral_dt"];
+opts.VariableNames = ["timestamp", "delta_angle0", "delta_angle1", "delta_angle2", "delta_angle_dt", "delta_velocity0", "delta_velocity1", "delta_velocity2", "delta_velocity_dt"];
 opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
 opts.ExtraColumnsRule = "ignore";
 opts.EmptyLineRule = "read";
@@ -48,15 +48,14 @@ tbl = readtable(sensors_file, opts);
 
 % Convert to output type
 timestamp = tbl.timestamp;
-gyro_rad0 = tbl.gyro_rad0;
-gyro_rad1 = tbl.gyro_rad1;
-gyro_rad2 = tbl.gyro_rad2;
-gyro_integral_dt = tbl.gyro_integral_dt;
-accelerometer_timestamp_relative = tbl.accelerometer_timestamp_relative;
-accelerometer_m_s20 = tbl.accelerometer_m_s20;
-accelerometer_m_s21 = tbl.accelerometer_m_s21;
-accelerometer_m_s22 = tbl.accelerometer_m_s22;
-accelerometer_integral_dt = tbl.accelerometer_integral_dt;
+delta_angle0 = tbl.delta_angle0;
+delta_angle1 = tbl.delta_angle1;
+delta_angle2 = tbl.delta_angle2;
+gyro_integral_dt = tbl.delta_angle_dt;
+delta_velocity0 = tbl.delta_velocity0;
+delta_velocity1 = tbl.delta_velocity1;
+delta_velocity2 = tbl.delta_velocity2;
+accelerometer_integral_dt = tbl.delta_velocity_dt;
 
 clear opts tbl
 
@@ -166,7 +165,7 @@ cd ../;
 %% ------     SECTION 3: Ground Truth Data (STIL only, optional)     ------
 
 if exist('attitude_file','var') && exist('localpos_file','var') && exist('globalpos_file','var')
-    
+
 %- Import Attitude data from text file
 opts = delimitedTextImportOptions("NumVariables", 13);
 opts.DataLines = [2, Inf];

src/modules/ekf2/EKF2.cpp

@@ -45,10 +45,9 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
 #endif // !CONSTRAINED_FLASH
 
-EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
+EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config):
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, config),
-	_replay_mode(replay_mode && !multi_mode),
 	_multi_mode(multi_mode),
 	_instance(multi_mode ? -1 : 0),
 	_attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
@@ -190,7 +189,6 @@ bool EKF2::multi_init(int imu, int mag)
 	_odometry_pub.advertise();
 	_wind_pub.advertise();
 
-	_ekf2_timestamps_pub.advertise();
 	_ekf_gps_drift_pub.advertise();
 	_estimator_baro_bias_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
@@ -247,9 +245,7 @@ int EKF2::print_status()
 void EKF2::Run()
 {
 	if (should_exit()) {
-		_sensor_combined_sub.unregisterCallback();
 		_vehicle_imu_sub.unregisterCallback();
-
 		return;
 	}
 
@@ -273,12 +269,7 @@ void EKF2::Run()
 	}
 
 	if (!_callback_registered) {
-		if (_multi_mode) {
-			_callback_registered = _vehicle_imu_sub.registerCallback();
-
-		} else {
-			_callback_registered = _sensor_combined_sub.registerCallback();
-		}
+		_callback_registered = _vehicle_imu_sub.registerCallback();
 
 		if (!_callback_registered) {
 			PX4_WARN("%d - failed to register callback, retrying", _instance);
@@ -309,107 +300,80 @@ void EKF2::Run()
 		}
 	}
 
-	bool imu_updated = false;
-	imuSample imu_sample_new {};
-
-	hrt_abstime imu_dt = 0; // for tracking time slip later
-
-	if (_multi_mode) {
-		const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
-		vehicle_imu_s imu;
-		imu_updated = _vehicle_imu_sub.update(&imu);
-
-		if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
-			perf_count(_msg_missed_imu_perf);
-		}
-
-		imu_sample_new.time_us = imu.timestamp_sample;
-		imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;
-		imu_sample_new.delta_ang = Vector3f{imu.delta_angle};
-		imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f;
-		imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
-
-		if (imu.delta_velocity_clipping > 0) {
-			imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
-			imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
-			imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
-		}
-
-		imu_dt = imu.delta_angle_dt;
-
-		if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
-			_device_id_accel = imu.accel_device_id;
-			_device_id_gyro = imu.gyro_device_id;
-			_imu_calibration_count = imu.calibration_count;
-
-		} else if ((imu.calibration_count > _imu_calibration_count)
-			   || (imu.accel_device_id != _device_id_accel)
-			   || (imu.gyro_device_id != _device_id_gyro)) {
-
-			PX4_DEBUG("%d - resetting IMU bias", _instance);
-			_device_id_accel = imu.accel_device_id;
-			_device_id_gyro = imu.gyro_device_id;
-
-			_ekf.resetImuBias();
-			_imu_calibration_count = imu.calibration_count;
-
-			SelectImuStatus();
-		}
-
-	} else {
-		const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
-		sensor_combined_s sensor_combined;
-		imu_updated = _sensor_combined_sub.update(&sensor_combined);
-
-		if (imu_updated && (_sensor_combined_sub.get_last_generation() != last_generation + 1)) {
-			perf_count(_msg_missed_imu_perf);
-		}
-
-		imu_sample_new.time_us = sensor_combined.timestamp;
-		imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f;
-		imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt;
-		imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
-		imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
-
-		if (sensor_combined.accelerometer_clipping > 0) {
-			imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X;
-			imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y;
-			imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z;
-		}
-
-		imu_dt = sensor_combined.gyro_integral_dt;
-
+	if (!_multi_mode) {
 		if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) {
+			bool imu_changed = false;
 			sensor_selection_s sensor_selection;
 
 			if (_sensor_selection_sub.copy(&sensor_selection)) {
 				if (_device_id_accel != sensor_selection.accel_device_id) {
 					_ekf.resetAccelBias();
 					_device_id_accel = sensor_selection.accel_device_id;
-					SelectImuStatus();
+					imu_changed = true;
 				}
 
 				if (_device_id_gyro != sensor_selection.gyro_device_id) {
 					_ekf.resetGyroBias();
 					_device_id_gyro = sensor_selection.gyro_device_id;
-					SelectImuStatus();
+					imu_changed = true;
+				}
+
+				if (imu_changed) {
+					SelectImu();
 				}
 			}
 		}
 	}
 
-	if (imu_updated) {
-		const hrt_abstime now = imu_sample_new.time_us;
+	const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
+	vehicle_imu_s vehicle_imu;
+
+	if (_vehicle_imu_sub.update(&vehicle_imu)) {
+
+		const hrt_abstime now = vehicle_imu.timestamp_sample;
+
+		if (_vehicle_imu_sub.get_last_generation() != last_generation + 1) {
+			perf_count(_msg_missed_imu_perf);
+		}
+
+		if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
+			_device_id_accel = vehicle_imu.accel_device_id;
+			_device_id_gyro = vehicle_imu.gyro_device_id;
+			_imu_calibration_count = vehicle_imu.calibration_count;
+
+		} else if ((vehicle_imu.calibration_count > _imu_calibration_count)
+			   || (vehicle_imu.accel_device_id != _device_id_accel)
+			   || (vehicle_imu.gyro_device_id != _device_id_gyro)) {
+
+			PX4_DEBUG("%d - resetting IMU bias", _instance);
+			_device_id_accel = vehicle_imu.accel_device_id;
+			_device_id_gyro = vehicle_imu.gyro_device_id;
+
+			_ekf.resetImuBias();
+			_imu_calibration_count = vehicle_imu.calibration_count;
+
+			SelectImu();
+		}
 
 		UpdateImuStatus();
 
 		// push imu data into estimator
+		imuSample imu_sample_new;
+		imu_sample_new.time_us = vehicle_imu.timestamp_sample;
+		imu_sample_new.delta_ang_dt = vehicle_imu.delta_angle_dt * 1.e-6f;
+		imu_sample_new.delta_ang = Vector3f{vehicle_imu.delta_angle};
+		imu_sample_new.delta_vel_dt = vehicle_imu.delta_velocity_dt * 1.e-6f;
+		imu_sample_new.delta_vel = Vector3f{vehicle_imu.delta_velocity};
+		imu_sample_new.delta_vel_clipping[0] = vehicle_imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
+		imu_sample_new.delta_vel_clipping[0] = vehicle_imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
+		imu_sample_new.delta_vel_clipping[0] = vehicle_imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
+
 		_ekf.setIMUData(imu_sample_new);
 		PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
 
 		// integrate time to monitor time slippage
 		if (_start_time_us > 0) {
-			_integrated_time_us += imu_dt;
+			_integrated_time_us += vehicle_imu.delta_angle_dt;
 			_last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us;
 
 		} else {
@@ -465,29 +429,18 @@ void EKF2::Run()
 			}
 		}
 
-		// ekf2_timestamps (using 0.1 ms relative timestamps)
-		ekf2_timestamps_s ekf2_timestamps {
-			.timestamp = now,
-			.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
-			.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
-			.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
-			.vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
-			.vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
-			.visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
-		};
-
-		UpdateAirspeedSample(ekf2_timestamps);
-		UpdateAuxVelSample(ekf2_timestamps);
-		UpdateBaroSample(ekf2_timestamps);
-		UpdateGpsSample(ekf2_timestamps);
-		UpdateMagSample(ekf2_timestamps);
-		UpdateRangeSample(ekf2_timestamps);
+		UpdateAirspeedSample();
+		UpdateAuxVelSample();
+		UpdateBaroSample();
+		UpdateGpsSample();
+		UpdateMagSample();
+		UpdateRangeSample();
 
 		vehicle_odometry_s ev_odom;
-		const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
+		const bool new_ev_odom = UpdateExtVisionSample(ev_odom);
 
 		optical_flow_s optical_flow;
-		const bool new_optical_flow = UpdateFlowSample(ekf2_timestamps, optical_flow);
+		const bool new_optical_flow = UpdateFlowSample(optical_flow);
 
 
 		// run the EKF update and output
@@ -529,9 +482,6 @@ void EKF2::Run()
 		if (new_optical_flow) {
 			PublishOpticalFlowVel(now, optical_flow);
 		}
-
-		// publish ekf2_timestamps
-		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
 }
 
@@ -545,13 +495,7 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 		q.copyTo(att.q);
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
-		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-		_attitude_pub.publish(att);
-
-	}  else if (_replay_mode) {
-		// in replay mode we have to tell the replay module not to wait for an update
-		// we do this by publishing an attitude with zero timestamp
-		vehicle_attitude_s att{};
+		att.timestamp = hrt_absolute_time();
 		_attitude_pub.publish(att);
 	}
 }
@@ -570,7 +514,7 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
 			baro_bias.innov = status.innov;
 			baro_bias.innov_var = status.innov_var;
 			baro_bias.innov_test_ratio = status.innov_test_ratio;
-			baro_bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+			baro_bias.timestamp = hrt_absolute_time();
 			_estimator_baro_bias_pub.publish(baro_bias);
 
 			_last_baro_bias_published = status.bias;
@@ -590,7 +534,7 @@ void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
 		drift_data.vpos_drift_rate = gps_drift[1];
 		drift_data.hspd = gps_drift[2];
 		drift_data.blocked = blocked;
-		drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		drift_data.timestamp = hrt_absolute_time();
 
 		_ekf_gps_drift_pub.publish(drift_data);
 	}
@@ -649,7 +593,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 		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();
+		event_flags.timestamp = hrt_absolute_time();
 		_estimator_event_flags_pub.publish(event_flags);
 	}
 
@@ -694,7 +638,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 		}
 
 		global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
-		global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		global_pos.timestamp = hrt_absolute_time();
 		_global_position_pub.publish(global_pos);
 	}
 }
@@ -719,7 +663,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu
 	// Not yet supported
 	innovations.aux_vvel = NAN;
 
-	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	innovations.timestamp = hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
 
 	// calculate noise filtered velocity innovations which are used for pre-flight checking
@@ -755,7 +699,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	// Not yet supported
 	test_ratios.aux_vvel = NAN;
 
-	test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	test_ratios.timestamp = hrt_absolute_time();
 	_estimator_innovation_test_ratios_pub.publish(test_ratios);
 }
 
@@ -779,7 +723,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	// Not yet supported
 	variances.aux_vvel = NAN;
 
-	variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	variances.timestamp = hrt_absolute_time();
 	_estimator_innovation_variances_pub.publish(variances);
 }
 
@@ -890,7 +834,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	}
 
 	// publish vehicle local position data
-	lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	lpos.timestamp = hrt_absolute_time();
 	_local_position_pub.publish(lpos);
 }
 
@@ -957,7 +901,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
 
 	// publish vehicle odometry data
-	odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	odom.timestamp = hrt_absolute_time();
 	_odometry_pub.publish(odom);
 }
 
@@ -1050,7 +994,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			_last_mag_bias_published = mag_bias;
 		}
 
-		bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		bias.timestamp = hrt_absolute_time();
 		_estimator_sensor_bias_pub.publish(bias);
 	}
 }
@@ -1063,7 +1007,7 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
 	states.n_states = Ekf::_k_num_states;
 	_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
 	_ekf.covariances_diagonal().copyTo(states.covariances);
-	states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	states.timestamp = hrt_absolute_time();
 	_estimator_states_pub.publish(states);
 }
 
@@ -1117,7 +1061,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 	status.gyro_device_id = _device_id_gyro;
 	status.mag_device_id = _device_id_mag;
 
-	status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	status.timestamp = hrt_absolute_time();
 	_estimator_status_pub.publish(status);
 }
 
@@ -1216,7 +1160,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_optflow_x                = _ekf.innov_check_fail_status_flags().reject_optflow_X;
 		status_flags.reject_optflow_y                = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
 
-		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		status_flags.timestamp = hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
 
 		_last_status_flag_update = status_flags.timestamp;
@@ -1236,7 +1180,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 			       yaw_est_test_data.weight)) {
 
 		yaw_est_test_data.timestamp_sample = timestamp;
-		yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		yaw_est_test_data.timestamp = hrt_absolute_time();
 
 		_yaw_est_pub.publish(yaw_est_test_data);
 	}
@@ -1260,7 +1204,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 		wind.windspeed_east = wind_vel(1);
 		wind.variance_north = wind_vel_var(0);
 		wind.variance_east = wind_vel_var(1);
-		wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		wind.timestamp = hrt_absolute_time();
 
 		_wind_pub.publish(wind);
 	}
@@ -1276,7 +1220,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flo
 	_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
 	_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
 	_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
-	flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	flow_vel.timestamp = hrt_absolute_time();
 
 	_estimator_optical_flow_vel_pub.publish(flow_vel);
 }
@@ -1302,17 +1246,31 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 	return amsl_hgt + _wgs84_hgt_offset;
 }
 
-void EKF2::SelectImuStatus()
+void EKF2::SelectImu()
 {
+	if (!_multi_mode) {
+		for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) {
+			uORB::Subscription vehicle_imu_sub{ORB_ID(vehicle_imu), imu_instance};
+
+			vehicle_imu_s vehicle_imu{};
+			vehicle_imu_sub.copy(&vehicle_imu);
+
+			if (vehicle_imu.accel_device_id == _device_id_accel) {
+				_vehicle_imu_sub.ChangeInstance(imu_instance);
+				break;
+			}
+		}
+	}
+
 	for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) {
-		uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance};
+		uORB::Subscription vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance};
 
-		vehicle_imu_status_s imu_status{};
-		imu_status_sub.copy(&imu_status);
+		vehicle_imu_status_s vehicle_imu_status{};
+		vehicle_imu_status_sub.copy(&vehicle_imu_status);
 
-		if (imu_status.accel_device_id == _device_id_accel) {
+		if (vehicle_imu_status.accel_device_id == _device_id_accel) {
 			_vehicle_imu_status_sub.ChangeInstance(imu_instance);
-			return;
+			break;
 		}
 	}
 
@@ -1326,7 +1284,7 @@ void EKF2::UpdateImuStatus()
 
 	if (_vehicle_imu_status_sub.update(&imu_status)) {
 		if (imu_status.accel_device_id != _device_id_accel) {
-			SelectImuStatus();
+			SelectImu();
 			return;
 		}
 
@@ -1339,7 +1297,7 @@ void EKF2::UpdateImuStatus()
 	}
 }
 
-void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateAirspeedSample()
 {
 	// EKF airspeed sample
 	const unsigned last_generation = _airspeed_sub.get_last_generation();
@@ -1370,13 +1328,10 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 			};
 			_ekf.setAirspeedData(airspeed_sample);
 		}
-
-		ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
 
-void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateAuxVelSample()
 {
 	// EKF auxillary velocity sample
 	//  - use the landing target pose estimate as another source of velocity data
@@ -1404,7 +1359,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
-void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateBaroSample()
 {
 	// EKF baro sample
 	const unsigned last_generation = _airdata_sub.get_last_generation();
@@ -1423,13 +1378,10 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});
 
 		_device_id_baro = airdata.baro_device_id;
-
-		ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
 
-bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
+bool EKF2::UpdateExtVisionSample(vehicle_odometry_s &ev_odom)
 {
 	// EKF external vision sample
 	bool new_ev_odom = false;
@@ -1519,15 +1471,12 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		_ekf.setExtVisionData(ev_data);
 
 		new_ev_odom = true;
-
-		ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 
 	return new_ev_odom;
 }
 
-bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow)
+bool EKF2::UpdateFlowSample(optical_flow_s &optical_flow)
 {
 	// EKF flow sample
 	bool new_optical_flow = false;
@@ -1563,15 +1512,12 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &
 
 			new_optical_flow = true;
 		}
-
-		ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 
 	return new_optical_flow;
 }
 
-void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateGpsSample()
 {
 	// EKF GPS message
 	const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation();
@@ -1614,7 +1560,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
-void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateMagSample()
 {
 	const unsigned last_generation = _magnetometer_sub.get_last_generation();
 	vehicle_magnetometer_s magnetometer;
@@ -1655,13 +1601,10 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 
 		_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
-
-		ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
 
-void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateRangeSample()
 {
 	if (!_distance_sensor_selected) {
 		// get subscription index of first downward-facing range sensor
@@ -1718,9 +1661,6 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 			_last_range_sensor_update = distance_sensor.timestamp;
 			return;
 		}
-
-		ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 
 	if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
@@ -1768,7 +1708,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 				_param_ekf2_mag_decl.set(declination_deg);
 				_mag_decl_saved = true;
 
-				if (!_multi_mode) {
+				if (!_multi_mode || (_multi_mode && _instance == 0)) {
 					_param_ekf2_mag_decl.commit_no_notification();
 				}
 			}
@@ -1784,12 +1724,6 @@ int EKF2::custom_command(int argc, char *argv[])
 int EKF2::task_spawn(int argc, char *argv[])
 {
 	bool success = false;
-	bool replay_mode = false;
-
-	if (argc > 1 && !strcmp(argv[1], "-r")) {
-		PX4_INFO("replay mode enabled");
-		replay_mode = true;
-	}
 
 #if !defined(CONSTRAINED_FLASH)
 	bool multi_mode = false;
@@ -1847,6 +1781,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 		}
 
 		const hrt_abstime time_started = hrt_absolute_time();
+		//PX4_INFO("start time %lu", time_started);
 		const int multi_instances = math::min(imu_instances * mag_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
 		int multi_instances_allocated = 0;
 
@@ -1857,9 +1792,11 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
-		       && ((hrt_elapsed_time(&time_started) < 30_s)
+		       && ((hrt_elapsed_time(&time_started) < 300_s)
 			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
 
+			//PX4_INFO("start time %lu elapsed: %lu", time_started, hrt_elapsed_time(&time_started));
+
 			vehicle_status_sub.update();
 
 			for (uint8_t mag = 0; mag < mag_instances; mag++) {
@@ -1876,7 +1813,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 					    && (vehicle_imu_sub.get().gyro_device_id != 0)) {
 
 						if (!ekf2_instance_created[imu][mag]) {
-							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
+							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu));
 
 							if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
 								int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
@@ -1924,13 +1861,15 @@ int EKF2::task_spawn(int argc, char *argv[])
 			}
 		}
 
-	}
+		//PX4_INFO("start time %lu elapsed: %lu", time_started, hrt_elapsed_time(&time_started));
+
+	} else
 
 #endif // !CONSTRAINED_FLASH
 
-	else {
+	{
 		// otherwise launch regular
-		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
+		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0);
 
 		if (ekf2_inst) {
 			_objects[0].store(ekf2_inst);
@@ -1955,14 +1894,10 @@ Attitude and position estimator using an Extended Kalman Filter. It is used for
 
 The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html) page.
 
-ekf2 can be started in replay mode (`-r`): in this mode it does not access the system time, but only uses the
-timestamps from the sensor topics.
-
 )DESCR_STR");
 
 	PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 #if !defined(CONSTRAINED_FLASH)
 	PRINT_MODULE_USAGE_COMMAND_DESCR("select_instance", "Request switch to new estimator instance");

src/modules/ekf2/EKF2.hpp

@@ -65,7 +65,6 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/ekf_gps_drift.h>
 #include <uORB/topics/estimator_baro_bias.h>
 #include <uORB/topics/estimator_event_flags.h>
@@ -78,7 +77,6 @@
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -102,7 +100,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	EKF2() = delete;
-	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode);
+	EKF2(bool multi_mode, const px4::wq_config_t &config);
 	~EKF2() override;
 
 	/** @see ModuleBase */
@@ -154,16 +152,16 @@ private:
 	void PublishWindEstimate(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 
-	void SelectImuStatus();
+	void SelectImu();
 
-	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
-	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
-	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
-	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
-	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow);
-	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
-	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
-	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateAirspeedSample();
+	void UpdateAuxVelSample();
+	void UpdateBaroSample();
+	bool UpdateExtVisionSample(vehicle_odometry_s &ev_odom);
+	bool UpdateFlowSample(optical_flow_s &optical_flow);
+	void UpdateGpsSample();
+	void UpdateMagSample();
+	void UpdateRangeSample();
 	void UpdateImuStatus();
 
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
@@ -175,7 +173,6 @@ private:
 
 	static constexpr float sq(float x) { return x * x; };
 
-	const bool _replay_mode{false};			///< true when we use replay data from a log
 	const bool _multi_mode;
 	int _instance{0};
 
@@ -247,7 +244,6 @@ private:
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
-	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
 
 	bool _callback_registered{false};
@@ -269,7 +265,6 @@ private:
 	uint32_t _filter_warning_event_changes{0};
 	uint32_t _filter_information_event_changes{0};
 
-	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::PublicationMulti<ekf_gps_drift_s>              _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
 	uORB::PublicationMulti<estimator_baro_bias_s>        _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};

src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py

@@ -122,15 +122,15 @@ def getMagnetometerData(ulog: ULog) -> pd.DataFrame:
 
 def getImuData(ulog: ULog) -> pd.DataFrame:
 
-	sensor_combined = ulog.get_dataset("sensor_combined").data
-	imu = pd.DataFrame({'timestamp': sensor_combined['timestamp'],
+	vehicle_imu = ulog.get_dataset("vehicle_imu").data
+	imu = pd.DataFrame({'timestamp': vehicle_imu['timestamp'],
 		'sensor' : 'imu',
-		'accel_m_s2[0]': sensor_combined["accelerometer_m_s2[0]"],
-		'accel_m_s2[1]': sensor_combined["accelerometer_m_s2[1]"],
-		'accel_m_s2[2]': sensor_combined["accelerometer_m_s2[2]"],
-		'gyro_rad[0]': sensor_combined["gyro_rad[0]"],
-		'gyro_rad[1]': sensor_combined["gyro_rad[1]"],
-		'gyro_rad[2]': sensor_combined["gyro_rad[2]"]})
+		'delta_velocity[0]': vehicle_imu["delta_velocity[0]"],
+		'delta_velocity[1]': vehicle_imu["delta_velocity[1]"],
+		'delta_velocity[2]': vehicle_imu["delta_velocity[2]"],
+		'delta_angle[0]': vehicle_imu["delta_angle[0]"],
+		'delta_angle[1]': vehicle_imu["delta_angle[1]"],
+		'delta_angle[2]': vehicle_imu["delta_angle[2]"]})
 	return imu
 
 def getVehicleLandingStatus(ulog: ULog) -> pd.DataFrame:

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -184,7 +184,7 @@ void BlockLocalPositionEstimator::Run()
 		}
 	}
 
-	sensor_combined_s imu;
+	vehicle_imu_s imu;
 
 	if (!_sensors_sub.update(&imu)) {
 		return;
@@ -914,11 +914,11 @@ void BlockLocalPositionEstimator::updateSSParams()
 	m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
 }
 
-void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
+void BlockLocalPositionEstimator::predict(const vehicle_imu_s &imu)
 {
 	// get acceleration
 	_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
-	Vector3f a(imu.accelerometer_m_s2);
+	const Vector3f a{Vector3f{imu.delta_velocity} * 1.e6f / (float)imu.delta_velocity_dt};
 	// note, bias is removed in dynamics function
 	_u = _R_att * a;
 	_u(U_az) += CONSTANTS_ONE_G;	// add g

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -21,7 +21,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/optical_flow.h>
-#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -184,7 +184,7 @@ private:
 	void updateSSParams();
 
 	// predict the next state
-	void predict(const sensor_combined_s &imu);
+	void predict(const vehicle_imu_s &imu);
 
 	// lidar
 	int  lidarMeasure(Vector<float, n_y_lidar> &y);
@@ -262,7 +262,7 @@ private:
 	// ----------------------------
 
 	// subscriptions
-	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
+	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(vehicle_imu)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};

src/modules/logger/logged_topics.cpp

@@ -82,7 +82,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("rpm", 500);
 	add_topic("rtl_flight_time", 1000);
 	add_topic("safety");
-	add_topic("sensor_combined");
 	add_topic("sensor_correction");
 	add_topic("sensor_gyro_fft", 50);
 	add_topic("sensor_selection");
@@ -92,7 +91,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("tecs_status", 200);
 	add_topic("trajectory_setpoint", 200);
 	add_topic("transponder_report");
-	add_topic("vehicle_acceleration", 50);
+	add_topic("vehicle_acceleration", 20);
 	add_topic("vehicle_air_data", 200);
 	add_topic("vehicle_angular_velocity", 20);
 	add_topic("vehicle_attitude", 50);
@@ -163,7 +162,7 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("sensor_gps", 1000, 2);
 	add_topic_multi("sensor_gyro", 1000, 4);
 	add_topic_multi("sensor_mag", 1000, 4);
-	add_topic_multi("vehicle_imu", 500, 4);
+	add_topic_multi("vehicle_imu", 10, 4);
 	add_topic_multi("vehicle_imu_status", 1000, 4);
 	add_topic_multi("vehicle_magnetometer", 500, 4);
 
@@ -195,12 +194,12 @@ void LoggedTopics::add_high_rate_topics()
 	add_topic("actuator_outputs");
 	add_topic("manual_control_setpoint");
 	add_topic("rate_ctrl_status", 20);
-	add_topic("sensor_combined");
 	add_topic("vehicle_angular_acceleration");
 	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_attitude");
 	add_topic("vehicle_attitude_setpoint");
 	add_topic("vehicle_rates_setpoint");
+	add_topic_multi("vehicle_imu", 0, 4);
 }
 
 void LoggedTopics::add_debug_topics()
@@ -217,21 +216,23 @@ void LoggedTopics::add_debug_topics()
 
 void LoggedTopics::add_estimator_replay_topics()
 {
-	// for estimator replay (need to be at full rate)
-	add_topic("ekf2_timestamps");
-
 	// current EKF2 subscriptions
 	add_topic("airspeed");
+	add_topic("landing_target_pose");
 	add_topic("optical_flow");
-	add_topic("sensor_combined");
+	add_topic("parameter_update");
 	add_topic("sensor_selection");
+	add_topic("sensors_status_imu");
 	add_topic("vehicle_air_data");
+	add_topic("vehicle_command");
 	add_topic("vehicle_gps_position");
 	add_topic("vehicle_land_detected");
-	add_topic("vehicle_magnetometer");
 	add_topic("vehicle_status");
 	add_topic("vehicle_visual_odometry");
-	add_topic_multi("distance_sensor");
+	add_topic_multi("distance_sensor", 0, 2);
+	add_topic_multi("vehicle_imu", 0, 4);
+	add_topic_multi("vehicle_imu_status", 0, 4);
+	add_topic_multi("vehicle_magnetometer", 0, 4);
 }
 
 void LoggedTopics::add_thermal_calibration_topics()
@@ -274,9 +275,9 @@ void LoggedTopics::add_system_identification_topics()
 	// for system id need to log imu and controls at full rate
 	add_topic("actuator_controls_0");
 	add_topic("actuator_controls_1");
-	add_topic("sensor_combined");
 	add_topic("vehicle_angular_acceleration");
 	add_topic("vehicle_angular_acceleration_setpoint");
+	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_torque_setpoint");
 }
 

src/modules/logger/logger.cpp

@@ -663,6 +663,8 @@ void Logger::run()
 
 	if (polling_topic_sub >= 0) {
 		_lockstep_component = px4_lockstep_register_component();
+
+		fprintf(stderr, "logger: px4_lockstep_register_component:%d\n", _lockstep_component);
 	}
 
 	bool was_started = false;

src/modules/mavlink/streams/SCALED_IMU.hpp

@@ -84,7 +84,7 @@ private:
 				msg.zacc = (int16_t)accel(2);
 
 				// Gyroscope in mrad/s
-				const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
+				const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
 				const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
 				msg.xgyro = gyro(0);
 				msg.ygyro = gyro(1);

src/modules/mavlink/streams/SCALED_IMU2.hpp

@@ -84,7 +84,7 @@ private:
 				msg.zacc = (int16_t)accel(2);
 
 				// Gyroscope in mrad/s
-				const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
+				const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
 				const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
 				msg.xgyro = gyro(0);
 				msg.ygyro = gyro(1);

src/modules/mavlink/streams/SCALED_IMU3.hpp

@@ -84,7 +84,7 @@ private:
 				msg.zacc = (int16_t)accel(2);
 
 				// Gyroscope in mrad/s
-				const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
+				const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
 				const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
 				msg.xgyro = gyro(0);
 				msg.ygyro = gyro(1);

src/modules/replay/CMakeLists.txt

@@ -39,6 +39,4 @@ px4_add_module(
 		replay_main.cpp
 		Replay.cpp
 		Replay.hpp
-		ReplayEkf2.cpp
-		ReplayEkf2.hpp
 	)

src/modules/replay/Replay.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -62,7 +62,6 @@
 #include <logger/messages.h>
 
 #include "Replay.hpp"
-#include "ReplayEkf2.hpp"
 
 #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt"
 
@@ -74,16 +73,6 @@ namespace px4
 
 char *Replay::_replay_file = nullptr;
 
-Replay::CompatSensorCombinedDtType::CompatSensorCombinedDtType(int gyro_integral_dt_offset_log,
-		int gyro_integral_dt_offset_intern, int accelerometer_integral_dt_offset_log,
-		int accelerometer_integral_dt_offset_intern)
-	: _gyro_integral_dt_offset_log(gyro_integral_dt_offset_log),
-	  _gyro_integral_dt_offset_intern(gyro_integral_dt_offset_intern),
-	  _accelerometer_integral_dt_offset_log(accelerometer_integral_dt_offset_log),
-	  _accelerometer_integral_dt_offset_intern(accelerometer_integral_dt_offset_intern)
-{
-}
-
 Replay::~Replay()
 {
 	for (size_t i = 0; i < _subscriptions.size(); ++i) {
@@ -93,27 +82,6 @@ Replay::~Replay()
 	_subscriptions.clear();
 }
 
-void *
-Replay::CompatSensorCombinedDtType::apply(void *data)
-{
-	// the types have the same size so we can do the conversion in-place
-	uint8_t *ptr = (uint8_t *)data;
-
-	float gyro_integral_dt;
-	memcpy(&gyro_integral_dt, ptr + _gyro_integral_dt_offset_log, sizeof(float));
-
-	float accel_integral_dt;
-	memcpy(&accel_integral_dt, ptr + _accelerometer_integral_dt_offset_log, sizeof(float));
-
-	uint32_t igyro_integral_dt = (uint32_t)(gyro_integral_dt * 1e6f);
-	memcpy(ptr + _gyro_integral_dt_offset_intern, &igyro_integral_dt, sizeof(float));
-
-	uint32_t iaccel_integral_dt = (uint32_t)(accel_integral_dt * 1e6f);
-	memcpy(ptr + _accelerometer_integral_dt_offset_intern, &iaccel_integral_dt, sizeof(float));
-
-	return data;
-}
-
 void
 Replay::setupReplayFile(const char *file_name)
 {
@@ -237,6 +205,8 @@ Replay::readFileDefinitions(std::ifstream &file)
 			break;
 
 		case (int)ULogMessageType::PARAMETER:
+
+			// case (int)ULogMessageType::PARAMETER_DEFAULT: // TODO
 			if (!readAndApplyParameter(file, message_header.msg_size)) {
 				return false;
 			}
@@ -249,6 +219,7 @@ Replay::readFileDefinitions(std::ifstream &file)
 
 		case (int)ULogMessageType::INFO: //skip
 		case (int)ULogMessageType::INFO_MULTIPLE: //skip
+		case (int)ULogMessageType::PARAMETER_DEFAULT: // TODO
 			file.seekg(message_header.msg_size, ios::cur);
 			break;
 
@@ -365,51 +336,21 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 		return true;
 	}
 
-	CompatBase *compat = nullptr;
 
 	// check the format: the field definitions must match
 	// FIXME: this should check recursively, all used nested types
 	string file_format = _file_formats[topic_name];
 
 	if (file_format != orb_meta->o_fields) {
-		// check if we have a compatibility conversion available
-		if (topic_name == "sensor_combined") {
-			if (string(orb_meta->o_fields) == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;"
-			    "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
-			    "uint32_t accelerometer_integral_dt" &&
-			    file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;"
-			    "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
-			    "float accelerometer_integral_dt;") {
-
-				int gyro_integral_dt_offset_log;
-				int gyro_integral_dt_offset_intern;
-				int accelerometer_integral_dt_offset_log;
-				int accelerometer_integral_dt_offset_intern;
-				int unused;
-
-				if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) &&
-				    findFieldOffset(orb_meta->o_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) &&
-				    findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) &&
-				    findFieldOffset(orb_meta->o_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) {
-
-					compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern,
-										accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern);
-				}
-			}
-		}
-
-		if (!compat) {
-			PX4_ERR("Formats for %s don't match. Will ignore it.", topic_name.c_str());
-			PX4_WARN(" Internal format: %s", orb_meta->o_fields);
-			PX4_WARN(" File format    : %s", file_format.c_str());
-			return true; // not a fatal error
-		}
+		PX4_ERR("Formats for %s don't match. Will ignore it.", topic_name.c_str());
+		PX4_WARN(" Internal format: %s", orb_meta->o_fields);
+		PX4_WARN(" File format    : %s", file_format.c_str());
+		return true; // not a fatal error
 	}
 
 	Subscription *subscription = new Subscription();
 	subscription->orb_meta = orb_meta;
 	subscription->multi_id = multi_id;
-	subscription->compat = compat;
 
 	//find the timestamp offset
 	int field_size;
@@ -764,6 +705,15 @@ Replay::readDefinitionsAndApplyParams(std::ifstream &file)
 void
 Replay::run()
 {
+	uint64_t timestamp_last = 0;
+
+	// time starts at 0
+	{
+		struct timespec ts {};
+		abstime_to_ts(&ts, timestamp_last);
+		px4_clock_settime(CLOCK_MONOTONIC, &ts);
+	}
+
 	ifstream replay_file(_replay_file, ios::in | ios::binary);
 
 	if (!readDefinitionsAndApplyParams(replay_file)) {
@@ -777,10 +727,6 @@ Replay::run()
 		_speed_factor = atof(speedup);
 	}
 
-	onEnterMainLoop();
-
-	_replay_start_time = hrt_absolute_time();
-
 	PX4_INFO("Replay in progress...");
 
 	ulog_message_header_s message_header;
@@ -794,14 +740,17 @@ Replay::run()
 		return;
 	}
 
-	const uint64_t timestamp_offset = getTimestampOffset();
 	uint32_t nr_published_messages = 0;
 	streampos last_additional_message_pos = _data_section_start;
 
-	while (!should_exit() && replay_file) {
+	int imu_publish_count = 0;
+	bool imu_published = false;
 
-		//Find the next message to publish. Messages from different subscriptions don't need
-		//to be in chronological order, so we need to check all subscriptions
+	hrt_abstime last_update = 0;
+
+	while (!should_exit() && replay_file) {
+		// Find the next message to publish. Messages from different subscriptions don't need
+		// to be in chronological order, so we need to check all subscriptions
 		uint64_t next_file_time = 0;
 		int next_msg_id = -1;
 		bool first_time = true;
@@ -823,36 +772,99 @@ Replay::run()
 		}
 
 		if (next_msg_id == -1) {
-			break; //no active subscription anymore. We're done.
+			break; // no active subscription anymore. We're done.
 		}
 
 		Subscription &sub = *_subscriptions[next_msg_id];
 
 		if (next_file_time == 0) {
-			//someone didn't set the timestamp properly. Consider the message invalid
+			// someone didn't set the timestamp properly. Consider the message invalid
+			PX4_ERR("%s:%d missing timestamp", sub.orb_meta->o_name, sub.multi_id);
 			nextDataMessage(replay_file, sub, next_msg_id);
 			continue;
 		}
 
-		//handle additional messages between last and next published data
+
+		// ekf2 replay
+		if (sub.orb_meta &&
+		    ((strcmp(sub.orb_meta->o_name, "estimator_status") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "estimator_local_position") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "estimator_global_position") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "estimator_attitude") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "estimator_sensor_bias") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "ekf_gps_drift") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "vehicle_attitude") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "vehicle_local_position") == 0)
+		     || (strcmp(sub.orb_meta->o_name, "vehicle_global_position") == 0))
+		   ) {
+			// skipping blocked message
+			nextDataMessage(replay_file, sub, next_msg_id);
+			continue;
+		}
+
+		// handle additional messages between last and next published data
 		replay_file.seekg(last_additional_message_pos);
 		streampos next_additional_message_pos = sub.next_read_pos;
 		readAndHandleAdditionalMessages(replay_file, next_additional_message_pos);
 		last_additional_message_pos = next_additional_message_pos;
 
-		const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset);
+		const uint64_t publish_timestamp = sub.next_timestamp;
+
+		// adjust the lockstep time to the publication time
+		struct timespec ts {};
+		abstime_to_ts(&ts, publish_timestamp);
+		px4_clock_settime(CLOCK_MONOTONIC, &ts);
+
+		if (_replay_start_time == 0) {
+			_replay_start_time = hrt_absolute_time();
+		}
 
 		// It's time to publish
 		readTopicDataToBuffer(sub, replay_file);
-		memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp
 
-		if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) {
+		if (publish_timestamp >= timestamp_last) {
+			PX4_DEBUG("%lu [%lu] (+%lu us) publishing %s:%d", hrt_absolute_time(), publish_timestamp,
+				  publish_timestamp - timestamp_last,
+				  sub.orb_meta->o_name, sub.multi_id);
+
+		} else {
+			PX4_ERR("%s:%d bad timestamp %lu (last timestamp %lu)", sub.orb_meta->o_name, sub.multi_id, publish_timestamp,
+				timestamp_last);
+		}
+
+		if (publishTopic(sub, _read_buffer.data())) {
 			++nr_published_messages;
 		}
 
+		timestamp_last = publish_timestamp;
+
 		nextDataMessage(replay_file, sub, next_msg_id);
 
 		// TODO: output status (eg. every sec), including total duration...
+
+
+		// Wait for other modules, such as logger or ekf2
+		if (imu_published) {
+			px4_lockstep_wait_for_components();
+
+		} else {
+			if (sub.orb_meta) {
+				if (strcmp(sub.orb_meta->o_name, "vehicle_imu") == 0) {
+					imu_publish_count++;
+
+					if (imu_publish_count > 1000) {
+						//imu_published = true;
+					}
+				}
+			}
+
+			//system_usleep(1);
+		}
+
+		if (hrt_elapsed_time(&last_update) >= 1_s) {
+			PX4_INFO("[%.6fs] %d messages published", timestamp_last * 1e-6, nr_published_messages);
+			last_update = timestamp_last;
+		}
 	}
 
 	for (auto &subscription : _subscriptions) {
@@ -860,11 +872,6 @@ Replay::run()
 			continue;
 		}
 
-		if (subscription->compat) {
-			delete subscription->compat;
-			subscription->compat = nullptr;
-		}
-
 		if (subscription->orb_advert) {
 			orb_unadvertise(subscription->orb_advert);
 			subscription->orb_advert = nullptr;
@@ -876,8 +883,6 @@ Replay::run()
 			 (double)hrt_elapsed_time(&_replay_start_time) / 1.e6);
 	}
 
-	onExitMainLoop();
-
 	if (!should_exit()) {
 		replay_file.close();
 		px4_shutdown_request();
@@ -903,50 +908,11 @@ Replay::readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_fil
 	replay_file.read((char *)_read_buffer.data(), msg_read_size);
 }
 
-bool
-Replay::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
-{
-	return publishTopic(sub, data);
-}
-
-uint64_t
-Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset)
-{
-	const uint64_t publish_timestamp = next_file_time + timestamp_offset;
-
-	// wait if necessary
-	uint64_t cur_time = hrt_absolute_time();
-
-	// if some topics have a timestamp smaller than the log file start, publish them immediately
-	if (cur_time < publish_timestamp && next_file_time > _file_start_time) {
-		if (_speed_factor > FLT_EPSILON) {
-			// avoid many small usleep calls
-			_accumulated_delay += (publish_timestamp - cur_time) / _speed_factor;
-
-			if (_accumulated_delay > 3000) {
-				system_usleep(_accumulated_delay);
-				_accumulated_delay = 0.f;
-			}
-		}
-
-		// adjust the lockstep time to the publication time
-		struct timespec ts;
-		abstime_to_ts(&ts, publish_timestamp);
-		px4_clock_settime(CLOCK_MONOTONIC, &ts);
-	}
-
-	return publish_timestamp;
-}
-
 bool
 Replay::publishTopic(Subscription &sub, void *data)
 {
 	bool published = false;
 
-	if (sub.compat) {
-		data = sub.compat->apply(data);
-	}
-
 	if (sub.orb_advert) {
 		orb_publish(sub.orb_meta, sub.orb_advert, data);
 		published = true;
@@ -988,6 +954,142 @@ Replay::publishTopic(Subscription &sub, void *data)
 	return published;
 }
 
+bool Replay::startsWith(const char *pre, const char *str)
+{
+	size_t lenpre = strlen(pre);
+	size_t lenstr = strlen(str);
+	return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0;
+}
+
+bool Replay::findTopic(const PublisherRule &rule, const char *topic_name)
+{
+	const char **topics_ptr = rule.topics;
+
+	while (*topics_ptr) {
+		if (strcmp(*topics_ptr, topic_name) == 0) {
+			return true;
+		}
+
+		++topics_ptr;
+	}
+
+	return false;
+}
+
+void Replay::strTrim(const char **str)
+{
+	while (**str == ' ' || **str == '\t') { ++(*str); }
+}
+
+int Replay::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
+{
+	FILE *fp;
+	static const int line_len = 1024;
+	int ret = PX4_OK;
+	char *line = new char[line_len];
+
+	if (!line) {
+		return -ENOMEM;
+	}
+
+	fp = fopen(file_name, "r");
+
+	if (fp == NULL) {
+		delete[](line);
+		return -errno;
+	}
+
+	const char *restrict_topics_str = "restrict_topics:";
+	const char *module_str = "module:";
+	const char *ignore_others = "ignore_others:";
+
+	rule.ignore_other_topics = false;
+	rule.module_name = nullptr;
+	rule.topics = nullptr;
+
+	while (fgets(line, line_len, fp) && ret == PX4_OK) {
+
+		if (strlen(line) < 2 || line[0] == '#') {
+			continue;
+		}
+
+		if (startsWith(restrict_topics_str, line)) {
+			//read topics list
+			char *start = line + strlen(restrict_topics_str);
+			strTrim((const char **)&start);
+			char *topics = strdup(start);
+			int topic_len = 0, num_topics = 0;
+
+			for (int i = 0; topics[i]; ++i) {
+				if (topics[i] == ',' || topics[i] == '\n') {
+					if (topic_len > 0) {
+						topics[i] = 0;
+						++num_topics;
+					}
+
+					topic_len = 0;
+
+				} else {
+					++topic_len;
+				}
+			}
+
+			if (num_topics > 0) {
+				rule.topics = new const char *[num_topics + 1];
+				int topic = 0;
+				strTrim((const char **)&topics);
+				rule.topics[topic++] = topics;
+
+				while (topic < num_topics) {
+					if (*topics == 0) {
+						++topics;
+						strTrim((const char **)&topics);
+						rule.topics[topic++] = topics;
+
+					} else {
+						++topics;
+					}
+				}
+
+				rule.topics[num_topics] = nullptr;
+			}
+
+		} else if (startsWith(module_str, line)) {
+			//read module name
+			char *start = line + strlen(module_str);
+			strTrim((const char **)&start);
+			int len = strlen(start);
+
+			if (len > 0 && start[len - 1] == '\n') {
+				start[len - 1] = 0;
+			}
+
+			rule.module_name = strdup(start);
+
+		} else if (startsWith(ignore_others, line)) {
+			const char *start = line + strlen(ignore_others);
+			strTrim(&start);
+
+			if (startsWith("true", start)) {
+				rule.ignore_other_topics = true;
+			}
+
+		} else {
+			PX4_ERR("orb rules file: wrong format: %s", line);
+			ret = -EINVAL;
+		}
+	}
+
+	if (ret == PX4_OK && (!rule.module_name || !rule.topics)) {
+		PX4_ERR("Wrong format in orb publisher rules file");
+		ret = -EINVAL;
+	}
+
+	delete[](line);
+	fclose(fp);
+	return ret;
+}
+
 int
 Replay::custom_command(int argc, char *argv[])
 {
@@ -999,6 +1101,8 @@ Replay::custom_command(int argc, char *argv[])
 		return Replay::task_spawn(argc, argv);
 	}
 
+	// ignore?
+
 	return print_usage("unknown command");
 }
 
@@ -1065,17 +1169,9 @@ Replay *
 Replay::instantiate(int argc, char *argv[])
 {
 	// check the replay mode
-	const char *replay_mode = getenv(replay::ENV_MODE);
+	//const char *replay_mode = getenv(replay::ENV_MODE);
 
-	Replay *instance = nullptr;
-
-	if (replay_mode && strcmp(replay_mode, "ekf2") == 0) {
-		PX4_INFO("Ekf2 replay mode");
-		instance = new ReplayEkf2();
-
-	} else {
-		instance = new Replay();
-	}
+	Replay *instance = new Replay();
 
 	return instance;
 }

src/modules/replay/Replay.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,7 +43,6 @@
 
 #include <px4_platform_common/module.h>
 #include <uORB/topics/uORBTopics.hpp>
-#include <uORB/topics/ekf2_timestamps.h>
 
 namespace px4
 {
@@ -95,36 +94,6 @@ public:
 
 protected:
 
-	/**
-	 * @class Compatibility base class to convert topics to an updated format
-	 */
-	class CompatBase
-	{
-	public:
-		virtual ~CompatBase() = default;
-
-		/**
-		 * apply compatibility to a topic
-		 * @param data input topic (can be modified in place)
-		 * @return new topic data
-		 */
-		virtual void *apply(void *data) = 0;
-	};
-
-	class CompatSensorCombinedDtType : public CompatBase
-	{
-	public:
-		CompatSensorCombinedDtType(int gyro_integral_dt_offset_log, int gyro_integral_dt_offset_intern,
-					   int accelerometer_integral_dt_offset_log, int accelerometer_integral_dt_offset_intern);
-
-		void *apply(void *data) override;
-	private:
-		int _gyro_integral_dt_offset_log;
-		int _gyro_integral_dt_offset_intern;
-		int _accelerometer_integral_dt_offset_log;
-		int _accelerometer_integral_dt_offset_intern;
-	};
-
 	struct Subscription {
 
 		const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid
@@ -137,8 +106,6 @@ protected:
 		std::streampos next_read_pos;
 		uint64_t next_timestamp; ///< timestamp of the file
 
-		CompatBase *compat = nullptr;
-
 		// statistics
 		int error_counter = 0;
 		int publication_counter = 0;
@@ -162,34 +129,16 @@ protected:
 	 */
 	bool publishTopic(Subscription &sub, void *data);
 
-	/**
-	 * called when entering the main replay loop
-	 */
-	virtual void onEnterMainLoop() {}
-
-	/**
-	 * called when exiting the main replay loop
-	 */
-	virtual void onExitMainLoop() {}
-
 	/**
 	 * called when a new subscription is added
 	 */
-	virtual void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {}
-
-	/**
-	 * handle delay until topic can be published.
-	 * @param next_file_timestamp timestamp of next message to publish
-	 * @param timestamp_offset offset between file start time and replay start time
-	 * @return timestamp that the message to publish should have
-	 */
-	virtual uint64_t handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset);
+	void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {}
 
 	/**
 	 * handle the publication of a topic update
 	 * @return true if published, false otherwise
 	 */
-	virtual bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
+	bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
 
 	/**
 	 * read a topic from the file (offset given by the subscription) into _read_buffer
@@ -206,7 +155,7 @@ protected:
 	 */
 	bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
 
-	virtual uint64_t getTimestampOffset()
+	uint64_t getTimestampOffset()
 	{
 		//we update the timestamps from the file by a constant offset to match
 		//the current replay time
@@ -278,6 +227,46 @@ private:
 	void setUserParams(const char *filename);
 
 	static char *_replay_file;
+
+
+
+
+	struct PublisherRule {
+		const char **topics; //null-terminated list of topic names
+		const char *module_name; //only this module is allowed to publish one of the topics
+		bool ignore_other_topics;
+	};
+
+	/**
+	 * test if str starts with pre
+	 */
+	bool startsWith(const char *pre, const char *str);
+
+	/**
+	 * find a topic in a rule
+	 */
+	bool findTopic(const PublisherRule &rule, const char *topic_name);
+
+	/**
+	 * trim whitespace from the beginning of a string
+	 */
+	void strTrim(const char **str);
+
+	/**
+	 * Read publisher rules from a file. It has the format:
+	 *
+	 * restrict_topics: <topic1>, <topic2>, <topic3>
+	 * module: <module_name>
+	 * [ignore_others:true]
+	 *
+	 * @return 0 on success, <0 otherwise
+	 */
+	int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
+
+	PublisherRule _publisher_rule;
+	bool _has_publisher_rules = false;
+
+
 };
 
 } //namespace px4

src/modules/replay/ReplayEkf2.cpp

@@ -1,214 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/posix.h>
-
-// for ekf2 replay
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_air_data.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_magnetometer.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_odometry.h>
-
-#include "ReplayEkf2.hpp"
-
-namespace px4
-{
-
-bool
-ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
-{
-	if (sub.orb_meta == ORB_ID(ekf2_timestamps)) {
-		ekf2_timestamps_s ekf2_timestamps;
-		memcpy(&ekf2_timestamps, data, sub.orb_meta->o_size);
-
-		if (!publishEkf2Topics(ekf2_timestamps, replay_file)) {
-			return false;
-		}
-
-		// Wait for modules to process the data
-		px4_lockstep_wait_for_components();
-
-		return true;
-
-	} else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected)
-		   || sub.orb_meta == ORB_ID(vehicle_gps_position)) {
-		return publishTopic(sub, data);
-	} // else: do not publish
-
-	return false;
-}
-
-void
-ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
-{
-	if (sub.orb_meta == ORB_ID(sensor_combined)) {
-		_sensor_combined_msg_id = msg_id;
-
-	} else if (sub.orb_meta == ORB_ID(airspeed)) {
-		_airspeed_msg_id = msg_id;
-
-	} else if (sub.orb_meta == ORB_ID(distance_sensor)) {
-		_distance_sensor_msg_id = msg_id;
-
-	} else if (sub.orb_meta == ORB_ID(optical_flow)) {
-		_optical_flow_msg_id = msg_id;
-
-	} else if (sub.orb_meta == ORB_ID(vehicle_air_data)) {
-		_vehicle_air_data_msg_id = msg_id;
-
-	} else if (sub.orb_meta == ORB_ID(vehicle_magnetometer)) {
-		_vehicle_magnetometer_msg_id = msg_id;
-
-	} else if (sub.orb_meta == ORB_ID(vehicle_visual_odometry)) {
-		_vehicle_visual_odometry_msg_id = msg_id;
-	}
-
-	// the main loop should only handle publication of the following topics, the sensor topics are
-	// handled separately in publishEkf2Topics()
-	// Note: the GPS is not treated here since not missing data is more important than the accuracy of the timestamp
-	sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status)
-		      && sub.orb_meta != ORB_ID(vehicle_land_detected) && sub.orb_meta != ORB_ID(vehicle_gps_position);
-}
-
-bool
-ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)
-{
-	auto handle_sensor_publication = [&](int16_t timestamp_relative, uint16_t msg_id) {
-		if (timestamp_relative != ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID) {
-			// timestamp_relative is already given in 0.1 ms
-			uint64_t t = timestamp_relative + ekf2_timestamps.timestamp / 100; // in 0.1 ms
-			findTimestampAndPublish(t, msg_id, replay_file);
-		}
-	};
-
-	handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id);
-	handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id);
-	handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id);
-	handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
-	handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id);
-	handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id);
-
-	// sensor_combined: publish last because ekf2 is polling on this
-	if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {
-		if (_sensor_combined_msg_id == msg_id_invalid) {
-			// subscription not found yet or sensor_combined not contained in log
-			return false;
-
-		} else if (!_subscriptions[_sensor_combined_msg_id]->orb_meta) {
-			return false; // read past end of file
-
-		} else {
-			// we should publish a topic, just publish the same again
-			readTopicDataToBuffer(*_subscriptions[_sensor_combined_msg_id], replay_file);
-			publishTopic(*_subscriptions[_sensor_combined_msg_id], _read_buffer.data());
-		}
-	}
-
-	return true;
-}
-
-bool
-ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file)
-{
-	if (msg_id == msg_id_invalid) {
-		// could happen if a topic is not logged
-		return false;
-	}
-
-	Subscription &sub = *_subscriptions[msg_id];
-
-	while (sub.next_timestamp / 100 < timestamp && sub.orb_meta) {
-		nextDataMessage(replay_file, sub, msg_id);
-	}
-
-	if (!sub.orb_meta) { // no messages anymore
-		return false;
-	}
-
-	if (sub.next_timestamp / 100 != timestamp) {
-		// this can happen in beginning of the log or on a dropout
-		PX4_DEBUG("No timestamp match found for topic %s (%i, %i)", sub.orb_meta->o_name, (int)sub.next_timestamp / 100,
-			  timestamp);
-		++sub.error_counter;
-		return false;
-	}
-
-	readTopicDataToBuffer(sub, replay_file);
-	publishTopic(sub, _read_buffer.data());
-	return true;
-}
-
-void
-ReplayEkf2::onEnterMainLoop()
-{
-	_speed_factor = 0.f; // iterate as fast as possible
-}
-
-void
-ReplayEkf2::onExitMainLoop()
-{
-	// print statistics
-	auto print_sensor_statistics = [this](uint16_t msg_id, const char *name) {
-		if (msg_id != msg_id_invalid) {
-			Subscription &sub = *_subscriptions[msg_id];
-
-			if (sub.publication_counter > 0 || sub.error_counter > 0) {
-				PX4_INFO("%s: %i, %i", name, sub.publication_counter, sub.error_counter);
-			}
-		}
-	};
-
-	PX4_INFO("");
-	PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):");
-
-	print_sensor_statistics(_airspeed_msg_id, "airspeed");
-	print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor");
-	print_sensor_statistics(_optical_flow_msg_id, "optical_flow");
-	print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined");
-	print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
-	print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
-	print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry");
-}
-
-} // namespace px4

src/modules/replay/ReplayEkf2.hpp

@@ -1,93 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "Replay.hpp"
-
-namespace px4
-{
-
-/**
- * @class ReplayEkf2
- * replay specialization for Ekf2 replay
- */
-class ReplayEkf2 : public Replay
-{
-public:
-protected:
-
-	void onEnterMainLoop() override;
-	void onExitMainLoop() override;
-
-	/**
-	 * handle ekf2 topic publication in ekf2 replay mode
-	 * @param sub
-	 * @param data
-	 * @param replay_file file currently replayed (file seek position should be considered arbitrary after this call)
-	 * @return true if published, false otherwise
-	 */
-	bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override;
-
-	void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override;
-
-	uint64_t getTimestampOffset() override
-	{
-		// avoid offsetting timestamps as we use them to compare against the log
-		return 0;
-	}
-private:
-
-	bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file);
-
-	/**
-	 * find the next message for a subscription that matches a given timestamp and publish it
-	 * @param timestamp in 0.1 ms
-	 * @param msg_id
-	 * @param replay_file file currently replayed (file seek position should be considered arbitrary after this call)
-	 * @return true if timestamp found and published
-	 */
-	bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file);
-
-	static constexpr uint16_t msg_id_invalid = 0xffff;
-
-	uint16_t _airspeed_msg_id = msg_id_invalid;
-	uint16_t _distance_sensor_msg_id = msg_id_invalid;
-	uint16_t _optical_flow_msg_id = msg_id_invalid;
-	uint16_t _sensor_combined_msg_id = msg_id_invalid;
-	uint16_t _vehicle_air_data_msg_id = msg_id_invalid;
-	uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid;
-	uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid;
-};
-
-} //namespace px4

src/modules/replay/replay_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,8 +35,7 @@
 
 using namespace px4;
 
-extern "C" __EXPORT int
-replay_main(int argc, char *argv[])
+extern "C" __EXPORT int replay_main(int argc, char *argv[])
 {
 	//check for logfile env variable
 	const char *logfile = getenv(replay::ENV_FILENAME);

src/modules/sensors/sensors.cpp

@@ -115,9 +115,6 @@ private:
 	bool		_armed{false};				/**< arming status of the vehicle */
 
 	hrt_abstime     _last_config_update{0};
-	hrt_abstime     _sensor_combined_prev_timestamp{0};
-
-	sensor_combined_s _sensor_combined{};
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] {
 		{this, ORB_ID(vehicle_imu), 0},
@@ -133,7 +130,6 @@ private:
 	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
 	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
-	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -632,7 +628,7 @@ void Sensors::Run()
 		InitializeVehicleIMU();
 		InitializeVehicleGPSPosition();
 		InitializeVehicleMagnetometer();
-		_voted_sensors_update.init(_sensor_combined);
+		_voted_sensors_update.init();
 		parameter_update_poll(true);
 	}
 
@@ -650,20 +646,13 @@ void Sensors::Run()
 		}
 	}
 
-	_voted_sensors_update.sensorsPoll(_sensor_combined);
+	_voted_sensors_update.sensorsPoll();
 
 	// check analog airspeed
 	adc_poll();
 
 	diff_pres_poll();
 
-	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
-
-		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
-		_sensor_pub.publish(_sensor_combined);
-		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
-	}
-
 	// keep adding sensors as long as we are not armed,
 	// when not adding sensors poll for param updates
 	if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) {
@@ -814,8 +803,7 @@ it into a more usable form, and publishes it for the rest of the system.
 The provided functionality includes:
 - Read the output from the sensor drivers (`sensor_gyro`, etc.).
   If there are multiple of the same type, do voting and failover handling.
-  Then apply the board rotation and temperature calibration (if enabled). And finally publish the data; one of the
-  topics is `sensor_combined`, used by many parts of the system.
+  Then apply the board rotation and temperature calibration (if enabled). And finally publish the selection.
 - Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
   on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
   sensor drivers must already be running when `sensors` is started.

src/modules/sensors/voted_sensors_update.cpp

@@ -60,11 +60,8 @@ VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled,
 	}
 }
 
-int VotedSensorsUpdate::init(sensor_combined_s &raw)
+int VotedSensorsUpdate::init()
 {
-	raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
-	raw.timestamp = 0;
-
 	initializeSensors();
 
 	_selection_changed = true;
@@ -138,7 +135,7 @@ void VotedSensorsUpdate::parametersUpdate()
 	}
 }
 
-void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
+void VotedSensorsUpdate::imuPoll()
 {
 	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
 		vehicle_imu_s imu_report;
@@ -151,36 +148,21 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 			_vehicle_imu_status_subs[uorb_index].copy(&imu_status);
 
 			_accel_device_id[uorb_index] = imu_report.accel_device_id;
-			_gyro_device_id[uorb_index] = imu_report.gyro_device_id;
-
 			// convert the delta velocities to an equivalent acceleration
 			const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt;
-			Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv;
-
+			_accel_last[uorb_index] = Vector3f{imu_report.delta_velocity} * accel_dt_inv;
+			float accel_array[3];
+			_accel_last[uorb_index].copyTo(accel_array);
+			_accel.voter.put(uorb_index, imu_report.timestamp, accel_array, imu_status.accel_error_count,
+					 _accel.priority[uorb_index]);
 
+			_gyro_device_id[uorb_index] = imu_report.gyro_device_id;
 			// convert the delta angles to an equivalent angular rate
 			const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt;
-			Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv;
-
-			_last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample;
-			_last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0);
-			_last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1);
-			_last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2);
-			_last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt;
-			_last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping;
-			_last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0);
-			_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
-			_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
-			_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
-
-
-			_last_accel_timestamp[uorb_index] = imu_report.timestamp_sample;
-
-			_accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
-					 imu_status.accel_error_count, _accel.priority[uorb_index]);
-
-			_gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad,
-					imu_status.gyro_error_count, _gyro.priority[uorb_index]);
+			_gyro_last[uorb_index] = Vector3f{imu_report.delta_angle} * gyro_dt_inv;
+			float gyro_array[3];
+			_gyro_last[uorb_index].copyTo(gyro_array);
+			_gyro.voter.put(uorb_index, imu_report.timestamp, gyro_array, imu_status.gyro_error_count, _gyro.priority[uorb_index]);
 		}
 	}
 
@@ -221,14 +203,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 
 	// write data for the best sensor to output variables
 	if ((accel_best_index >= 0) && (gyro_best_index >= 0)) {
-		raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
-		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
-		       sizeof(raw.accelerometer_m_s2));
-		memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad));
-		raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
-		raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt;
-		raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping;
-
 		if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
 			_accel.last_best_vote = (uint8_t)accel_best_index;
 			_selection.accel_device_id = _accel_device_id[accel_best_index];
@@ -386,9 +360,9 @@ void VotedSensorsUpdate::printStatus()
 	_accel.voter.print();
 }
 
-void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
+void VotedSensorsUpdate::sensorsPoll()
 {
-	imuPoll(raw);
+	imuPoll();
 
 	calcAccelInconsistency();
 	calcGyroInconsistency();
@@ -423,14 +397,6 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 	_sensors_status_imu_pub.publish(status);
 }
 
-void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
-{
-	if (_last_accel_timestamp[_accel.last_best_vote]) {
-		raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] -
-						       (int64_t)raw.timestamp);
-	}
-}
-
 void VotedSensorsUpdate::calcAccelInconsistency()
 {
 	Vector3f accel_mean{};
@@ -440,7 +406,7 @@ void VotedSensorsUpdate::calcAccelInconsistency()
 	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
 		if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
 			accel_count++;
-			accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2};
+			accel_all[sensor_index] = _accel_last[sensor_index];
 			accel_mean += accel_all[sensor_index];
 		}
 	}
@@ -465,7 +431,7 @@ void VotedSensorsUpdate::calcGyroInconsistency()
 	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
 		if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
 			gyro_count++;
-			gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad};
+			gyro_all[sensor_index] = _gyro_last[sensor_index];
 			gyro_mean += gyro_all[sensor_index];
 		}
 	}

src/modules/sensors/voted_sensors_update.h

@@ -53,7 +53,6 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensors_status_imu.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_imu.h>
@@ -82,7 +81,7 @@ public:
 	 * initialize subscriptions etc.
 	 * @return 0 on success, <0 otherwise
 	 */
-	int init(sensor_combined_s &raw);
+	int init();
 
 	/**
 	 * This tries to find new sensor instances. This is called from init(), then it can be called periodically.
@@ -100,13 +99,7 @@ public:
 	/**
 	 * read new sensor data
 	 */
-	void sensorsPoll(sensor_combined_s &raw);
-
-	/**
-	 * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll,
-	 * so that the data can be published.
-	 */
-	void setRelativeTimestamps(sensor_combined_s &raw);
+	void sensorsPoll();
 
 private:
 
@@ -134,7 +127,7 @@ private:
 	 * @param raw	Combined sensor data structure into which
 	 *		data should be returned.
 	 */
-	void imuPoll(sensor_combined_s &raw);
+	void imuPoll();
 
 	/**
 	 * Check & handle failover of a sensor
@@ -166,20 +159,19 @@ private:
 
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 
-	sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {};	/**< latest sensor data from all sensors instances */
-
 	const bool _hil_enabled{false};			/**< is hardware-in-the-loop mode enabled? */
 
 	bool _selection_changed{true};			/**< true when a sensor selection has changed and not been published */
 
+	matrix::Vector3f _accel_last[MAX_SENSOR_COUNT] {};
 	matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {};		/**< filtered accel differences between IMU units (m/s/s) */
+
+	matrix::Vector3f _gyro_last[MAX_SENSOR_COUNT] {};
 	matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {};			/**< filtered gyro differences between IMU uinits (rad/s) */
 
 	uint32_t _accel_device_id[MAX_SENSOR_COUNT] {};	/**< accel driver device id for each uorb instance */
 	uint32_t _gyro_device_id[MAX_SENSOR_COUNT] {};	/**< gyro driver device id for each uorb instance */
 
-	uint64_t _last_accel_timestamp[MAX_SENSOR_COUNT] {};	/**< latest full timestamp */
-
 	sensor_selection_s _selection {};		/**< struct containing the sensor selection to be published to the uORB */
 
 	DEFINE_PARAMETERS(

src/systemcmds/uorb/uorb.cpp

@@ -78,9 +78,6 @@ The code is optimized to minimize the memory footprint and the latency to exchan
 
 Messages are defined in the `/msg` directory. They are converted into C/C++ code at build-time.
 
-If compiled with ORB_USE_PUBLISHER_RULES, a file with uORB publication rules can be used to configure which
-modules are allowed to publish which topics. This is used for system-wide replay.
-
 ### Examples
 Monitor topic publication rates. Besides `top`, this is an important command for general system inspection:
 $ uorb top

src/templates/template_module/template_module.cpp

@@ -38,8 +38,6 @@
 #include <px4_platform_common/posix.h>
 
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_combined.h>
-
 
 int TemplateModule::print_status()
 {
@@ -137,11 +135,11 @@ TemplateModule::TemplateModule(int example_param, bool example_flag)
 
 void TemplateModule::run()
 {
-	// Example: run the loop synchronized to the sensor_combined topic publication
-	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	// Example: run the loop synchronized to the vehicle_angular_velocity topic publication
+	int vehicle_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity));
 
 	px4_pollfd_struct_t fds[1];
-	fds[0].fd = sensor_combined_sub;
+	fds[0].fd = vehicle_angular_velocity_sub;
 	fds[0].events = POLLIN;
 
 	// initialize parameters
@@ -163,8 +161,8 @@ void TemplateModule::run()
 
 		} else if (fds[0].revents & POLLIN) {
 
-			struct sensor_combined_s sensor_combined;
-			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor_combined);
+			vehicle_angular_velocity_s vehicle_angular_velocity;
+			orb_copy(ORB_ID(vehicle_angular_velocity), vehicle_angular_velocity_sub, &vehicle_angular_velocity);
 			// TODO: do something with the data...
 
 		}
@@ -172,7 +170,7 @@ void TemplateModule::run()
 		parameters_update();
 	}
 
-	orb_unsubscribe(sensor_combined_sub);
+	orb_unsubscribe(vehicle_angular_velocity_sub);
 }
 
 void TemplateModule::parameters_update(bool force)