WIP: replay overhaul (no more special ekf2 replay)

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