mixer_module: send a last sample out after all outputs were disabled

This matters for PWM when the last output gets disabled on either FMU or IO
it would just keep on running.

Also when rebooting with a parameters reset or new airframe with no mapped outputs
it would previously keep outputting PWM with the disarmed value of the new airframe
e.g. 1000us which is a safety hazard because servos could break the physical limit of the
model or miscalibrated ESCs spinning motors.

.ci/Jenkinsfile-hardware

@@ -813,6 +813,7 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true'

.editorconfig

@@ -0,0 +1,14 @@
+root = true
+
+[*]
+insert_final_newline = false
+
+[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
+indent_style = tab
+tab_width = 8
+# Not in the official standard, but supported by many editors
+max_line_length = 120
+
+[*.yaml]
+indent_style = space
+indent_size = 2

.gitmodules

@@ -81,5 +81,5 @@
 	url = https://github.com/PX4/PX4-gazebo-models.git
 	branch = main
 [submodule "boards/modalai/voxl2/libfc-sensor-api"]
-	path = src/modules/muorb/apps/libfc-sensor-api
+	path = boards/modalai/voxl2/libfc-sensor-api
 	url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

Makefile

@@ -122,6 +122,13 @@ 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
+
 CMAKE_ARGS ?=
 
 # additional config parameters passed to cmake

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

@@ -37,12 +37,6 @@ px4_add_romfs_files(
 	px4-rc.mavlink
 	px4-rc.params
 	px4-rc.simulator
-
-	rc.replay_ekf2
-	rc.replay_mc_hover_thrust_estimator
-	rc.replay_mixing
-	rc.replay_sensor_calibration
-	rc.replay_sensor_filtering
-
+	rc.replay
 	rcS
 )

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -6,36 +6,35 @@ param set-default IMU_INTEG_RATE 250
 
 if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
 
-
 	echo "INFO  [init] SIH simulator"
 
-	# if [ -n "${PX4_HOME_LAT}" ]; then
-	# 	param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
-	# fi
+	if [ -n "${PX4_HOME_LAT}" ]; then
+		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
+	fi
 
-	# if [ -n "${PX4_HOME_LON}" ]; then
-	# 	param set SIH_LOC_LON0 ${PX4_HOME_LON}
-	# fi
+	if [ -n "${PX4_HOME_LON}" ]; then
+		param set SIH_LOC_LON0 ${PX4_HOME_LON}
+	fi
 
-	# if simulator_sih start; then
+	if simulator_sih start; then
 
-	# 	if param compare -s SENS_EN_BAROSIM 1
-	# 	then
-	# 		sensor_baro_sim start
-	# 	fi
-	# 	if param compare -s SENS_EN_GPSSIM 1
-	# 	then
-	# 		sensor_gps_sim start
-	# 	fi
-	# 	if param compare -s SENS_EN_MAGSIM 1
-	# 	then
-	# 		sensor_mag_sim start
-	# 	fi
+		if param compare -s SENS_EN_BAROSIM 1
+		then
+			sensor_baro_sim start
+		fi
+		if param compare -s SENS_EN_GPSSIM 1
+		then
+			sensor_gps_sim start
+		fi
+		if param compare -s SENS_EN_MAGSIM 1
+		then
+			sensor_mag_sim start
+		fi
 
-	# else
-	# 	echo "ERROR [init] simulator_sih failed to start"
-	# 	exit 1
-	# fi
+	else
+		echo "ERROR [init] simulator_sih failed to start"
+		exit 1
+	fi
 
 elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 	# Use Gazebo
@@ -191,7 +190,7 @@ else
 
 	else
 		echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
-		#simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
+		simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
 	fi
 
 fi

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

@@ -13,14 +13,18 @@ if [ ! -f replay_params.txt ]; then
 	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
 fi
 
-param set SDLOG_DIRS_MAX 7
+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
 
-# TODO:
-# input:  vehicle_local_position, vehicle_local_position_setpoint
-# output: hover_thrust_estimate
+param set SDLOG_DIRS_MAX 7
+param set SDLOG_PROFILE 3
 
 # apply all params before ekf starts, as some params cannot be changed after startup
-#replay tryapplyparams
-mc_hover_thrust_estimator start
+replay tryapplyparams
+ekf2 start -r
 logger start -f -t -b 1000 -p vehicle_attitude
 replay start

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

@@ -1,54 +0,0 @@
-#!/bin/sh
-
-# EKF2 replay script
-
-# shellcheck disable=SC2154
-if [ ! -f ${replay_file} ]; then
-	echo "Invalid replay log file ${replay}"
-	exit 1
-fi
-
-echo "replay file: ${replay_file}"
-
-if [ ! -f replay_params.txt ]; then
-	echo "Creating $(pwd)/replay_params.txt"
-	ulog_params -i "${replay_file}" -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
-
-
-# replay start --ignore=estimator_status,
-# TODO: replay file?
-
-# apply all params before ekf starts, as some params cannot be changed after startup
-replay tryapplyparams
-
-#param set SENS_IMU_MODE 0
-#param set EKF2_MULTI_IMU 3
-
-ekf2 start &
-ekf2 status
-logger start -f -t -b 1000 -p vehicle_attitude
-logger status
-replay start
-
-# ekf2 status
-# logger status
-# logger stop
-
-# 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_mixing

@@ -1,29 +0,0 @@
-#!/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

@@ -1,30 +0,0 @@
-#!/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/rc.replay_sensor_filtering

@@ -1,28 +0,0 @@
-#!/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
-
-sensors start
-
-logger start -f -t -b 1000 -p vehicle_angular_velocity
-replay start

ROMFS/px4fmu_common/init.d-posix/rcS

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

Tools/astyle/files_to_check_code_style.sh

@@ -30,5 +30,5 @@ exec find boards msg src platforms test \
     -path src/lib/cdrstream/cyclonedds -prune -o \
     -path src/lib/cdrstream/rosidl -prune -o \
     -path src/modules/zenoh/zenoh-pico -prune -o \
-    -path src/modules/muorb/apps/libfc-sensor-api -prune -o \
+    -path boards/modalai/voxl2/libfc-sensor-api -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

boards/modalai/fc-v2/default.px4board

@@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y

boards/modalai/voxl2-slpi/default.px4board

@@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
+CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y

boards/modalai/voxl2/cmake/init.cmake

@@ -0,0 +1,34 @@
+############################################################################
+#
+#   Copyright (c) 2024 ModalAI, Inc. 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_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)

boards/modalai/voxl2/cmake/link_libraries.cmake

@@ -1,7 +1,7 @@
 
 # Link against the public stub version of the proprietary fc sensor library
 target_link_libraries(px4 PRIVATE
-		${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
+		${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
 		px4_layer
 		${module_libraries}
 )

boards/modalai/voxl2/scripts/build-deps.sh

@@ -1,10 +1,9 @@
 #!/bin/bash
 
-cd src/modules/muorb/apps/libfc-sensor-api
+cd boards/modalai/voxl2/libfc-sensor-api
 rm -fR build
 mkdir build
 cd build
 CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
 make
-cd ../../../../../..
-
+cd ../../../../..

boards/px4/fmu-v5x/default.px4board

@@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -102,7 +101,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y

msg/CMakeLists.txt

@@ -74,6 +74,7 @@ set(msg_files
 	DifferentialDriveSetpoint.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
+	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
 	EstimatorAidSource1d.msg

msg/Ekf2Timestamps.msg

@@ -0,0 +1,23 @@
+# 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

platforms/common/CMakeLists.txt

@@ -52,6 +52,7 @@ add_library(px4_platform STATIC
 	shutdown.cpp
 	spi.cpp
 	pab_manifest.c
+	Serial.cpp
 	${SRCS}
 )
 target_link_libraries(px4_platform prebuild_targets px4_work_queue)

platforms/common/Serial.cpp

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <px4_platform_common/Serial.hpp>
+
+namespace device
+{
+
+Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+	       FlowControl flowcontrol) :
+	_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
+{
+	// If no baudrate was specified then set it to a reasonable default value
+	if (baudrate == 0) {
+		(void) _impl.setBaudrate(9600);
+	}
+}
+
+Serial::~Serial()
+{
+}
+
+bool Serial::open()
+{
+	return _impl.open();
+}
+
+bool Serial::isOpen() const
+{
+	return _impl.isOpen();
+}
+
+bool Serial::close()
+{
+	return _impl.close();
+}
+
+ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
+{
+	return _impl.read(buffer, buffer_size);
+}
+
+ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
+}
+
+ssize_t Serial::write(const void *buffer, size_t buffer_size)
+{
+	return _impl.write(buffer, buffer_size);
+}
+
+uint32_t Serial::getBaudrate() const
+{
+	return _impl.getBaudrate();
+}
+
+bool Serial::setBaudrate(uint32_t baudrate)
+{
+	return _impl.setBaudrate(baudrate);
+}
+
+ByteSize Serial::getBytesize() const
+{
+	return _impl.getBytesize();
+}
+
+bool Serial::setBytesize(ByteSize bytesize)
+{
+	return _impl.setBytesize(bytesize);
+}
+
+Parity Serial::getParity() const
+{
+	return _impl.getParity();
+}
+
+bool Serial::setParity(Parity parity)
+{
+	return _impl.setParity(parity);
+}
+
+StopBits Serial::getStopbits() const
+{
+	return _impl.getStopbits();
+}
+
+bool Serial::setStopbits(StopBits stopbits)
+{
+	return _impl.setStopbits(stopbits);
+}
+
+FlowControl Serial::getFlowcontrol() const
+{
+	return _impl.getFlowcontrol();
+}
+
+bool Serial::setFlowcontrol(FlowControl flowcontrol)
+{
+	return _impl.setFlowcontrol(flowcontrol);
+}
+
+const char *Serial::getPort() const
+{
+	return _impl.getPort();
+}
+
+} // namespace device

platforms/common/include/px4_platform_common/Serial.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <SerialImpl.hpp>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device __EXPORT
+{
+
+class Serial
+{
+public:
+	Serial(const char *port, uint32_t baudrate = 57600,
+	       ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
+	       StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
+	virtual ~Serial();
+
+	// Open sets up the port and gets it configured based on desired configuration
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	// If port is already open then the following configuration functions
+	// will reconfigure the port. If the port is not yet open then they will
+	// simply store the configuration in preparation for the port to be opened.
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+	const char *getPort() const;
+
+private:
+	// Disable copy constructors
+	Serial(const Serial &);
+	Serial &operator=(const Serial &);
+
+	// platform implementation
+	SerialImpl _impl;
+};
+
+} // namespace device

platforms/common/include/px4_platform_common/SerialCommon.hpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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
+
+namespace device
+{
+namespace SerialConfig
+{
+
+
+// ByteSize: number of data bits
+enum class ByteSize {
+	FiveBits  = 5,
+	SixBits   = 6,
+	SevenBits = 7,
+	EightBits = 8,
+};
+
+// Parity: enable parity checking
+enum class Parity {
+	None = 0,
+	Odd  = 1,
+	Even = 2,
+};
+
+// StopBits: number of stop bits
+enum class StopBits {
+	One = 1,
+	Two = 2
+};
+
+// FlowControl: enable flow control
+enum class FlowControl {
+	Disabled = 0,
+	Enabled  = 1,
+};
+
+} // namespace SerialConfig
+} // namespace device

platforms/common/include/px4_platform_common/time.h

@@ -13,11 +13,21 @@ __END_DECLS
 #define px4_clock_gettime system_clock_gettime
 #endif
 
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
 
 __BEGIN_DECLS
 __EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
+__END_DECLS
 
+#else
+
+#define px4_clock_settime system_clock_settime
+
+#endif
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+__BEGIN_DECLS
 __EXPORT int px4_usleep(useconds_t usec);
 __EXPORT unsigned int px4_sleep(unsigned int seconds);
 __EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
@@ -27,7 +37,6 @@ __END_DECLS
 
 #else
 
-#define px4_clock_settime system_clock_settime
 #define px4_usleep system_usleep
 #define px4_sleep system_sleep
 #define px4_pthread_cond_timedwait system_pthread_cond_timedwait

platforms/common/uORB/SubscriptionBlocking.hpp

@@ -56,31 +56,6 @@ public:
 	SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
 		SubscriptionCallback(meta, interval_us, instance)
 	{
-		// pthread_mutexattr_init
-		pthread_mutexattr_t attr;
-		int ret_attr_init = pthread_mutexattr_init(&attr);
-
-		if (ret_attr_init != 0) {
-			PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
-		}
-
-#if defined(PTHREAD_PRIO_NONE)
-		// pthread_mutexattr_settype
-		//  PTHREAD_PRIO_NONE not available on cygwin
-		int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
-
-		if (ret_mutexattr_settype != 0) {
-			PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
-		}
-
-#endif // PTHREAD_PRIO_NONE
-
-		// pthread_mutex_init
-		int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
-
-		if (ret_mutex_init != 0) {
-			PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
-		}
 	}
 
 	virtual ~SubscriptionBlocking()

platforms/common/uORB/uORBDeviceNode.cpp

@@ -412,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription()
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
-		ch->send_message(_meta->o_name, _meta->o_size, _data);
+		// Only send the most recent data to initialize the remote end.
+		if (_data_valid) {
+			ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)));
+		}
 	}
 
 	return PX4_OK;

