logged topics, don't get greedy

.ci/Jenkinsfile-hardware

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

@@ -1,14 +0,0 @@
-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 = boards/modalai/voxl2/libfc-sensor-api
+	path = src/modules/muorb/apps/libfc-sensor-api
 	url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

Makefile

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

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

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

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

@@ -6,35 +6,36 @@ 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
@@ -190,7 +191,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_ekf2

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

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

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

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

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

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

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

@@ -0,0 +1,28 @@
+#!/bin/sh
+
+# EKF2 replay script
+
+# shellcheck disable=SC2154
+if [ ! -f ${replay} ]; then
+	echo "Invalid replay log file ${replay}"
+	exit 1
+fi
+
+if [ ! -f replay_params.txt ]; then
+	echo "Creating $(pwd)/replay_params.txt"
+	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+fi
+
+param set SDLOG_DIRS_MAX 7
+
+# TODO:
+# input: 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,11 +17,10 @@ PATH="$PATH:${R}etc/init.d-posix"
 # Main SITL startup script
 #
 
-# check for ekf2 replay
-# shellcheck disable=SC2154
-if [ "$replay_mode" = "ekf2" ]
-then
-	. ${R}etc/init.d-posix/rc.replay
+# check for replay
+if [ ! -z ${replay_mode+x} ]; then
+	echo "Replay Mode: ${replay_mode}"
+	. ${R}etc/init.d-posix/rc.replay_${replay_mode}
 	exit 0
 fi
 
@@ -330,12 +329,7 @@ fi
 [ -e "$autostart_file".post ] && . "$autostart_file".post
 
 # Run script to start logging
-if param compare SYS_MC_EST_GROUP 2
-then
-	set LOGGER_ARGS "-p ekf2_timestamps"
-else
-	set LOGGER_ARGS "-p vehicle_attitude"
-fi
+set LOGGER_ARGS "-p vehicle_attitude"
 . ${R}etc/init.d/rc.logging
 
 mavlink boot_complete

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 boards/modalai/voxl2/libfc-sensor-api -prune -o \
+    -path src/modules/muorb/apps/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,7 +4,6 @@ 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

@@ -1,34 +0,0 @@
-############################################################################
-#
-#   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_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
+		${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
 		px4_layer
 		${module_libraries}
 )

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

@@ -1,9 +1,10 @@
 #!/bin/bash
 
-cd boards/modalai/voxl2/libfc-sensor-api
+cd src/modules/muorb/apps/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,6 +81,7 @@ 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
@@ -101,6 +102,7 @@ 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,7 +74,6 @@ set(msg_files
 	DifferentialDriveSetpoint.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
-	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
 	EstimatorAidSource1d.msg

msg/Ekf2Timestamps.msg

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

platforms/common/CMakeLists.txt

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

@@ -1,138 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,97 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   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,21 +13,11 @@ __END_DECLS
 #define px4_clock_gettime system_clock_gettime
 #endif
 
-#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_clock_settime(clockid_t clk_id, const struct timespec *tp);
+
 __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,
@@ -37,6 +27,7 @@ __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,6 +56,31 @@ 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,10 +412,7 @@ 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.
-		// 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)));
-		}
+		ch->send_message(_meta->o_name, _meta->o_size, _data);
 	}
 
 	return PX4_OK;

platforms/common/uORB/uORBManager.cpp

@@ -77,24 +77,6 @@ bool uORB::Manager::terminate()
 	return false;
 }
 
-uORB::Manager::Manager()
-{
-#ifdef ORB_USE_PUBLISHER_RULES
-	const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules";
-	int ret = readPublisherRulesFromFile(file_name, _publisher_rule);
-
-	if (ret == PX4_OK) {
-		_has_publisher_rules = true;
-		PX4_INFO("Using orb rules from %s", file_name);
-
-	} else {
-		PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret));
-	}
-
-#endif /* ORB_USE_PUBLISHER_RULES */
-
-}
-
 uORB::Manager::~Manager()
 {
 	delete _device_master;
@@ -267,30 +249,6 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 
 orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
-#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);
 
@@ -335,14 +293,6 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 
 int uORB::Manager::orb_unadvertise(orb_advert_t handle)
 {
-#ifdef ORB_USE_PUBLISHER_RULES
-
-	if (handle == _Instance) {
-		return PX4_OK; //pretend success
-	}
-
-#endif /* ORB_USE_PUBLISHER_RULES */
-
 	return uORB::DeviceNode::unadvertise(handle);
 }
 
@@ -364,14 +314,6 @@ 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);
 }
 
@@ -715,142 +657,3 @@ 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();
+	Manager() = default;
 	virtual ~Manager();
 
 #ifdef CONFIG_ORB_COMMUNICATOR
@@ -559,46 +559,6 @@ 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");
+	PX4_ERR("logic error (not expected to get here)");
 	_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");
