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.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.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(