platforms/common/uORB/uORBManager.cpp

@@ -77,6 +77,24 @@ 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;
@@ -249,6 +267,30 @@ 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)
 {
+#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);
 
@@ -293,6 +335,14 @@ 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);
 }
 
@@ -314,6 +364,14 @@ int uORB::Manager::orb_unsubscribe(int 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);
 }
 
@@ -657,3 +715,142 @@ int16_t uORB::Manager::process_received_message(const char *messageName, int32_t
 }
 
 #endif /* CONFIG_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

@@ -503,7 +503,7 @@ private: // data members
 	DeviceMaster *_device_master{nullptr};
 
 private: //class methods
-	Manager() = default;
+	Manager();
 	virtual ~Manager();
 
 #ifdef CONFIG_ORB_COMMUNICATOR
@@ -559,6 +559,46 @@ private: //class methods
 	 */
 	virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data);
 #endif /* CONFIG_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/common/uORB/uORBMessageFields.cpp

@@ -181,7 +181,7 @@ MessageFormatReader::State MessageFormatReader::readMore()
 	}
 
 	// Not expected to get here
-	PX4_ERR("logic error (not expected to get here)");
+	PX4_ERR("logic error");
 	_state = State::Failure;
 	return _state;
 }
@@ -237,7 +237,7 @@ int MessageFormatReader::expandMessageFormat(char *format, unsigned len, unsigne
 	}
 
 	if (format_idx + 1 != (int)len) {
-		PX4_ERR("logic error (format_idx %d, len %d)", format_idx, len);
+		PX4_ERR("logic error");
 		return -1;
 	}
 

platforms/nuttx/init/stm32h7/rc.board_arch_defaults

@@ -13,4 +13,4 @@ param set-default -s MC_AT_EN 1
 
 param set-default -s UAVCAN_ENABLE 2
 
-set LOGGER_BUF 256
+set LOGGER_BUF 64

platforms/nuttx/src/px4/common/SerialImpl.cpp

@@ -0,0 +1,394 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+#define MODULE_NAME "SerialImpl"
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	return ((baudrate == 9600) ||
+		(baudrate == 19200) ||
+		(baudrate == 38400) ||
+		(baudrate == 57600) ||
+		(baudrate == 115200) ||
+		(baudrate == 230400) ||
+		(baudrate == 460800) ||
+		(baudrate == 921600));
+}
+
+bool SerialImpl::configure()
+{
+	/* process baud rate */
+	int speed;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
+		return false;
+	}
+
+	switch (_baudrate) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
+#endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %lu", _baudrate);
+		return false;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
+		return false;
+	}
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	if (isOpen()) {
+		return true;
+	}
+
+	// Open the serial port
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s err: %d", _port, errno);
+		return false;
+	}
+
+	_serial_fd = serial_fd;
+
+	// Configure the serial port
+	if (! configure()) {
+		PX4_ERR("failed to configure %s err: %d", _port, errno);
+		return false;
+	}
+
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+
+	if (_serial_fd >= 0) {
+		::close(_serial_fd);
+	}
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret = ::read(_serial_fd, buffer, buffer_size);
+
+	if (ret < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret);
+	}
+
+	return ret;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
+		// Poll for incoming UART data.
+		pollfd fds[1];
+		fds[0].fd = _serial_fd;
+		fds[0].events = POLLIN;
+
+		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
+
+		if (remaining_time <= 0) { break; }
+
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
+
+		if (ret > 0) {
+			if (fds[0].revents & POLLIN) {
+				const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
+
+				int err = 0;
+				int bytes_available = 0;
+				err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+
+				if (err != 0 || bytes_available < (int)character_count) {
+					px4_usleep(sleeptime);
+				}
+
+				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
+
+				if (ret > 0) {
+					total_bytes_read += ret;
+				}
+
+			} else {
+				PX4_ERR("Got a poll error");
+				return -1;
+			}
+		}
+	}
+
+	return total_bytes_read;
+}
+
+ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot write to serial device until it has been opened");
+		return -1;
+	}
+
+	int written = ::write(_serial_fd, buffer, buffer_size);
+	::fsync(_serial_fd);
+
+	if (written < 0) {
+		PX4_ERR("%s write error %d", _port, written);
+	}
+
+	return written;
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("ERR: invalid baudrate: %lu", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if ((baudrate == _baudrate) && _open) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return configure();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device

platforms/nuttx/src/px4/common/include/SerialImpl.hpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+	bool configure();
+
+};
+
+} // namespace device

platforms/nuttx/src/px4/common/px4_layer.cmake

@@ -3,6 +3,8 @@
 add_library(px4_layer
 		${KERNEL_SRCS}
 		cdc_acm_check.cpp
+		${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+		SerialImpl.cpp
 	)
 
 target_link_libraries(px4_layer

platforms/nuttx/src/px4/common/px4_protected_layers.cmake

@@ -15,6 +15,8 @@ add_library(px4_layer
 	usr_board_ctrl.c
 	usr_hrt.cpp
 	usr_mcu_version.cpp
+	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+	SerialImpl.cpp
 )
 
 target_link_libraries(px4_layer

platforms/posix/include/SerialImpl.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <stdint.h>
+#include <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+	bool configure();
+};
+
+} // namespace device

platforms/posix/src/px4/common/CMakeLists.txt

@@ -46,6 +46,8 @@ add_library(px4_layer
 	drv_hrt.cpp
 	cpuload.cpp
 	print_load.cpp
+	${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
+	SerialImpl.cpp
 )
 target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
 target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable

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

@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <termios.h>
+#include <px4_log.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	return ((baudrate == 9600) ||
+		(baudrate == 19200) ||
+		(baudrate == 38400) ||
+		(baudrate == 57600) ||
+		(baudrate == 115200) ||
+		(baudrate == 230400) ||
+		(baudrate == 460800) ||
+		(baudrate == 921600));
+}
+
+bool SerialImpl::configure()
+{
+	/* process baud rate */
+	int speed;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("ERR: unknown baudrate: %u", _baudrate);
+		return false;
+	}
+
+	switch (_baudrate) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
+#endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	case 921600: speed = B921600; break;
+
+	default:
+		PX4_ERR("ERR: unknown baudrate: %d", _baudrate);
+		return false;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcgetattr)", termios_state);
+		return false;
+	}
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return false;
+	}
+
+	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	if (isOpen()) {
+		return true;
+	}
+
+	// Open the serial port
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s err: %d", _port, errno);
+		return false;
+	}
+
+	_serial_fd = serial_fd;
+
+	// Configure the serial port
+	if (! configure()) {
+		PX4_ERR("failed to configure %s err: %d", _port, errno);
+		return false;
+	}
+
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+
+	if (_serial_fd >= 0) {
+		::close(_serial_fd);
+	}
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret = ::read(_serial_fd, buffer, buffer_size);
+
+	if (ret < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret);
+
+	}
+
+	return ret;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
+		// Poll for incoming UART data.
+		pollfd fds[1];
+		fds[0].fd = _serial_fd;
+		fds[0].events = POLLIN;
+
+		hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us);
+
+		if (remaining_time <= 0) { break; }
+
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time);
+
+		if (ret > 0) {
+			if (fds[0].revents & POLLIN) {
+				const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10);
+				px4_usleep(sleeptime);
+
+				ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
+
+				if (ret > 0) {
+					total_bytes_read += ret;
+				}
+
+			} else {
+				PX4_ERR("Got a poll error");
+				return -1;
+			}
+		}
+	}
+
+	return total_bytes_read;
+}
+
+ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot write to serial device until it has been opened");
+		return -1;
+	}
+
+	int written = ::write(_serial_fd, buffer, buffer_size);
+	::fsync(_serial_fd);
+
+	if (written < 0) {
+		PX4_ERR("%s write error %d", _port, written);
+
+	}
+
+	return written;
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("ERR: invalid baudrate: %u", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if ((baudrate == _baudrate) && _open) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return configure();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device

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

@@ -51,6 +51,11 @@
 #include <errno.h>
 #include "hrt_work.h"
 
+// Voxl2 board specific API definitions to get time offset
+#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+#include "fc_sensor.h"
+#endif
+
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
 static LockstepScheduler lockstep_scheduler {true};
@@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time()
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	struct timespec ts;
 	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+
+# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+	hrt_abstime temp_abstime = ts_to_abstime(&ts);
+	int apps_time_offset = fc_sensor_get_time_offset();
+
+	if (apps_time_offset < 0) {
+		hrt_abstime temp_offset = -apps_time_offset;
+
+		if (temp_offset >= temp_abstime) {
+			temp_abstime = 0;
+
+		} else {
+			temp_abstime -= temp_offset;
+		}
+
+	} else {
+		temp_abstime += (hrt_abstime) apps_time_offset;
+	}
+
+	ts.tv_sec = temp_abstime / 1000000;
+	ts.tv_nsec = (temp_abstime % 1000000) * 1000;
+# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+
 	return ts_to_abstime(&ts);
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 }
@@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	return system_clock_gettime(clk_id, tp);
+
 }
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)