+		PX4_ERR("logic error (format_idx %d, len %d)", format_idx, len);
 		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 64
+set LOGGER_BUF 256

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

@@ -1,394 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- *   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,8 +3,6 @@
 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,8 +15,6 @@ 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

@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   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,8 +46,6 @@ 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

@@ -1,387 +0,0 @@
-/****************************************************************************
- *
- *   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,11 +51,6 @@
 #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};
@@ -112,29 +107,6 @@ 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)
 }
@@ -477,7 +449,6 @@ 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,4 +50,5 @@ add_library(px4 SHARED
 target_link_libraries(px4
 	modules__muorb__slpi
 	${module_libraries}
+	px4_layer
 	)

platforms/qurt/include/SerialImpl.hpp

@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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,7 +38,6 @@ 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

@@ -1,326 +0,0 @@
-/****************************************************************************
- *
- *   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, const struct timespec *tp)
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
 {
 	return 0;
 }

src/drivers/drv_hrt.h

@@ -162,16 +162,7 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
  */
 static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *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;
+	return hrt_absolute_time() - *then;
 }
 
 /**

src/drivers/gps/gps.cpp

@@ -45,6 +45,7 @@
 #include <poll.h>
 #endif
 
+#include <termios.h>
 #include <cstring>
 
 #include <drivers/drv_sensor.h>
@@ -56,8 +57,6 @@
 #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>
@@ -82,7 +81,6 @@
 #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
@@ -171,10 +169,7 @@ public:
 	void reset_if_scheduled();
 
 private:
-#ifdef __PX4_LINUX
-	int				_spi_fd {-1};					///< SPI interface to GPS
-#endif
-	Serial 			*_uart = nullptr;				///< UART interface to GPS
+	int				_serial_fd{-1};					///< serial 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
@@ -334,11 +329,8 @@ 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) {
@@ -411,23 +403,10 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 			return num_read;
 		}
 
-	case GPSCallbackType::writeDeviceData: {
+	case GPSCallbackType::writeDeviceData:
 		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true);
 
-			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;
-		}
+		return ::write(gps->_serial_fd, data1, (size_t)data2);
 
 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -470,33 +449,25 @@ 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 ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-		ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
+#if !defined(__PX4_QURT)
 
-// SPI is only supported on LInux
-#if defined(__PX4_LINUX)
+	/* For non QURT, use the usual polling. */
 
-	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
-
-		//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
+	//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;
 
 	pollfd fds[1];
-		fds[0].fd = _spi_fd;
+	fds[0].fd = _serial_fd;
 	fds[0].events = POLLIN;
 
-		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
+	int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
 
 	if (ret > 0) {
 		/* if we have new data from GPS, go handle it */
@@ -508,12 +479,24 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 			 * 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);
 
-				px4_usleep(sleeptime);
+#ifdef __PX4_NUTTX
+			int err = 0;
+			int bytes_available = 0;
+			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
 
-				ret = ::read(_spi_fd, buf, buf_length);
+			if (err != 0 || bytes_available < (int)character_count) {
+				px4_usleep(sleeptime);
+			}
+
+#else
+			px4_usleep(sleeptime);
+#endif
+
+			ret = ::read(_serial_fd, buf, buf_length);
 
 			if (ret > 0) {
 				_num_bytes_read += ret;
@@ -524,10 +507,14 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 		}
 	}
 
-#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()
@@ -596,38 +583,105 @@ bool GPS::injectData(uint8_t *data, size_t len)
 {
 	dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
 
-	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
-	}
-
+	size_t written = ::write(_serial_fd, data, len);
+	::fsync(_serial_fd);
 	return written == len;
 }
 
 int GPS::setBaudrate(unsigned baud)
 {
-	if (_interface == GPSHelper::Interface::UART) {
-		if ((_uart) && (_uart->setBaudrate(baud))) {
-			return 0;
-		}
+	/* process baud rate */
+	int speed;
 
-#ifdef __PX4_LINUX
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
 
-	} else if (_interface == GPSHelper::Interface::SPI) {
-		// Can't set the baudrate on a SPI port but just return a success
-		return 0;
+	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", 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;
 }
 
 void GPS::initializeCommunicationDump()
@@ -786,59 +840,32 @@ GPS::run()
 			_helper = nullptr;
 		}
 
-		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
+		if (_serial_fd < 0) {
+			/* open the serial port */
+			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
-			// 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);
+			if (_serial_fd < 0) {
+				PX4_ERR("failed to open %s err: %d", _port, errno);
 				px4_sleep(1);
 				continue;
 			}
 
 #ifdef __PX4_LINUX
 
-		} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) {
-			_spi_fd = ::open(_port, O_RDWR | O_NOCTTY);
-
-			if (_spi_fd < 0) {
-				PX4_ERR("failed to open SPI port %s err: %d", _port, errno);
-				px4_sleep(1);
-				continue;
-			}
-
+			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(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+				int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
 
 				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);
+				status_value = ::ioctl(_serial_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 */
 		}
@@ -1029,17 +1056,9 @@ GPS::run()
 			}
 		}
 