platforms/qurt/CMakeLists.txt

@@ -50,5 +50,4 @@ add_library(px4 SHARED
 target_link_libraries(px4
 	modules__muorb__slpi
 	${module_libraries}
-	px4_layer
 	)

platforms/qurt/include/SerialImpl.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <unistd.h>
+
+#include <px4_platform_common/SerialCommon.hpp>
+
+using device::SerialConfig::ByteSize;
+using device::SerialConfig::Parity;
+using device::SerialConfig::StopBits;
+using device::SerialConfig::FlowControl;
+
+namespace device
+{
+
+class SerialImpl
+{
+public:
+
+	SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		   FlowControl flowcontrol);
+	virtual ~SerialImpl();
+
+	bool open();
+	bool isOpen() const;
+
+	bool close();
+
+	ssize_t read(uint8_t *buffer, size_t buffer_size);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
+
+	ssize_t write(const void *buffer, size_t buffer_size);
+
+	const char *getPort() const;
+	bool setPort(const char *port);
+
+	uint32_t getBaudrate() const;
+	bool setBaudrate(uint32_t baudrate);
+
+	ByteSize getBytesize() const;
+	bool setBytesize(ByteSize bytesize);
+
+	Parity getParity() const;
+	bool setParity(Parity parity);
+
+	StopBits getStopbits() const;
+	bool setStopbits(StopBits stopbits);
+
+	FlowControl getFlowcontrol() const;
+	bool setFlowcontrol(FlowControl flowcontrol);
+
+private:
+
+	int _serial_fd{-1};
+
+	bool _open{false};
+
+	char _port[32] {};
+
+	uint32_t _baudrate{0};
+
+	ByteSize _bytesize{ByteSize::EightBits};
+	Parity _parity{Parity::None};
+	StopBits _stopbits{StopBits::One};
+	FlowControl _flowcontrol{FlowControl::Disabled};
+
+	bool validateBaudrate(uint32_t baudrate);
+
+	// Mutex used to lock the read functions
+	//pthread_mutex_t read_mutex;
+
+	// Mutex used to lock the write functions
+	//pthread_mutex_t write_mutex;
+};
+
+} // namespace device

platforms/qurt/src/px4/CMakeLists.txt

@@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS
 	px4_qurt_impl.cpp
 	main.cpp
 	qurt_log.cpp
+	SerialImpl.cpp
 	)
 
 add_library(px4_layer

platforms/qurt/src/px4/SerialImpl.cpp

@@ -0,0 +1,326 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <SerialImpl.hpp>
+#include <string.h> // strncpy
+#include <px4_log.h>
+#include <drivers/device/qurt/uart.h>
+#include <drivers/drv_hrt.h>
+
+namespace device
+{
+
+SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
+		       FlowControl flowcontrol) :
+	_baudrate(baudrate),
+	_bytesize(bytesize),
+	_parity(parity),
+	_stopbits(stopbits),
+	_flowcontrol(flowcontrol)
+{
+	if (port) {
+		strncpy(_port, port, sizeof(_port) - 1);
+		_port[sizeof(_port) - 1] = '\0';
+
+	} else {
+		_port[0] = 0;
+	}
+}
+
+SerialImpl::~SerialImpl()
+{
+	if (isOpen()) {
+		close();
+	}
+}
+
+bool SerialImpl::validateBaudrate(uint32_t baudrate)
+{
+	if ((baudrate != 9600)   &&
+	    (baudrate != 38400)  &&
+	    (baudrate != 57600)  &&
+	    (baudrate != 115200) &&
+	    (baudrate != 230400) &&
+	    (baudrate != 250000) &&
+	    (baudrate != 420000) &&
+	    (baudrate != 460800) &&
+	    (baudrate != 921600) &&
+	    (baudrate != 1000000) &&
+	    (baudrate != 1843200) &&
+	    (baudrate != 2000000)) {
+		return false;
+	}
+
+	return true;
+}
+
+bool SerialImpl::open()
+{
+	// There's no harm in calling open multiple times on the same port.
+	// In fact, that's the only way to change the baudrate
+
+	_open = false;
+	_serial_fd = -1;
+
+	if (! validateBaudrate(_baudrate)) {
+		PX4_ERR("Invalid baudrate: %u", _baudrate);
+		return false;
+	}
+
+	if (_bytesize != ByteSize::EightBits) {
+		PX4_ERR("Qurt platform only supports ByteSize::EightBits");
+		return false;
+	}
+
+	if (_parity != Parity::None) {
+		PX4_ERR("Qurt platform only supports Parity::None");
+		return false;
+	}
+
+	if (_stopbits != StopBits::One) {
+		PX4_ERR("Qurt platform only supports StopBits::One");
+		return false;
+	}
+
+	if (_flowcontrol != FlowControl::Disabled) {
+		PX4_ERR("Qurt platform only supports FlowControl::Disabled");
+		return false;
+	}
+
+	// qurt_uart_open will check validity of port and baudrate
+	int serial_fd = qurt_uart_open(_port, _baudrate);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd);
+		return false;
+
+	} else {
+		PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate);
+	}
+
+	_serial_fd = serial_fd;
+	_open = true;
+
+	return _open;
+}
+
+bool SerialImpl::isOpen() const
+{
+	return _open;
+}
+
+bool SerialImpl::close()
+{
+	// No close defined for qurt uart yet
+	// if (_serial_fd >= 0) {
+	// 	qurt_uart_close(_serial_fd);
+	// }
+
+	_serial_fd = -1;
+	_open = false;
+
+	return true;
+}
+
+ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot read from serial device until it has been opened");
+		return -1;
+	}
+
+	int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
+
+	if (ret_read < 0) {
+		PX4_DEBUG("%s read error %d", _port, ret_read);
+
+	}
+
+	return ret_read;
+}
+
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
+{
+	if (!_open) {
+		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
+		return -1;
+	}
+
+	if (buffer_size < character_count) {
+		PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__);
+		return -1;
+	}
+
+	const hrt_abstime start_time_us = hrt_absolute_time();
+	int total_bytes_read = 0;
+
+	while (total_bytes_read < (int) character_count) {
+
+		if (timeout_us > 0) {
+			const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
+
+			if (elapsed_us >= timeout_us) {
+				// If there was a partial read but not enough to satisfy the minimum then they will be lost
+				// but this really should never happen when everything is working normally.
+				// PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us);
+				// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
+				return total_bytes_read;
+			}
+		}
+
+		int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read);
+
+		if (current_bytes_read < 0) {
+			// Again, if there was a partial read but not enough to satisfy the minimum then they will be lost
+			// but this really should never happen when everything is working normally.
+			PX4_ERR("%s failed to read uart", __FUNCTION__);
+			// Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)?
+			return -1;
+		}
+
+		// Current bytes read could be zero
+		total_bytes_read += current_bytes_read;
+
+		// If we have at least reached our desired minimum number of characters
+		// then we can return now
+		if (total_bytes_read >= (int) character_count) {
+			return total_bytes_read;
+		}
+
+		// Wait a set amount of time before trying again or the remaining time
+		// until the timeout if we are getting close
+		const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us);
+		int64_t time_until_timeout = timeout_us - elapsed_us;
+		uint64_t time_to_sleep = 5000;
+
+		if ((time_until_timeout >= 0) &&
+		    (time_until_timeout < (int64_t) time_to_sleep)) {
+			time_to_sleep = time_until_timeout;
+		}
+
+		px4_usleep(time_to_sleep);
+	}
+
+	return -1;
+}
+
+ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
+{
+	if (!_open) {
+		PX4_ERR("Cannot write to serial device until it has been opened");
+		return -1;
+	}
+
+	int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size);
+
+	if (ret_write < 0) {
+		PX4_ERR("%s write error %d", _port, ret_write);
+
+	}
+
+	return ret_write;
+}
+
+const char *SerialImpl::getPort() const
+{
+	return _port;
+}
+
+uint32_t SerialImpl::getBaudrate() const
+{
+	return _baudrate;
+}
+
+bool SerialImpl::setBaudrate(uint32_t baudrate)
+{
+	if (! validateBaudrate(baudrate)) {
+		PX4_ERR("Invalid baudrate: %u", baudrate);
+		return false;
+	}
+
+	// check if already configured
+	if (baudrate == _baudrate) {
+		return true;
+	}
+
+	_baudrate = baudrate;
+
+	// process baud rate change now if port is already open
+	if (_open) {
+		return open();
+	}
+
+	return true;
+}
+
+ByteSize SerialImpl::getBytesize() const
+{
+	return _bytesize;
+}
+
+bool SerialImpl::setBytesize(ByteSize bytesize)
+{
+	return bytesize == ByteSize::EightBits;
+}
+
+Parity SerialImpl::getParity() const
+{
+	return _parity;
+}
+
+bool SerialImpl::setParity(Parity parity)
+{
+	return parity == Parity::None;
+}
+
+StopBits SerialImpl::getStopbits() const
+{
+	return _stopbits;
+}
+
+bool SerialImpl::setStopbits(StopBits stopbits)
+{
+	return stopbits == StopBits::One;
+}
+
+FlowControl SerialImpl::getFlowcontrol() const
+{
+	return _flowcontrol;
+}
+
+bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
+{
+	return flowcontrol == FlowControl::Disabled;
+}
+
+} // namespace device

platforms/qurt/src/px4/drv_hrt.cpp

@@ -81,7 +81,7 @@ static void hrt_unlock()
 	px4_sem_post(&_hrt_lock);
 }
 
-int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
 {
 	return 0;
 }

src/drivers/drv_hrt.h