-		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 (_serial_fd >= 0) {
+			::close(_serial_fd);
+			_serial_fd = -1;
 		}
 
 		if (_mode_auto) {
@@ -1387,7 +1406,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;
 	}
 
@@ -1458,12 +1477,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'i':
-			if (!strcmp(myoptarg, "uart")) {
-				interface = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface: %s", myoptarg);
 				error_flag = true;
@@ -1471,12 +1490,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			break;
 
 		case 'j':
-			if (!strcmp(myoptarg, "uart")) {
-				interface_secondary = GPSHelper::Interface::UART;
-#ifdef __PX4_LINUX
-			} else if (!strcmp(myoptarg, "spi")) {
+			if (!strcmp(myoptarg, "spi")) {
 				interface_secondary = GPSHelper::Interface::SPI;
-#endif
+
+			} else if (!strcmp(myoptarg, "uart")) {
+				interface_secondary = GPSHelper::Interface::UART;
+
 			} else {
 				PX4_ERR("unknown interface for secondary: %s", myoptarg);
 				error_flag = true;

src/drivers/px4io/px4io.cpp

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

src/lib/mixer_module/mixer_module.hpp

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

src/modules/ekf2/EKF2.cpp

@@ -49,10 +49,9 @@ 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, bool replay_mode):
+EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config):
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, config),
-	_replay_mode(replay_mode && !multi_mode),
 	_multi_mode(multi_mode),
 	_instance(multi_mode ? -1 : 0),
 	_attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
@@ -237,7 +236,6 @@ 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();
@@ -703,42 +701,32 @@ 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(ekf2_timestamps);
+		UpdateAirspeedSample();
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_AUXVEL)
-		UpdateAuxVelSample(ekf2_timestamps);
+		UpdateAuxVelSample();
 #endif // CONFIG_EKF2_AUXVEL
 #if defined(CONFIG_EKF2_BAROMETER)
-		UpdateBaroSample(ekf2_timestamps);
+		UpdateBaroSample();
 #endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-		UpdateExtVisionSample(ekf2_timestamps);
+		UpdateExtVisionSample();
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-		UpdateFlowSample(ekf2_timestamps);
+		UpdateFlowSample();
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 #if defined(CONFIG_EKF2_GNSS)
-		UpdateGpsSample(ekf2_timestamps);
+		UpdateGpsSample();
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-		UpdateMagSample(ekf2_timestamps);
+		UpdateMagSample();
 #endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-		UpdateRangeSample(ekf2_timestamps);
+		UpdateRangeSample();
 #endif // CONFIG_EKF2_RANGE_FINDER
-		UpdateSystemFlagsSample(ekf2_timestamps);
+
+		UpdateSystemFlagsSample();
 
 		// run the EKF update and output
 		const hrt_abstime ekf_update_start = hrt_absolute_time();
@@ -793,9 +781,6 @@ void EKF2::Run()
 			UpdateMagCalibration(now);
 #endif // CONFIG_EKF2_MAGNETOMETER
 		}
-
-		// publish ekf2_timestamps
-		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
 
 	// re-schedule as backup timeout
@@ -921,13 +906,7 @@ 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 = _replay_mode ? timestamp : hrt_absolute_time();
-		_attitude_pub.publish(att);
-
-	}  else if (_replay_mode) {
-		// in replay mode we have to tell the replay module not to wait for an update
-		// we do this by publishing an attitude with zero timestamp
-		vehicle_attitude_s att{};
+		att.timestamp = hrt_absolute_time();
 		_attitude_pub.publish(att);
 	}
 }
@@ -1004,7 +983,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 = _replay_mode ? timestamp : hrt_absolute_time();
+			bias.timestamp = hrt_absolute_time();
 			_estimator_ev_pos_bias_pub.publish(bias);
 
 			_last_ev_bias_published = Vector3f(bias.bias);
@@ -1024,7 +1003,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 = _replay_mode ? timestamp : hrt_absolute_time();
+	bias.timestamp = hrt_absolute_time();
 
 	return bias;
 }
@@ -1086,7 +1065,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 		event_flags.emergency_yaw_reset_mag_stopped     = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
 		event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped;
 
-		event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		event_flags.timestamp = hrt_absolute_time();
 		_estimator_event_flags_pub.update(event_flags);
 
 		_last_event_flags_publish = event_flags.timestamp;
@@ -1096,7 +1075,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 = _replay_mode ? timestamp : hrt_absolute_time();
+		_estimator_event_flags_pub.get().timestamp = hrt_absolute_time();
 		_estimator_event_flags_pub.update();
 		_last_event_flags_publish = _estimator_event_flags_pub.get().timestamp;
 	}