@@ -162,7 +162,16 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
  */
 static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
 {
-	return hrt_absolute_time() - *then;
+	hrt_abstime now = hrt_absolute_time();
+
+	// Cannot allow a negative elapsed time as this would appear
+	// to be a huge positive elapsed time when represented as an
+	// unsigned value!
+	if (*then > now) {
+		return 0;
+	}
+
+	return now - *then;
 }
 
 /**

src/drivers/gps/gps.cpp

@@ -45,7 +45,6 @@
 #include <poll.h>
 #endif
 
-#include <termios.h>
 #include <cstring>
 
 #include <drivers/drv_sensor.h>
@@ -57,6 +56,8 @@
 #include <px4_platform_common/cli.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/Serial.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -81,6 +82,7 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */
 
+using namespace device;
 using namespace time_literals;
 
 #define TIMEOUT_1HZ		1300	//!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
@@ -169,7 +171,10 @@ public:
 	void reset_if_scheduled();
 
 private:
-	int				_serial_fd{-1};					///< serial interface to GPS
+#ifdef __PX4_LINUX
+	int				_spi_fd {-1};					///< SPI interface to GPS
+#endif
+	Serial 			*_uart = nullptr;				///< UART interface to GPS
 	unsigned			_baudrate{0};					///< current baudrate
 	const unsigned			_configured_baudrate{0};			///< configured baudrate (0=auto-detect)
 	char				_port[20] {};					///< device / serial port path
@@ -329,8 +334,11 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
 		set_device_bus(c - 48); // sub 48 to convert char to integer
 
+#ifdef __PX4_LINUX
+
 	} else if (_interface == GPSHelper::Interface::SPI) {
 		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
+#endif
 	}
 
 	if (_mode == gps_driver_mode_t::None) {
@@ -403,10 +411,23 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 			return num_read;
 		}
 
-	case GPSCallbackType::writeDeviceData:
-		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
+	case GPSCallbackType::writeDeviceData: {
+			gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
 
-		return ::write(gps->_serial_fd, data1, (size_t)data2);
+			int ret = 0;
+
+			if (gps->_uart) {
+				ret = gps->_uart->write((void *) data1, (size_t) data2);
+
+#ifdef __PX4_LINUX
+
+			} else if (gps->_spi_fd >= 0) {
+				ret = ::write(gps->_spi_fd, data1, (size_t)data2);
+#endif
+			}
+
+			return ret;
+		}
 
 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -449,72 +470,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 
 int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 {
+	int ret = 0;
+	const unsigned character_count = 32; // minimum bytes that we want to read
+	const int max_timeout = 50;
+	int timeout_adjusted = math::min(max_timeout, timeout);
+
 	handleInjectDataTopic();
 
-#if !defined(__PX4_QURT)
+	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
+		ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
 
-	/* For non QURT, use the usual polling. */
+// SPI is only supported on LInux
+#if defined(__PX4_LINUX)
 
-	//Poll only for the serial data. In the same thread we also need to handle orb messages,
-	//so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
-	//two pollings use different underlying mechanisms (at least under posix), which makes this
-	//impossible. Instead we limit the maximum polling interval and regularly check for new orb
-	//messages.
-	//FIXME: add a unified poll() API
-	const int max_timeout = 50;
+	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
 
-	pollfd fds[1];
-	fds[0].fd = _serial_fd;
-	fds[0].events = POLLIN;
+		//Poll only for the SPI data. In the same thread we also need to handle orb messages,
+		//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
+		//two pollings use different underlying mechanisms (at least under posix), which makes this
+		//impossible. Instead we limit the maximum polling interval and regularly check for new orb
+		//messages.
+		//FIXME: add a unified poll() API
 
-	int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
+		pollfd fds[1];
+		fds[0].fd = _spi_fd;
+		fds[0].events = POLLIN;
 
-	if (ret > 0) {
-		/* if we have new data from GPS, go handle it */
-		if (fds[0].revents & POLLIN) {
-			/*
-			 * We are here because poll says there is some data, so this
-			 * won't block even on a blocking device. But don't read immediately
-			 * by 1-2 bytes, wait for some more data to save expensive read() calls.
-			 * If we have all requested data available, read it without waiting.
-			 * If more bytes are available, we'll go back to poll() again.
-			 */
-			const unsigned character_count = 32; // minimum bytes that we want to read
-			unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
-			const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
+		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
 
-#ifdef __PX4_NUTTX
-			int err = 0;
-			int bytes_available = 0;
-			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+		if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device. But don't read immediately
+				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+				 * If we have all requested data available, read it without waiting.
+				 * If more bytes are available, we'll go back to poll() again.
+				 */
+				unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
+				const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
 
-			if (err != 0 || bytes_available < (int)character_count) {
 				px4_usleep(sleeptime);
+
+				ret = ::read(_spi_fd, buf, buf_length);
+
+				if (ret > 0) {
+					_num_bytes_read += ret;
+				}
+
+			} else {
+				ret = -1;
 			}
-
-#else
-			px4_usleep(sleeptime);
-#endif
-
-			ret = ::read(_serial_fd, buf, buf_length);
-
-			if (ret > 0) {
-				_num_bytes_read += ret;
-			}
-
-		} else {
-			ret = -1;
 		}
+
+#endif
 	}
 
 	return ret;
-
-#else
-	/* For QURT, just use read for now, since this doesn't block, we need to slow it down
-	 * just a bit. */
-	px4_usleep(10000);
-	return ::read(_serial_fd, buf, buf_length);
-#endif
 }
 
 void GPS::handleInjectDataTopic()
@@ -583,105 +596,38 @@ bool GPS::injectData(uint8_t *data, size_t len)
 {
 	dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
 
-	size_t written = ::write(_serial_fd, data, len);
-	::fsync(_serial_fd);
+	size_t written = 0;
+
+	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
+		written = _uart->write((const void *) data, len);
+
+#ifdef __PX4_LINUX
+
+	} else if (_interface == GPSHelper::Interface::SPI) {
+		written = ::write(_spi_fd, data, len);
+		::fsync(_spi_fd);
+#endif
+	}
+
 	return written == len;
 }
 
 int GPS::setBaudrate(unsigned baud)
 {
-	/* process baud rate */
-	int speed;
+	if (_interface == GPSHelper::Interface::UART) {
+		if ((_uart) && (_uart->setBaudrate(baud))) {
+			return 0;
+		}
 
-	switch (baud) {
-	case 9600:   speed = B9600;   break;
+#ifdef __PX4_LINUX
 
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-#ifndef B460800
-#define B460800 460800
+	} else if (_interface == GPSHelper::Interface::SPI) {
+		// Can't set the baudrate on a SPI port but just return a success
+		return 0;
 #endif
-
-	case 460800: speed = B460800; break;
-
-#ifndef B921600
-#define B921600 921600
-#endif
-
-	case 921600: speed = B921600; break;
-
-	default:
-		PX4_ERR("ERR: unknown baudrate: %d", baud);
-		return -EINVAL;
 	}
 
-	struct termios uart_config;
-
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	tcgetattr(_serial_fd, &uart_config);
-
-	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
-
-	//
-	// Input flags - Turn off input processing
-	//
-	// convert break to null byte, no CR to NL translation,
-	// no NL to CR translation, don't mark parity errors or breaks
-	// no input parity check, don't strip high bit off,
-	// no XON/XOFF software flow control
-	//
-	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
-				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
-	//
-	// Output flags - Turn off output processing
-	//
-	// no CR to NL translation, no NL to CR-NL translation,
-	// no NL to CR translation, no column 0 CR suppression,
-	// no Ctrl-D suppression, no fill characters, no case mapping,
-	// no local output processing
-	//
-	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
-	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
-	uart_config.c_oflag = 0;
-
-	//
-	// No line processing
-	//
-	// echo off, echo newline off, canonical mode off,
-	// extended input processing off, signal chars off
-	//
-	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
-
-	/* no parity, one stop bit, disable flow control */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
-		return -1;
-	}
-
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
-		return -1;
-	}
-
-	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
-		GPS_ERR("ERR: %d (tcsetattr)", termios_state);
-		return -1;
-	}
-
-	return 0;
+	return -1;
 }
 
 void GPS::initializeCommunicationDump()
@@ -840,31 +786,58 @@ GPS::run()
 			_helper = nullptr;
 		}
 
-		if (_serial_fd < 0) {
-			/* open the serial port */
-			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
 
-			if (_serial_fd < 0) {
-				PX4_ERR("failed to open %s err: %d", _port, errno);
+			// Create the UART port instance
+			_uart = new Serial(_port);
+
+			if (_uart == nullptr) {
+				PX4_ERR("Error creating serial device %s", _port);
+				px4_sleep(1);
+				continue;
+			}
+		}
+
+		if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) {
+			// Configure the desired baudrate if one was specified by the user.
+			// Otherwise the default baudrate will be used.
+			if (_configured_baudrate) {
+				if (! _uart->setBaudrate(_configured_baudrate)) {
+					PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
+					px4_sleep(1);
+					continue;
+				}
+			}
+
+			// Open the UART. If this is successful then the UART is ready to use.
+			if (! _uart->open()) {
+				PX4_ERR("Error opening serial device  %s", _port);
 				px4_sleep(1);
 				continue;
 			}
 
 #ifdef __PX4_LINUX
 
-			if (_interface == GPSHelper::Interface::SPI) {
-				int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
-				int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
+			_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
-				if (status_value < 0) {
-					PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-				}
+			if (_spi_fd < 0) {
+				PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
+				px4_sleep(1);
+				continue;
+			}
 
-				status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
+			int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+			int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
 
-				if (status_value < 0) {
-					PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-				}
+			if (status_value < 0) {
+				PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+			}
+
+			status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
+
+			if (status_value < 0) {
+				PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
 			}
 
 #endif /* __PX4_LINUX */
@@ -1056,9 +1029,17 @@ GPS::run()
 			}
 		}
 
-		if (_serial_fd >= 0) {
-			::close(_serial_fd);
-			_serial_fd = -1;
+		if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
+			(void) _uart->close();
+			delete _uart;
+			_uart = nullptr;
+
+#ifdef __PX4_LINUX
+
+		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
+			::close(_spi_fd);
+			_spi_fd = -1;
+#endif
 		}
 
 		if (_mode_auto) {
@@ -1406,7 +1387,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)
 				   entry_point, (char *const *)argv);
 
 	if (task_id < 0) {
-		task_id = -1;
+		_task_id = -1;
 		return -errno;
 	}
 
@@ -1477,12 +1458,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'i':
-			if (!strcmp(myoptarg, "spi")) {
-				interface = GPSHelper::Interface::SPI;
-
-			} else if (!strcmp(myoptarg, "uart")) {
+			if (!strcmp(myoptarg, "uart")) {
 				interface = GPSHelper::Interface::UART;
-
+#ifdef __PX4_LINUX
+			} else if (!strcmp(myoptarg, "spi")) {
+				interface = GPSHelper::Interface::SPI;
+#endif
 			} else {
 				PX4_ERR("unknown interface: %s", myoptarg);
 				error_flag = true;
@@ -1490,12 +1471,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'j':
-			if (!strcmp(myoptarg, "spi")) {
-				interface_secondary = GPSHelper::Interface::SPI;
-
-			} else if (!strcmp(myoptarg, "uart")) {
+			if (!strcmp(myoptarg, "uart")) {
 				interface_secondary = GPSHelper::Interface::UART;
-
+#ifdef __PX4_LINUX
+			} else if (!strcmp(myoptarg, "spi")) {
+				interface_secondary = GPSHelper::Interface::SPI;
+#endif
 			} else {
 				PX4_ERR("unknown interface for secondary: %s", myoptarg);
 				error_flag = true;

src/drivers/px4io/px4io.cpp

@@ -364,6 +364,13 @@ PX4IO::~PX4IO()
 bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
+	for (size_t i = 0; i < num_outputs; i++) {
+		if (!_mixing_output.isFunctionSet(i)) {
+			// do not run any signal on disabled channels
+			outputs[i] = 0;
+		}
+	}
+
 	if (!_test_fmu_fail) {
 		/* output to the servos */
 		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);

src/lib/mixer_module/mixer_module.cpp

@@ -455,7 +455,8 @@ bool MixingOutput::update()
 		}
 	}
 
-	if (!all_disabled) {
+	// Send output if any function mapped or one last disabling sample
+	if (!all_disabled || !_was_all_disabled) {
 		if (!_armed.armed && !_armed.manual_lockdown) {
 			_actuator_test.overrideValues(outputs, _max_num_outputs);
 		}
@@ -463,6 +464,8 @@ bool MixingOutput::update()
 		limitAndUpdateOutputs(outputs, has_updates);
 	}
 
+	_was_all_disabled = all_disabled;
+
 	return true;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -288,6 +288,7 @@ private:
 	hrt_abstime _lowrate_schedule_interval{300_ms};
 	ActuatorTest _actuator_test{_function_assignment};
 	uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only)
+	bool _was_all_disabled{false};
 
 	uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback
 

src/lib/mixer_module/mixer_module_tests.cpp

@@ -191,11 +191,15 @@ TEST_F(MixerModuleTest, basic)
 	mixing_output.setAllMaxValues(MAX_VALUE);
 	EXPECT_EQ(test_module.num_updates, 0);
 
-	// all functions disabled: not expected to get an update
+	// all functions disabled: expect to get one single update to process disabling the output signal
 	mixing_output.update();
 	mixing_output.updateSubscriptions(false);
 	mixing_output.update();
-	EXPECT_EQ(test_module.num_updates, 0);
+	EXPECT_EQ(test_module.num_updates, 1);
+	mixing_output.update();
+	mixing_output.updateSubscriptions(false);
+	mixing_output.update();
+	EXPECT_EQ(test_module.num_updates, 1);
 	test_module.reset();
 
 	// configure motor, ensure all still disarmed

src/modules/ekf2/EKF2.cpp

@@ -49,9 +49,10 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
 #endif // CONFIG_EKF2_MULTI_INSTANCE
 
-EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config):
+EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	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)),
@@ -236,6 +237,7 @@ EKF2::~EKF2()
 bool EKF2::multi_init(int imu, int mag)
 {
 	// advertise all topics to ensure consistent uORB instance numbering
+	_ekf2_timestamps_pub.advertise();
 	_estimator_event_flags_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
@@ -701,32 +703,42 @@ void EKF2::Run()
 			_last_time_slip_us = 0;
 		}
 
+		// 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,
+		};
+
 #if defined(CONFIG_EKF2_AIRSPEED)
-		UpdateAirspeedSample();
+		UpdateAirspeedSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_AUXVEL)
-		UpdateAuxVelSample();
+		UpdateAuxVelSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_AUXVEL
 #if defined(CONFIG_EKF2_BAROMETER)
-		UpdateBaroSample();
+		UpdateBaroSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-		UpdateExtVisionSample();
+		UpdateExtVisionSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-		UpdateFlowSample();
+		UpdateFlowSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 #if defined(CONFIG_EKF2_GNSS)
-		UpdateGpsSample();
+		UpdateGpsSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-		UpdateMagSample();
+		UpdateMagSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-		UpdateRangeSample();
+		UpdateRangeSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_RANGE_FINDER
-
-		UpdateSystemFlagsSample();
+		UpdateSystemFlagsSample(ekf2_timestamps);
 
 		// run the EKF update and output
 		const hrt_abstime ekf_update_start = hrt_absolute_time();
@@ -781,6 +793,9 @@ void EKF2::Run()
 			UpdateMagCalibration(now);
 #endif // CONFIG_EKF2_MAGNETOMETER
 		}
+
+		// publish ekf2_timestamps
+		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
 
 	// re-schedule as backup timeout
@@ -906,7 +921,13 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 		_ekf.getQuaternion().copyTo(att.q);
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
-		att.timestamp = hrt_absolute_time();
+		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{};
 		_attitude_pub.publish(att);
 	}
 }
@@ -983,7 +1004,7 @@ void EKF2::PublishEvPosBias(const hrt_abstime &timestamp)
 
 		if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) {
 			bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample;
-			bias.timestamp = hrt_absolute_time();
+			bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 			_estimator_ev_pos_bias_pub.publish(bias);
 
 			_last_ev_bias_published = Vector3f(bias.bias);
@@ -1003,7 +1024,7 @@ estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status,
 	bias.innov = status.innov;
 	bias.innov_var = status.innov_var;
 	bias.innov_test_ratio = status.innov_test_ratio;
-	bias.timestamp = hrt_absolute_time();
+	bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 	return bias;
 }
@@ -1065,7 +1086,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 = hrt_absolute_time();
+		event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_event_flags_pub.update(event_flags);
 
 		_last_event_flags_publish = event_flags.timestamp;
@@ -1075,7 +1096,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 
 	} else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) {
 		// continue publishing periodically
-		_estimator_event_flags_pub.get().timestamp = hrt_absolute_time();
+		_estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_event_flags_pub.update();
 		_last_event_flags_publish = _estimator_event_flags_pub.get().timestamp;
 	}
@@ -1132,7 +1153,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 		global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
 					    || _ekf.control_status_flags().wind_dead_reckoning;
 
-		global_pos.timestamp = hrt_absolute_time();
+		global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_global_position_pub.publish(global_pos);
 	}
 }
@@ -1166,7 +1187,7 @@ void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
 	estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed;
 	estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed;
 
-	estimator_gps_status.timestamp = hrt_absolute_time();
+	estimator_gps_status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_gps_status_pub.publish(estimator_gps_status);
 
 
@@ -1267,7 +1288,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	innovations.hagl_rate = _ekf.getHaglRateInnov();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	innovations.timestamp = hrt_absolute_time();
+	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
 
 	// calculate noise filtered velocity innovations which are used for pre-flight checking
@@ -1401,7 +1422,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	test_ratios.timestamp = hrt_absolute_time();
+	test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_test_ratios_pub.publish(test_ratios);
 }
 
@@ -1498,7 +1519,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	variances.hagl_rate = _ekf.getHaglRateInnovVar();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	variances.timestamp = hrt_absolute_time();
+	variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_variances_pub.publish(variances);
 }
 
@@ -1604,7 +1625,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	}
 
 	// publish vehicle local position data
-	lpos.timestamp = hrt_absolute_time();
+	lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_local_position_pub.publish(lpos);
 }
 
@@ -1646,7 +1667,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
 	odom.quality = 0;
 
 	// publish vehicle odometry data
-	odom.timestamp = hrt_absolute_time();
+	odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_odometry_pub.publish(odom);
 }
 
@@ -1712,7 +1733,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-		bias.timestamp = hrt_absolute_time();
+		bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_sensor_bias_pub.publish(bias);
 
 		_last_sensor_bias_published = bias.timestamp;
@@ -1728,7 +1749,7 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
 	state_vector.copyTo(states.states);
 	states.n_states = state_vector.size();
 	_ekf.covariances_diagonal().copyTo(states.covariances);
-	states.timestamp = hrt_absolute_time();
+	states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_states_pub.publish(states);
 }
 
@@ -1789,7 +1810,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 			    status.mag_strength_ref_gs);
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-	status.timestamp = hrt_absolute_time();
+	status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_status_pub.publish(status);
 }
 
@@ -1890,7 +1911,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 = hrt_absolute_time();
+		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
 
 		_last_status_flags_publish = status_flags.timestamp;
@@ -1912,7 +1933,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 
 		yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable();
 		yaw_est_test_data.timestamp_sample = _ekf.time_delayed_us();
-		yaw_est_test_data.timestamp = hrt_absolute_time();
+		yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_yaw_est_pub.publish(yaw_est_test_data);
 	}
@@ -1943,7 +1964,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 = hrt_absolute_time();
+		wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_wind_pub.publish(wind);
 	}
@@ -1971,7 +1992,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 		_ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias);
 		_ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro);
 
-		flow_vel.timestamp = hrt_absolute_time();
+		flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_estimator_optical_flow_vel_pub.publish(flow_vel);
 
@@ -2004,7 +2025,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-void EKF2::UpdateAirspeedSample()
+void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF airspeed sample
 	// prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed)
@@ -2029,7 +2050,7 @@ void EKF2::UpdateAirspeedSample()
 			_airspeed_validated_timestamp_last = airspeed_validated.timestamp;
 		}
 
-	} else if ((hrt_elapsed_time(&_airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
+	} else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
 		// use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable
 		airspeed_s airspeed;
 
@@ -2049,13 +2070,16 @@ void EKF2::UpdateAirspeedSample()
 				};
 				_ekf.setAirspeedData(airspeed_sample);
 			}
+
+			ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
+					(int64_t)ekf2_timestamps.timestamp / 100);
 		}
 	}
 }
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_AUXVEL)
-void EKF2::UpdateAuxVelSample()
+void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF auxiliary velocity sample
 	//  - use the landing target pose estimate as another source of velocity data
@@ -2077,7 +2101,7 @@ void EKF2::UpdateAuxVelSample()
 #endif // CONFIG_EKF2_AUXVEL
 
 #if defined(CONFIG_EKF2_BAROMETER)
-void EKF2::UpdateBaroSample()
+void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF baro sample
 	vehicle_air_data_s airdata;
@@ -2108,12 +2132,15 @@ void EKF2::UpdateBaroSample()
 		_ekf.set_air_density(airdata.rho);
 
 		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset});
+
+		ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
+				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-bool EKF2::UpdateExtVisionSample()
+bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF external vision sample
 	bool new_ev_odom = false;