@@ -1153,7 +1132,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 = _replay_mode ? timestamp : hrt_absolute_time();
+		global_pos.timestamp = hrt_absolute_time();
 		_global_position_pub.publish(global_pos);
 	}
 }
@@ -1187,7 +1166,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 = _replay_mode ? timestamp : hrt_absolute_time();
+	estimator_gps_status.timestamp = hrt_absolute_time();
 	_estimator_gps_status_pub.publish(estimator_gps_status);
 
 
@@ -1288,7 +1267,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	innovations.hagl_rate = _ekf.getHaglRateInnov();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	innovations.timestamp = hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
 
 	// calculate noise filtered velocity innovations which are used for pre-flight checking
@@ -1422,7 +1401,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	test_ratios.timestamp = hrt_absolute_time();
 	_estimator_innovation_test_ratios_pub.publish(test_ratios);
 }
 
@@ -1519,7 +1498,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	variances.hagl_rate = _ekf.getHaglRateInnovVar();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	variances.timestamp = hrt_absolute_time();
 	_estimator_innovation_variances_pub.publish(variances);
 }
 
@@ -1625,7 +1604,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	}
 
 	// publish vehicle local position data
-	lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	lpos.timestamp = hrt_absolute_time();
 	_local_position_pub.publish(lpos);
 }
 
@@ -1667,7 +1646,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
 	odom.quality = 0;
 
 	// publish vehicle odometry data
-	odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	odom.timestamp = hrt_absolute_time();
 	_odometry_pub.publish(odom);
 }
 
@@ -1733,7 +1712,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-		bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		bias.timestamp = hrt_absolute_time();
 		_estimator_sensor_bias_pub.publish(bias);
 
 		_last_sensor_bias_published = bias.timestamp;
@@ -1749,7 +1728,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 = _replay_mode ? timestamp : hrt_absolute_time();
+	states.timestamp = hrt_absolute_time();
 	_estimator_states_pub.publish(states);
 }
 
@@ -1810,7 +1789,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 			    status.mag_strength_ref_gs);
 #endif // CONFIG_EKF2_MAGNETOMETER
 
-	status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	status.timestamp = hrt_absolute_time();
 	_estimator_status_pub.publish(status);
 }
 
@@ -1911,7 +1890,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_optflow_x                = _ekf.innov_check_fail_status_flags().reject_optflow_X;
 		status_flags.reject_optflow_y                = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
 
-		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		status_flags.timestamp = hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
 
 		_last_status_flags_publish = status_flags.timestamp;
@@ -1933,7 +1912,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 = _replay_mode ? timestamp : hrt_absolute_time();
+		yaw_est_test_data.timestamp = hrt_absolute_time();
 
 		_yaw_est_pub.publish(yaw_est_test_data);
 	}
@@ -1964,7 +1943,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 		wind.windspeed_east = wind_vel(1);
 		wind.variance_north = wind_vel_var(0);
 		wind.variance_east = wind_vel_var(1);
-		wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		wind.timestamp = hrt_absolute_time();
 
 		_wind_pub.publish(wind);
 	}
@@ -1992,7 +1971,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 = _replay_mode ? timestamp : hrt_absolute_time();
+		flow_vel.timestamp = hrt_absolute_time();
 
 		_estimator_optical_flow_vel_pub.publish(flow_vel);
 
@@ -2025,7 +2004,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateAirspeedSample()
 {
 	// EKF airspeed sample
 	// prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed)
@@ -2050,7 +2029,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 			_airspeed_validated_timestamp_last = airspeed_validated.timestamp;
 		}
 
-	} else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
+	} else if ((hrt_elapsed_time(&_airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
 		// use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable
 		airspeed_s airspeed;
 
@@ -2070,16 +2049,13 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 				};
 				_ekf.setAirspeedData(airspeed_sample);
 			}
-
-			ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
-					(int64_t)ekf2_timestamps.timestamp / 100);
 		}
 	}
 }
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_AUXVEL)
-void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateAuxVelSample()
 {
 	// EKF auxiliary velocity sample
 	//  - use the landing target pose estimate as another source of velocity data
@@ -2101,7 +2077,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 #endif // CONFIG_EKF2_AUXVEL
 
 #if defined(CONFIG_EKF2_BAROMETER)
-void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateBaroSample()
 {
 	// EKF baro sample
 	vehicle_air_data_s airdata;
@@ -2132,15 +2108,12 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 		_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(ekf2_timestamps_s &ekf2_timestamps)
+bool EKF2::UpdateExtVisionSample()
 {
 	// EKF external vision sample
 	bool new_ev_odom = false;
@@ -2279,9 +2252,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
 		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;
@@ -2289,7 +2259,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
+bool EKF2::UpdateFlowSample()
 {
 	// EKF flow sample
 	bool new_optical_flow = false;
@@ -2332,7 +2302,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 
 		// use optical_flow distance as range sample if distance_sensor unavailable
-		if (PX4_ISFINITE(optical_flow.distance_m) && (ekf2_timestamps.timestamp > _last_range_sensor_update + 1_s)) {
+		if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) {
 
 			int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
 
@@ -2346,9 +2316,6 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 			// 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;
@@ -2356,7 +2323,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_GNSS)
-void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateGpsSample()
 {
 	// EKF GPS message
 	sensor_gps_s vehicle_gps_position;
@@ -2401,7 +2368,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateMagSample()
 {
 	vehicle_magnetometer_s magnetometer;
 
@@ -2432,29 +2399,23 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 
 		_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(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateRangeSample()
 {
 	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 != 0) && (distance_sensor.timestamp > timestamp_stale)
+					if ((distance_sensor.timestamp > 100_ms) && (hrt_elapsed_time(&distance_sensor.timestamp) > 100_ms)
 					    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
 
 						int ndist = orb_group_count(ORB_ID(distance_sensor));
@@ -2485,33 +2446,32 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 			// Save sensor limits reported by the rangefinder
 			_ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
 
-			_last_range_sensor_update = ekf2_timestamps.timestamp;
+			_last_range_sensor_update = hrt_absolute_time();
+		}
 	}
 
-		ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
-	}
-
-	if (_last_range_sensor_update < ekf2_timestamps.timestamp - 1_s) {
+	if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
 		// force reselection after timeout
 		_distance_sensor_selected = -1;
 	}
 }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
+void EKF2::UpdateSystemFlagsSample()
 {
 	// EKF system flags
 	if (_status_sub.updated() || _vehicle_land_detected_sub.updated()) {
 
 		systemFlagUpdate flags{};
-		flags.time_us = ekf2_timestamps.timestamp;
+		flags.time_us = 0;
 
 		// vehicle_status
 		vehicle_status_s vehicle_status;
 
 		if (_status_sub.copy(&vehicle_status)
-		    && (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) {
+		    && (hrt_elapsed_time(&vehicle_status.timestamp) < 3_s)) {
+
+			flags.time_us = math::max(flags.time_us, vehicle_status.timestamp);
 
 			// 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);
@@ -2534,7 +2494,9 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 		vehicle_land_detected_s vehicle_land_detected;
 
 		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
-		    && (ekf2_timestamps.timestamp < vehicle_land_detected.timestamp + 3_s)) {
+		    && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 3_s)) {
+
+			flags.time_us = math::max(flags.time_us, vehicle_land_detected.timestamp);
 
 			flags.at_rest = vehicle_land_detected.at_rest;
 			flags.in_air = !vehicle_land_detected.landed;
@@ -2666,12 +2628,6 @@ int EKF2::custom_command(int argc, char *argv[])
 int EKF2::task_spawn(int argc, char *argv[])
 {
 	bool success = false;
-	bool replay_mode = false;
-
-	if (argc > 1 && !strcmp(argv[1], "-r")) {
-		PX4_INFO("replay mode enabled");
-		replay_mode = true;
-	}
 
 #if defined(CONFIG_EKF2_MULTI_INSTANCE)
 	bool multi_mode = false;
@@ -2731,7 +2687,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 #endif // CONFIG_EKF2_MAGNETOMETER
 	}
 
-	if (multi_mode && !replay_mode) {
+	if (multi_mode) {
 		// Start EKF2Selector if it's not already running
 		if (_ekf2_selector.load() == nullptr) {
 			EKF2Selector *inst = new EKF2Selector();
@@ -2756,7 +2712,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) < 30_s)
+		       && ((hrt_elapsed_time(&time_started) < 300_s)
 			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
 
 			vehicle_status_sub.update();
@@ -2773,7 +2729,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), false);
+							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu));
 
 							if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
 								int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
@@ -2821,7 +2777,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 	{
 		// otherwise launch regular
-		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
+		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0);
 
 		if (ekf2_inst) {
 			_objects[0].store(ekf2_inst);
@@ -2846,14 +2802,10 @@ Attitude and position estimator using an Extended Kalman Filter. It is used for
 
 The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/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,7 +64,6 @@
 #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>
@@ -127,7 +126,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	EKF2() = delete;
-	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode);
+	EKF2(bool multi_mode, const px4::wq_config_t &config);
 	~EKF2() override;
 
 	/** @see ModuleBase */
@@ -196,16 +195,16 @@ private:
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateAirspeedSample();
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_AUXVEL)
-	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateAuxVelSample();
 #endif // CONFIG_EKF2_AUXVEL
 #if defined(CONFIG_EKF2_BAROMETER)
-	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateBaroSample();
 #endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
+	bool UpdateExtVisionSample();
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_GNSS)
 	/*
@@ -216,20 +215,20 @@ private:
 	void PublishGpsStatus(const hrt_abstime &timestamp);
 	void PublishGnssHgtBias(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
-	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateGpsSample();
 #endif // CONFIG_EKF2_GNSS
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
+	bool UpdateFlowSample();
 	void PublishOpticalFlowVel(const hrt_abstime &timestamp);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateMagSample();
 #endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateRangeSample();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateSystemFlagsSample();
 
 	// Used to check, save and use learned accel/gyro/mag biases
 	struct InFlightCalibration {
@@ -265,7 +264,6 @@ private:
 
 	static constexpr float sq(float x) { return x * x; };
 
-	const bool _replay_mode{false};			///< true when we use replay data from a log
 	const bool _multi_mode;
 	int _instance{0};
 
@@ -434,7 +432,6 @@ private:
 	uint32_t _filter_warning_event_changes{0};
 	uint32_t _filter_information_event_changes{0};
 
-	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::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,65 +151,68 @@ void LoggedTopics::add_default_topics()
 #if CONSTRAINED_MEMORY
 	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1;
 #else
-	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed
+	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4; // artificially limited until PlotJuggler fixed
 	add_optional_topic("estimator_selector_status");
-	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);
+	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);
 #endif
 
 	// always add the first instance
-	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);
+#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_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_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_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_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_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);
@@ -338,22 +341,25 @@ 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("vehicle_optical_flow");
+	add_topic("airspeed_validated");
+	add_topic("landing_target_pose");
+	add_topic("parameter_update");
+	add_topic("sensor_airflow");
 	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,7 +690,8 @@ 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();
+		//_lockstep_component = px4_lockstep_register_component();
+		//PX4_ERR("Lockstep component: %i", _lockstep_component);
 	}
 
 	bool was_started = false;
@@ -899,7 +900,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;
@@ -928,7 +929,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);
@@ -1472,8 +1473,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
-		${PX4_BOARD_DIR}/libfc-sensor-api/inc
+		libfc-sensor-api/inc
 	SRCS
 		uORBAppsProtobufChannel.cpp
 		muorb_main.cpp

src/modules/muorb/apps/Kconfig

@@ -4,11 +4,3 @@ 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,21 +180,17 @@ mixer_tick()
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
-		// Set failsafe value if the PWM output isn't disabled
+		/* copy failsafe values to the servo outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			if (r_page_servos[i] != 0) {
 			r_page_servos[i] = r_page_servo_failsafe[i];
 		}
-		}
 
 	} else if (source == MIX_DISARMED) {
-		// Set disarmed value if the PWM output isn't disabled
+		/* copy disarmed values to the servo outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			if (r_page_servos[i] != 0) {
 			r_page_servos[i] = r_page_servo_disarmed[i];
 		}
 	}
-	}
 
 	/* set arming */
 	bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);

src/modules/replay/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016-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
@@ -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-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-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
@@ -49,6 +49,10 @@
 #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>
@@ -63,7 +67,6 @@
 #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"
@@ -76,16 +79,6 @@ namespace px4
 
 char *Replay::_replay_file = nullptr;
 
-Replay::CompatSensorCombinedDtType::CompatSensorCombinedDtType(int gyro_integral_dt_offset_log,
-		int gyro_integral_dt_offset_intern, int accelerometer_integral_dt_offset_log,
-		int accelerometer_integral_dt_offset_intern)
-	: _gyro_integral_dt_offset_log(gyro_integral_dt_offset_log),
-	  _gyro_integral_dt_offset_intern(gyro_integral_dt_offset_intern),
-	  _accelerometer_integral_dt_offset_log(accelerometer_integral_dt_offset_log),
-	  _accelerometer_integral_dt_offset_intern(accelerometer_integral_dt_offset_intern)
-{
-}
-
 Replay::~Replay()
 {
 	for (size_t i = 0; i < _subscriptions.size(); ++i) {
@@ -95,29 +88,7 @@ Replay::~Replay()
 	_subscriptions.clear();
 }
 
-void *
-Replay::CompatSensorCombinedDtType::apply(void *data)
-{
-	// the types have the same size so we can do the conversion in-place
-	uint8_t *ptr = (uint8_t *)data;
-
-	float gyro_integral_dt;
-	memcpy(&gyro_integral_dt, ptr + _gyro_integral_dt_offset_log, sizeof(float));
-
-	float accel_integral_dt;
-	memcpy(&accel_integral_dt, ptr + _accelerometer_integral_dt_offset_log, sizeof(float));
-
-	uint32_t igyro_integral_dt = (uint32_t)(gyro_integral_dt * 1e6f);
-	memcpy(ptr + _gyro_integral_dt_offset_intern, &igyro_integral_dt, sizeof(float));
-
-	uint32_t iaccel_integral_dt = (uint32_t)(accel_integral_dt * 1e6f);
-	memcpy(ptr + _accelerometer_integral_dt_offset_intern, &iaccel_integral_dt, sizeof(float));
-
-	return data;
-}
-
-void
-Replay::setupReplayFile(const char *file_name)
+void Replay::setupReplayFile(const char *file_name)
 {
 	if (_replay_file) {
 		free(_replay_file);
@@ -126,8 +97,7 @@ 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);
@@ -151,7 +121,7 @@ Replay::setParameter(const string &parameter_name, const double parameter_value)
 		float value = (float)parameter_value;
 
 		if (fabsf(orig_value - value) > FLT_EPSILON) {
-			PX4_WARN("Setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value);
+			PX4_WARN("Setting %s (FLOAT) %.6f -> %.6f", param_name(handle), (double)orig_value, (double)value);
 		}
 
 		param_set(handle, (const void *)&value);
@@ -391,7 +361,6 @@ Replay::readFormat(std::ifstream &file, uint16_t msg_size)
 	return true;
 }
 
-
 string Replay::getOrbFields(const orb_metadata *meta)
 {
 	char format[3000];
@@ -440,6 +409,53 @@ 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) {
@@ -447,7 +463,6 @@ 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
@@ -456,34 +471,9 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 	const string orb_fields = getOrbFields(orb_meta);
 
 	if (file_format != orb_fields) {
-		// 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;") {
+		PX4_ERR("Formats for %s (msg_id %d) don't match. Will ignore it.", topic_name.c_str(), msg_id);
 
-				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());
+		if (false) {
 			PX4_WARN(" Internal format:");
 			size_t start = 0;
 
@@ -519,15 +509,14 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 					start = i + 1;
 				}
 			}
+		}
 
 		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;
@@ -561,7 +550,32 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 		return true;
 	}
 
-	PX4_DEBUG("adding subscription for %s (msg_id %i)", subscription->orb_meta->o_name, msg_id);
+
+
+
+
+
+	{
+		// 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);
 
 	//add subscription
 	if (_subscriptions.size() <= msg_id) {
@@ -773,6 +787,12 @@ 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();
@@ -885,6 +905,15 @@ Replay::readDefinitionsAndApplyParams(std::ifstream &file)
 void
 Replay::run()
 {
+	uint64_t timestamp_last = 0;
+
+	// time starts at 0
+	{
+		struct timespec ts {};
+		abstime_to_ts(&ts, timestamp_last);
+		px4_clock_settime(CLOCK_MONOTONIC, &ts);
+	}
+
 	ifstream replay_file(_replay_file, ios::in | ios::binary);
 
 	if (!readDefinitionsAndApplyParams(replay_file)) {
@@ -898,10 +927,6 @@ Replay::run()
 		_speed_factor = atof(speedup);
 	}
 
-	onEnterMainLoop();
-
-	_replay_start_time = hrt_absolute_time();
-
 	PX4_INFO("Replay in progress...");
 
 	ulog_message_header_s message_header;
@@ -910,19 +935,36 @@ 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;
 
-	while (!should_exit() && replay_file) {
+	bool wait_on_imu_pub[4] {};
+	bool imu_published = false;
+	uint8_t imu_id = 0;
 
-		//Find the next message to publish. Messages from different subscriptions don't need
-		//to be in chronological order, so we need to check all subscriptions
+	hrt_abstime last_update = 0;
+
+	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
 		uint64_t next_file_time = 0;
 		int next_msg_id = -1;
 		bool first_time = true;
@@ -944,18 +986,24 @@ 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
+			// someone didn't set the timestamp properly. Consider the message invalid
+			PX4_ERR("%s:%d missing timestamp", sub.orb_meta->o_name, sub.multi_id);
 			nextDataMessage(replay_file, sub, next_msg_id);
 			continue;
 		}
 
-		//handle additional messages between last and next published data
+		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
 		replay_file.seekg(last_additional_message_pos);
 		streampos next_additional_message_pos = sub.next_read_pos;
 		readAndHandleAdditionalMessages(replay_file, next_additional_message_pos);
@@ -972,34 +1020,190 @@ Replay::run()
 			_next_param_change++;
 		}
 
-		const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset);
+		// Perform scheduled parameter changes
+		while (_next_param_change < _dynamic_parameter_schedule.size() &&
+		       _dynamic_parameter_schedule[_next_param_change].timestamp <= next_file_time) {
+
+			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);
+
+			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);
-		memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp
 
-		if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) {
+			if (publish_timestamp >= timestamp_last) {
+
+				// 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;
+			}
+		}
+
 		nextDataMessage(replay_file, sub, next_msg_id);
 
 		// TODO: output status (eg. every sec), including total duration...
 	}
 