@@ -2252,6 +2279,9 @@ bool EKF2::UpdateExtVisionSample()
 		if (new_ev_odom)  {
 			_ekf.setExtVisionData(ev_data);
 		}
+
+		ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
+				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 
 	return new_ev_odom;
@@ -2259,7 +2289,7 @@ bool EKF2::UpdateExtVisionSample()
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-bool EKF2::UpdateFlowSample()
+bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF flow sample
 	bool new_optical_flow = false;
@@ -2302,7 +2332,7 @@ bool EKF2::UpdateFlowSample()
 		}
 
 		// use optical_flow distance as range sample if distance_sensor unavailable
-		if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) {
+		if (PX4_ISFINITE(optical_flow.distance_m) && (ekf2_timestamps.timestamp > _last_range_sensor_update + 1_s)) {
 
 			int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
 
@@ -2316,6 +2346,9 @@ bool EKF2::UpdateFlowSample()
 			// set sensor limits
 			_ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance);
 		}
+
+		ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
+				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 
 	return new_optical_flow;
@@ -2323,7 +2356,7 @@ bool EKF2::UpdateFlowSample()
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_GNSS)
-void EKF2::UpdateGpsSample()
+void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF GPS message
 	sensor_gps_s vehicle_gps_position;
@@ -2368,7 +2401,7 @@ void EKF2::UpdateGpsSample()
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-void EKF2::UpdateMagSample()
+void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	vehicle_magnetometer_s magnetometer;
 
@@ -2399,23 +2432,29 @@ void EKF2::UpdateMagSample()
 		}
 
 		_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset});
+
+		ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
+				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-void EKF2::UpdateRangeSample()
+void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	distance_sensor_s distance_sensor;
 
 	if (_distance_sensor_selected < 0) {
+
+		// only consider distance sensors that have updated within the last 0.1s
+		const hrt_abstime timestamp_stale = math::max(ekf2_timestamps.timestamp, 100_ms) - 100_ms;
+
 		if (_distance_sensor_subs.advertised()) {
 			for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
 
 				if (_distance_sensor_subs[i].update(&distance_sensor)) {
-					// only consider distance sensors that have updated within the last 0.1s
 					// only use the first instace which has the correct orientation
-					if ((distance_sensor.timestamp > 100_ms) && (hrt_elapsed_time(&distance_sensor.timestamp) > 100_ms)
+					if ((distance_sensor.timestamp != 0) && (distance_sensor.timestamp > timestamp_stale)
 					    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
 
 						int ndist = orb_group_count(ORB_ID(distance_sensor));
@@ -2446,32 +2485,33 @@ void EKF2::UpdateRangeSample()
 			// Save sensor limits reported by the rangefinder
 			_ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
 
-			_last_range_sensor_update = hrt_absolute_time();
+			_last_range_sensor_update = ekf2_timestamps.timestamp;
 		}
+
+		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) {
+	if (_last_range_sensor_update < ekf2_timestamps.timestamp - 1_s) {
 		// force reselection after timeout
 		_distance_sensor_selected = -1;
 	}
 }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-void EKF2::UpdateSystemFlagsSample()
+void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF system flags
 	if (_status_sub.updated() || _vehicle_land_detected_sub.updated()) {
 
 		systemFlagUpdate flags{};
-		flags.time_us = 0;
+		flags.time_us = ekf2_timestamps.timestamp;
 
 		// vehicle_status
 		vehicle_status_s vehicle_status;
 
 		if (_status_sub.copy(&vehicle_status)
-		    && (hrt_elapsed_time(&vehicle_status.timestamp) < 3_s)) {
-
-			flags.time_us = math::max(flags.time_us, vehicle_status.timestamp);
+		    && (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) {
 
 			// initially set in_air from arming_state (will be overridden if land detector is available)
 			flags.in_air = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
@@ -2494,9 +2534,7 @@ void EKF2::UpdateSystemFlagsSample()
 		vehicle_land_detected_s vehicle_land_detected;
 
 		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
-		    && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 3_s)) {
-
-			flags.time_us = math::max(flags.time_us, vehicle_land_detected.timestamp);
+		    && (ekf2_timestamps.timestamp < vehicle_land_detected.timestamp + 3_s)) {
 
 			flags.at_rest = vehicle_land_detected.at_rest;
 			flags.in_air = !vehicle_land_detected.landed;
@@ -2628,6 +2666,12 @@ 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(CONFIG_EKF2_MULTI_INSTANCE)
 	bool multi_mode = false;
@@ -2687,7 +2731,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 #endif // CONFIG_EKF2_MAGNETOMETER
 	}
 
-	if (multi_mode) {
+	if (multi_mode && !replay_mode) {
 		// Start EKF2Selector if it's not already running
 		if (_ekf2_selector.load() == nullptr) {
 			EKF2Selector *inst = new EKF2Selector();
@@ -2712,7 +2756,7 @@ 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) < 300_s)
+		       && ((hrt_elapsed_time(&time_started) < 30_s)
 			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
 
 			vehicle_status_sub.update();
@@ -2729,7 +2773,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 					if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
 
 						if (!ekf2_instance_created[imu][mag]) {
-							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu));
+							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
 
 							if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
 								int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
@@ -2777,7 +2821,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 	{
 		// otherwise launch regular
-		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0);
+		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
 
 		if (ekf2_inst) {
 			_objects[0].store(ekf2_inst);
@@ -2802,10 +2846,14 @@ 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/main/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_COMMAND("stop");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info");
 #if defined(CONFIG_EKF2_VERBOSE_STATUS)

src/modules/ekf2/EKF2.hpp

@@ -64,6 +64,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/estimator_bias.h>
 #include <uORB/topics/estimator_bias3d.h>
 #include <uORB/topics/estimator_event_flags.h>
@@ -126,7 +127,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	EKF2() = delete;
-	EKF2(bool multi_mode, const px4::wq_config_t &config);
+	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode);
 	~EKF2() override;
 
 	/** @see ModuleBase */
@@ -195,16 +196,16 @@ private:
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-	void UpdateAirspeedSample();
+	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_AUXVEL)
-	void UpdateAuxVelSample();
+	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_AUXVEL
 #if defined(CONFIG_EKF2_BAROMETER)
-	void UpdateBaroSample();
+	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	bool UpdateExtVisionSample();
+	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_GNSS)
 	/*
@@ -215,20 +216,20 @@ private:
 	void PublishGpsStatus(const hrt_abstime &timestamp);
 	void PublishGnssHgtBias(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
-	void UpdateGpsSample();
+	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	bool UpdateFlowSample();
+	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
 	void PublishOpticalFlowVel(const hrt_abstime &timestamp);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	void UpdateMagSample();
+	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	void UpdateRangeSample();
+	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	void UpdateSystemFlagsSample();
+	void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps);
 
 	// Used to check, save and use learned accel/gyro/mag biases
 	struct InFlightCalibration {
@@ -264,6 +265,7 @@ 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};
 
@@ -432,6 +434,7 @@ 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::PublicationMultiData<estimator_event_flags_s>  _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};

src/modules/logger/logged_topics.cpp

@@ -151,68 +151,65 @@ void LoggedTopics::add_default_topics()
 #if CONSTRAINED_MEMORY
 	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1;
 #else
-	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4; // artificially limited until PlotJuggler fixed
+	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed
 	add_optional_topic("estimator_selector_status");
-	add_topic_multi("estimator_attitude", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_global_position", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_local_position", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_wind", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES);
 #endif
 
 	// always add the first instance
-#if 0
-	add_topic_multi("estimator_baro_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_gnss_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_rng_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_ev_pos_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_gps_status", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_innovation_test_ratios", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_innovation_variances", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_innovations", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_states", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("yaw_estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
+	add_topic("estimator_baro_bias", 500);
+	add_topic("estimator_gnss_hgt_bias", 500);
+	add_topic("estimator_rng_hgt_bias", 500);
+	add_topic("estimator_ev_pos_bias", 500);
+	add_topic("estimator_event_flags", 0);
+	add_topic("estimator_gps_status", 1000);
+	add_topic("estimator_innovation_test_ratios", 500);
+	add_topic("estimator_innovation_variances", 500);
+	add_topic("estimator_innovations", 500);
+	add_topic("estimator_optical_flow_vel", 200);
+	add_topic("estimator_sensor_bias", 0);
+	add_topic("estimator_states", 1000);
+	add_topic("estimator_status", 200);
+	add_topic("estimator_status_flags", 0);
+	add_topic("yaw_estimator_status", 1000);
 
-	add_topic_multi("estimator_baro_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_gnss_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_rng_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_ev_pos_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_gps_status", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_innovation_test_ratios", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_innovation_variances", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_innovations", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_states", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("yaw_estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES);
 
-	add_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_ev_pos", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_ev_vel", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
-	add_topic_multi("estimator_aid_src_wind", 0, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES);
-#endif
 
 	// log all raw sensors at minimal rate (at least 1 Hz)
 	add_topic_multi("battery_status", 200, 2);
@@ -341,25 +338,22 @@ 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("airspeed_validated");
-	add_topic("landing_target_pose");
-	add_topic("parameter_update");
-	add_topic("sensor_airflow");
+	add_topic("vehicle_optical_flow");
 	add_topic("sensor_combined");
 	add_topic("sensor_selection");
 	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_optical_flow");
 	add_topic("vehicle_status");
 	add_topic("vehicle_visual_odometry");
 	add_topic("aux_global_position");
 	add_topic_multi("distance_sensor");
-	add_topic_multi("vehicle_imu");
 }
 
 void LoggedTopics::add_thermal_calibration_topics()

src/modules/logger/logger.cpp

@@ -690,8 +690,7 @@ void Logger::run()
 	int next_subscribe_topic_index = -1; // this is used to distribute the checks over time
 
 	if (polling_topic_sub >= 0) {
-		//_lockstep_component = px4_lockstep_register_component();
-		//PX4_ERR("Lockstep component: %i", _lockstep_component);
+		_lockstep_component = px4_lockstep_register_component();
 	}
 
 	bool was_started = false;
@@ -900,7 +899,7 @@ void Logger::run()
 
 		// wait for next loop iteration...
 		if (polling_topic_sub >= 0) {
-			//px4_lockstep_progress(_lockstep_component);
+			px4_lockstep_progress(_lockstep_component);
 
 			px4_pollfd_struct_t fds[1];
 			fds[0].fd = polling_topic_sub;
@@ -929,7 +928,7 @@ void Logger::run()
 		}
 	}
 
-	//px4_lockstep_unregister_component(_lockstep_component);
+	px4_lockstep_unregister_component(_lockstep_component);
 
 	stop_log_file(LogType::Full);
 	stop_log_file(LogType::Mission);
@@ -1473,8 +1472,8 @@ void Logger::start_log_mavlink()
 	}
 
 	// mavlink log does not work in combination with lockstep, it leads to dead-locks
-	//px4_lockstep_unregister_component(_lockstep_component);
-	//_lockstep_component = -1;
+	px4_lockstep_unregister_component(_lockstep_component);
+	_lockstep_component = -1;
 
 	// initialize cpu load as early as possible to get more data
 	initialize_load_output(PrintLoadReason::Preflight);

src/modules/muorb/apps/CMakeLists.txt

@@ -39,7 +39,7 @@ px4_add_module(
 	INCLUDES
 		../test
 		../aggregator
-		libfc-sensor-api/inc
+		${PX4_BOARD_DIR}/libfc-sensor-api/inc
 	SRCS
 		uORBAppsProtobufChannel.cpp
 		muorb_main.cpp

src/modules/muorb/apps/Kconfig

@@ -4,3 +4,11 @@ menuconfig MODULES_MUORB_APPS
 	depends on PLATFORM_POSIX
 	---help---
 		Enable support for muorb apps
+
+
+    config MUORB_APPS_SYNC_TIMESTAMP
+        bool "Sync timestamp with external processor"
+        depends on MODULES_MUORB_APPS
+        default y
+        help
+            causes HRT timestamp to use an externally calculated offset for synchronization

src/modules/px4iofirmware/mixer.cpp

@@ -180,15 +180,19 @@ mixer_tick()
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
-		/* copy failsafe values to the servo outputs */
+		// Set failsafe value if the PWM output isn't disabled
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			r_page_servos[i] = r_page_servo_failsafe[i];
+			if (r_page_servos[i] != 0) {
+				r_page_servos[i] = r_page_servo_failsafe[i];
+			}
 		}
 
 	} else if (source == MIX_DISARMED) {
-		/* copy disarmed values to the servo outputs */
+		// Set disarmed value if the PWM output isn't disabled
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			r_page_servos[i] = r_page_servo_disarmed[i];
+			if (r_page_servos[i] != 0) {
+				r_page_servos[i] = r_page_servo_disarmed[i];
+			}
 		}
 	}
 