-	for (auto &subscription : _subscriptions) {
-		if (!subscription) {
+	for (auto &sub : _subscriptions) {
+		if (!sub) {
 			continue;
 		}
 
-		if (subscription->compat) {
-			delete subscription->compat;
-			subscription->compat = nullptr;
+		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->orb_advert) {
-			orb_unadvertise(subscription->orb_advert);
-			subscription->orb_advert = nullptr;
+		if (sub->orb_advert) {
+			orb_unadvertise(sub->orb_advert);
+			sub->orb_advert = nullptr;
 		}
 	}
 
@@ -1008,8 +1212,6 @@ Replay::run()
 			 (double)hrt_elapsed_time(&_replay_start_time) / 1.e6);
 	}
 
-	onExitMainLoop();
-
 	if (!should_exit()) {
 		replay_file.close();
 		px4_shutdown_request();
@@ -1035,50 +1237,11 @@ Replay::readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_fil
 	replay_file.read((char *)_read_buffer.data(), msg_read_size);
 }
 
-bool
-Replay::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
-{
-	return publishTopic(sub, data);
-}
-
-uint64_t
-Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset)
-{
-	const uint64_t publish_timestamp = next_file_time + timestamp_offset;
-
-	// wait if necessary
-	uint64_t cur_time = hrt_absolute_time();
-
-	// if some topics have a timestamp smaller than the log file start, publish them immediately
-	if (cur_time < publish_timestamp && next_file_time > _file_start_time) {
-		if (_speed_factor > FLT_EPSILON) {
-			// avoid many small usleep calls
-			_accumulated_delay += (publish_timestamp - cur_time) / _speed_factor;
-
-			if (_accumulated_delay > 3000) {
-				system_usleep(_accumulated_delay);
-				_accumulated_delay = 0.f;
-			}
-		}
-
-		// adjust the lockstep time to the publication time
-		struct timespec ts;
-		abstime_to_ts(&ts, publish_timestamp);
-		px4_clock_settime(CLOCK_MONOTONIC, &ts);
-	}
-
-	return publish_timestamp;
-}
-
 bool
 Replay::publishTopic(Subscription &sub, void *data)
 {
 	bool published = false;
 
-	if (sub.compat) {
-		data = sub.compat->apply(data);
-	}
-
 	if (sub.orb_advert) {
 		orb_publish(sub.orb_meta, sub.orb_advert, data);
 		published = true;
@@ -1120,6 +1283,123 @@ 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[])
 {
@@ -1131,6 +1411,8 @@ Replay::custom_command(int argc, char *argv[])
 		return Replay::task_spawn(argc, argv);
 	}
 
+	// ignore?
+
 	return print_usage("unknown command");
 }
 
@@ -1197,17 +1479,9 @@ Replay *
 Replay::instantiate(int argc, char *argv[])
 {
 	// check the replay mode
-	const char *replay_mode = getenv(replay::ENV_MODE);
+	//const char *replay_mode = getenv(replay::ENV_MODE);
 
-	Replay *instance = nullptr;
-
-	if (replay_mode && strcmp(replay_mode, "ekf2") == 0) {
-		PX4_INFO("Ekf2 replay mode");
-		instance = new ReplayEkf2();
-
-	} else {
-		instance = new Replay();
-	}
+	Replay *instance = new Replay();
 
 	return instance;
 }
@@ -1224,7 +1498,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`, which must be set to an ULog file name - it's
+There are 2 environment variables used for configuration: `replay_file`, 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-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2024 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,7 +44,6 @@
 
 #include <px4_platform_common/module.h>
 #include <uORB/topics/uORBTopics.hpp>
-#include <uORB/topics/ekf2_timestamps.h>
 
 namespace px4
 {
@@ -96,39 +95,10 @@ 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
@@ -138,8 +108,6 @@ protected:
 		std::streampos next_read_pos;
 		uint64_t next_timestamp; ///< timestamp of the file
 
-		CompatBase *compat = nullptr;
-
 		// statistics
 		int error_counter = 0;
 		int publication_counter = 0;
@@ -163,34 +131,16 @@ protected:
 	 */
 	bool publishTopic(Subscription &sub, void *data);
 
-	/**
-	 * called when entering the main replay loop
-	 */
-	virtual void onEnterMainLoop() {}
-
-	/**
-	 * called when exiting the main replay loop
-	 */
-	virtual void onExitMainLoop() {}
-
 	/**
 	 * called when a new subscription is added
 	 */
-	virtual void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {}
-
-	/**
-	 * handle delay until topic can be published.
-	 * @param next_file_timestamp timestamp of next message to publish
-	 * @param timestamp_offset offset between file start time and replay start time
-	 * @return timestamp that the message to publish should have
-	 */
-	virtual uint64_t handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset);
+	void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {}
 
 	/**
 	 * handle the publication of a topic update
 	 * @return true if published, false otherwise
 	 */
-	virtual bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
+	bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
 
 	/**
 	 * read a topic from the file (offset given by the subscription) into _read_buffer
@@ -207,7 +157,7 @@ protected:
 	 */
 	bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
 
-	virtual uint64_t getTimestampOffset()
+	uint64_t getTimestampOffset()
 	{
 		//we update the timestamps from the file by a constant offset to match
 		//the current replay time
@@ -300,6 +250,43 @@ 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

@@ -1,225 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/posix.h>
-
-#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

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

src/systemcmds/uorb/uorb.cpp

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