src/modules/replay/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2016-2023 PX4 Development Team. All rights reserved.
+#   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
@@ -34,11 +34,11 @@ px4_add_module(
 	MODULE modules__replay
 	MAIN replay
 	COMPILE_FLAGS
-		-O3
-		#-DDEBUG_BUILD
 	SRCS
 		definitions.hpp
 		replay_main.cpp
 		Replay.cpp
 		Replay.hpp
+		ReplayEkf2.cpp
+		ReplayEkf2.hpp
 	)

src/modules/replay/Replay.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2023 PX4 Development Team. All rights reserved.
+ *   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
@@ -49,10 +49,6 @@
 #include <lib/parameters/param.h>
 #include <uORB/uORBMessageFields.hpp>
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_local_position.h>
-
 #include <cstring>
 #include <float.h>
 #include <fstream>
@@ -67,6 +63,7 @@
 #include <logger/messages.h>
 
 #include "Replay.hpp"
+#include "ReplayEkf2.hpp"
 
 #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt"
 #define DYNAMIC_PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params_dynamic.txt"
@@ -79,6 +76,16 @@ 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) {
@@ -88,7 +95,29 @@ Replay::~Replay()
 	_subscriptions.clear();
 }
 
-void Replay::setupReplayFile(const char *file_name)
+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)
 {
 	if (_replay_file) {
 		free(_replay_file);
@@ -97,7 +126,8 @@ void Replay::setupReplayFile(const char *file_name)
 	_replay_file = strdup(file_name);
 }
 
-void Replay::setParameter(const string &parameter_name, const double parameter_value)
+void
+Replay::setParameter(const string &parameter_name, const double parameter_value)
 {
 	param_t handle = param_find(parameter_name.c_str());
 	param_type_t param_format = param_type(handle);
@@ -121,7 +151,7 @@ void Replay::setParameter(const string &parameter_name, const double parameter_v
 		float value = (float)parameter_value;
 
 		if (fabsf(orig_value - value) > FLT_EPSILON) {
-			PX4_WARN("Setting %s (FLOAT) %.6f -> %.6f", param_name(handle), (double)orig_value, (double)value);
+			PX4_WARN("Setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value);
 		}
 
 		param_set(handle, (const void *)&value);
@@ -361,6 +391,7 @@ Replay::readFormat(std::ifstream &file, uint16_t msg_size)
 	return true;
 }
 
+
 string Replay::getOrbFields(const orb_metadata *meta)
 {
 	char format[3000];
@@ -409,53 +440,6 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 	uint8_t multi_id = *(uint8_t *)message;
 	uint16_t msg_id = ((uint16_t)message[1]) | (((uint16_t)message[2]) << 8);
 	string topic_name((char *)message + 3);
-
-
-	const auto whitelist = {
-		"actuator_armed", // HACK
-		"action_request", // HACK
-
-		"airspeed",
-		"airspeed_validated",
-		"landing_target_pose",
-		"parameter_update",
-		"sensor_combined",
-		"sensor_selection",
-		"sensor_wind",
-		"vehicle_air_data",
-		"vehicle_command",
-		"vehicle_gps_position",
-		"vehicle_land_detected",
-		"vehicle_magnetometer",
-		"vehicle_optical_flow",
-		"vehicle_status",
-		"vehicle_visual_odometry",
-		"distance_sensor",
-		"vehicle_imu",
-
-		// "sensor_accel",
-		// "sensor_gyro",
-
-
-	};
-
-	{
-		// TODO: ekf2 replay
-		bool topic_whitelisted = false;
-
-		for (auto &t : whitelist) {
-			if (topic_name.compare(t) == 0) {
-				topic_whitelisted = true;
-				break;
-			}
-		}
-
-		if (!topic_whitelisted) {
-			PX4_DEBUG("readAndAddSubscription: skipping topic %s (not whitelisted)", topic_name.c_str());
-			return true;
-		}
-	}
-
 	const orb_metadata *orb_meta = findTopic(topic_name);
 
 	if (!orb_meta) {
@@ -463,6 +447,7 @@ 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
@@ -471,9 +456,34 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 	const string orb_fields = getOrbFields(orb_meta);
 
 	if (file_format != orb_fields) {
-		PX4_ERR("Formats for %s (msg_id %d) don't match. Will ignore it.", topic_name.c_str(), msg_id);
+		// check if we have a compatibility conversion available
+		if (topic_name == "sensor_combined") {
+			if (orb_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;") {
 
-		if (false) {
+				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_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) &&
+				    findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) &&
+				    findFieldOffset(orb_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:");
 			size_t start = 0;
 
@@ -509,14 +519,15 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 					start = i + 1;
 				}
 			}
-		}
 
-		return true; // not a fatal error
+			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;
@@ -550,32 +561,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 		return true;
 	}
 
-
-
-
-
-
-	{
-		// TODO: ekf2 replay
-		bool topic_whitelisted = false;
-
-		for (auto &t : whitelist) {
-			if (strcmp(subscription->orb_meta->o_name, t) == 0) {
-				topic_whitelisted = true;
-				break;
-			}
-		}
-
-		if (!topic_whitelisted) {
-			PX4_ERR("readAndAddSubscription: skipping topic %s late", subscription->orb_meta->o_name);
-			delete subscription;
-			return true;
-		}
-	}
-
-
-
-	PX4_INFO("adding subscription for %s (msg_id %i)", subscription->orb_meta->o_name, msg_id);
+	PX4_DEBUG("adding subscription for %s (msg_id %i)", subscription->orb_meta->o_name, msg_id);
 
 	//add subscription
 	if (_subscriptions.size() <= msg_id) {
@@ -787,12 +773,6 @@ Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg
 	if (file.eof()) { //no more data messages for this subscription
 		subscription.orb_meta = nullptr;
 		file.clear();
-
-	} else {
-		if (subscription.orb_meta) {
-			subscription.topic_name = (char *)subscription.orb_meta->o_name;
-		}
-
 	}
 
 	return file.good();
@@ -905,15 +885,6 @@ 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)) {
@@ -927,6 +898,10 @@ Replay::run()
 		_speed_factor = atof(speedup);
 	}
 
+	onEnterMainLoop();
+
+	_replay_start_time = hrt_absolute_time();
+
 	PX4_INFO("Replay in progress...");
 
 	ulog_message_header_s message_header;
@@ -935,36 +910,19 @@ Replay::run()
 	//we know the next message must be an ADD_LOGGED_MSG
 	replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
 
-
-
-
-	// TODO: add all subscriptions here
-	//  from whitelist find all topics and subscriptions
-
-
-
-
-
-
 	if (!readAndAddSubscription(replay_file, message_header.msg_size)) {
 		PX4_ERR("Failed to read subscription");
 		return;
 	}
 
+	const uint64_t timestamp_offset = getTimestampOffset();
 	uint32_t nr_published_messages = 0;
 	streampos last_additional_message_pos = _data_section_start;
 
-	bool wait_on_imu_pub[4] {};
-	bool imu_published = false;
-	uint8_t imu_id = 0;
-
-	hrt_abstime last_update = 0;
-
-	PX4_INFO("Replay begin");
-
 	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
+
+		//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;
@@ -986,24 +944,18 @@ 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 || next_file_time < _file_start_time) {
-			// 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);
+			//someone didn't set the timestamp properly. Consider the message invalid
 			nextDataMessage(replay_file, sub, next_msg_id);
 			continue;
 		}
 
-		if (findTopic(_publisher_rule, sub.orb_meta->o_name)) {
-			PX4_DEBUG("not allowing publish topic %s", sub.orb_meta->o_name);
-			//return (orb_advert_t)_Instance;
-		}
-
-		// handle additional messages between last and next published data
+		//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);
@@ -1020,164 +972,14 @@ Replay::run()
 			_next_param_change++;
 		}
 
-		// Perform scheduled parameter changes
-		while (_next_param_change < _dynamic_parameter_schedule.size() &&
-		       _dynamic_parameter_schedule[_next_param_change].timestamp <= next_file_time) {
+		const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset);
 
-			const auto param_change = _dynamic_parameter_schedule[_next_param_change];
-			PX4_WARN("Performing param change scheduled for t=%.3lf at t=%.3lf.",
-				 (double)param_change.timestamp / 1.e6,
-				 (double)next_file_time / 1.e6);
+		// It's time to publish
+		readTopicDataToBuffer(sub, replay_file);
+		memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp
 
-			setParameter(param_change.parameter_name, param_change.parameter_value);
-			_next_param_change++;
-		}
-
-
-
-		bool publish_msg = true;
-
-		const uint64_t publish_timestamp = sub.next_timestamp;
-
-
-		// // TODO: ekf2 replay
-		// const auto whitelist = {
-		// 	"airspeed",
-		// 	"airspeed_validated",
-		// 	"landing_target_pose",
-		// 	"parameter_update",
-		// 	"sensor_combined",
-		// 	"sensor_selection",
-		// 	"sensor_wind",
-		// 	"vehicle_air_data",
-		// 	"vehicle_command",
-		// 	"vehicle_gps_position",
-		// 	"vehicle_land_detected",
-		// 	"vehicle_magnetometer",
-		// 	"vehicle_optical_flow",
-		// 	"vehicle_status",
-		// 	"vehicle_visual_odometry",
-		// 	"distance_sensor",
-		// 	"vehicle_imu",
-
-		// 	"sensor_accel",
-		// 	"sensor_gyro",
-		// };
-
-		// if (sub.orb_meta) {
-
-		// 	bool topic_whitelisted = false;
-
-		// 	for (auto &topic : whitelist) {
-		// 		if (strcmp(sub.orb_meta->o_name, topic) == 0) {
-		// 			topic_whitelisted = true;
-		// 			break;
-		// 		}
-		// 	}
-
-		// 	if (!topic_whitelisted) {
-
-		// 		// skip
-		// 		PX4_DEBUG("%lu [%lu] (+%lu us) skipping %s:%d", hrt_absolute_time(), publish_timestamp,
-		// 			  publish_timestamp - timestamp_last,
-		// 			  sub.orb_meta->o_name, sub.multi_id);
-
-		// 		//continue;
-
-		// 		publish_msg = false;
-		// 	}
-		// }
-
-		if (publish_msg) {
-
-			// It's time to publish
-			readTopicDataToBuffer(sub, replay_file);
-
-			if (publish_timestamp >= timestamp_last) {
-
-				// 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();
-				}
-
-				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 %" PRIu64 " (last timestamp %" PRIu64 ")", sub.orb_meta->o_name, sub.multi_id,
-					publish_timestamp,
-					timestamp_last);
-			}
-
-			if (sub.orb_advert) {
-
-
-
-			}
-
-			if (publishTopic(sub, _read_buffer.data())) {
-
-				if (strcmp(sub.orb_meta->o_name, "vehicle_imu") == 0) {
-
-					imu_published = true;
-					imu_id = sub.multi_id;
-					//fprintf(stderr, "\n\n\npublished IMU:%d\n", imu_id);
-				}
-
-				++nr_published_messages;
-			}
-
-			timestamp_last = publish_timestamp;
-
-
-			// Wait for other modules, such as logger or ekf2
-			if (imu_published) {
-
-
-				if (!wait_on_imu_pub[imu_id]) {
-
-
-					uORB::SubscriptionData<vehicle_local_position_s> est_sub{ORB_ID(estimator_local_position), imu_id};
-
-					est_sub.update();
-
-					if (est_sub.get().timestamp >= publish_timestamp) {
-						PX4_INFO("will now wait on IMU %d", imu_id);
-						wait_on_imu_pub[imu_id] = true;
-					}
-
-				} else {
-					//PX4_WARN("LOCKSTEP START wait for components, IMU:%d", imu_id);
-					px4_lockstep_wait_for_components();
-					//PX4_WARN("LOCKSTEP FINISH waited for components, IMU:%d", imu_id);
-				}
-			}
-
-
-			if (hrt_elapsed_time(&last_update) >= 10_s) {
-
-				// char buf[80];
-
-				// {
-				// 	struct timespec now;
-				// 	system_clock_gettime(CLOCK_REALTIME, &now);
-				// 	time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
-
-				// 	//convert to date time
-
-				// 	struct tm date_time;
-				// 	localtime_r(&utc_time_sec, &date_time);
-				// 	strftime(buf, sizeof(buf), "%a %Y-%m-%d %H:%M:%S %Z", &date_time);
-				// }
-
-				PX4_INFO("[%.6fs] %d messages published", timestamp_last * 1e-6, nr_published_messages);
-				last_update = timestamp_last;
-			}
+		if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) {
+			++nr_published_messages;
 		}
 
 		nextDataMessage(replay_file, sub, next_msg_id);
@@ -1185,25 +987,19 @@ Replay::run()
 		// TODO: output status (eg. every sec), including total duration...
 	}
 
-	for (auto &sub : _subscriptions) {
-		if (!sub) {
+	for (auto &subscription : _subscriptions) {
+		if (!subscription) {
 			continue;
 		}
 
-		if (sub && (sub->publication_counter > 0 || sub->error_counter > 0)) {
-
-			if (sub->topic_name) {
-				//PX4_INFO("%s: %i, %i", sub->orb_meta->o_name, sub->publication_counter, sub->error_counter);
-				PX4_INFO("%s: %i, %i", sub->topic_name, sub->publication_counter, sub->error_counter);
-
-			} else {
-				PX4_WARN("%s: %i, %i", "HUH?", sub->publication_counter, sub->error_counter);
-			}
+		if (subscription->compat) {
+			delete subscription->compat;
+			subscription->compat = nullptr;
 		}
 
-		if (sub->orb_advert) {
-			orb_unadvertise(sub->orb_advert);
-			sub->orb_advert = nullptr;
+		if (subscription->orb_advert) {
+			orb_unadvertise(subscription->orb_advert);
+			subscription->orb_advert = nullptr;
 		}
 	}
 
@@ -1212,6 +1008,8 @@ Replay::run()
 			 (double)hrt_elapsed_time(&_replay_start_time) / 1.e6);
 	}
 
+	onExitMainLoop();
+
 	if (!should_exit()) {
 		replay_file.close();
 		px4_shutdown_request();
@@ -1237,11 +1035,50 @@ 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;
@@ -1283,123 +1120,6 @@ 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);
-
-	if (lenstr < lenpre) {
-		return false;
-	}
-
-	return (strncmp(pre, str, lenpre) == 0);
-}
-
-bool Replay::findTopic(const PublisherRule &rule, const char *topic_name)
-{
-	const char **topics_ptr = rule.topics;
-
-	while (topics_ptr && *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)
-{
-	static const int line_len = 1024;
-	int ret = PX4_OK;
-	char *line = new char[line_len];
-
-	if (!line) {
-		return -ENOMEM;
-	}
-
-	FILE *fp = fopen(file_name, "r");
-
-	if (fp == NULL) {
-		delete[](line);
-		return -errno;
-	}
-
-	const char *restrict_topics_str = "restrict_topics:";
-
-	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;
-			int 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 {
-			PX4_ERR("orb rules file: wrong format: %s", line);
-			ret = -EINVAL;
-		}
-	}
-
-	if (ret == PX4_OK && !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[])
 {
@@ -1411,8 +1131,6 @@ Replay::custom_command(int argc, char *argv[])
 		return Replay::task_spawn(argc, argv);
 	}
 
-	// ignore?
-
 	return print_usage("unknown command");
 }
 
@@ -1479,9 +1197,17 @@ 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 = new Replay();
+	Replay *instance = nullptr;
+
+	if (replay_mode && strcmp(replay_mode, "ekf2") == 0) {
+		PX4_INFO("Ekf2 replay mode");
+		instance = new ReplayEkf2();
+
+	} else {
+		instance = new Replay();
+	}
 
 	return instance;
 }
@@ -1498,7 +1224,7 @@ Replay::print_usage(const char *reason)
 ### Description
 This module is used to replay ULog files.
 
-There are 2 environment variables used for configuration: `replay_file`, which must be set to an ULog file name - it's
+There are 2 environment variables used for configuration: `replay`, which must be set to an ULog file name - it's
 the log file to be replayed. The second is the mode, specified via `replay_mode`:
 - `replay_mode=ekf2`: specific EKF2 replay mode. It can only be used with the ekf2 module, but allows the replay
   to run as fast as possible.

src/modules/replay/Replay.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2024 PX4 Development Team. All rights reserved.
+ *   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
@@ -44,6 +44,7 @@
 
 #include <px4_platform_common/module.h>
 #include <uORB/topics/uORBTopics.hpp>
+#include <uORB/topics/ekf2_timestamps.h>
 
 namespace px4
 {
@@ -95,10 +96,39 @@ 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
-		char *topic_name = nullptr;
 		orb_advert_t orb_advert = nullptr;
 		uint8_t multi_id;
 		int timestamp_offset; ///< marks the field of the timestamp
@@ -108,6 +138,8 @@ 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;
@@ -131,16 +163,34 @@ 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
 	 */
-	void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {}
+	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);
 
 	/**
 	 * handle the publication of a topic update
 	 * @return true if published, false otherwise
 	 */
-	bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
+	virtual bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
 
 	/**
 	 * read a topic from the file (offset given by the subscription) into _read_buffer
@@ -157,7 +207,7 @@ protected:
 	 */
 	bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
 
-	uint64_t getTimestampOffset()
+	virtual uint64_t getTimestampOffset()
 	{
 		//we update the timestamps from the file by a constant offset to match
 		//the current replay time
@@ -250,43 +300,6 @@ private:
 	std::string getOrbFields(const orb_metadata *meta);
 
 	static char *_replay_file;
-
-
-
-
-	struct PublisherRule {
-		const char **topics{nullptr}; //null-terminated list of topic names
-	};
-
-	/**
-	 * 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

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   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>
+
+#include <lib/parameters/param.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/sensor_combined.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_global_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_optical_flow.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(vehicle_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;
+
+	} else if (sub.orb_meta == ORB_ID(aux_global_position)) {
+		_aux_global_position_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);
+	handle_sensor_publication(0, _aux_global_position_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
+
+	// disable parameter auto save
+	param_control_autosave(false);
+}
+
+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, "vehicle_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");
+	print_sensor_statistics(_aux_global_position_msg_id, "aux_global_position");
+}
+
+} // namespace px4

src/modules/replay/ReplayEkf2.hpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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;
+	uint16_t _aux_global_position_msg_id = msg_id_invalid;
+};
+
+} //namespace px4

src/modules/replay/definitions.hpp

@@ -38,7 +38,7 @@ namespace px4
 namespace replay
 {
 
-static const char __attribute__((unused)) *ENV_FILENAME = "replay_file"; ///< name for getenv()
+static const char __attribute__((unused)) *ENV_FILENAME = "replay"; ///< name for getenv()
 static const char __attribute__((unused)) *ENV_MODE = "replay_mode";  ///< name for getenv()
 
 

src/modules/replay/replay_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2023 PX4 Development Team. All rights reserved.
+ *   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
@@ -35,7 +35,8 @@
 
 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/systemcmds/uorb/uorb.cpp

@@ -78,6 +78,9 @@ 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