WIP: multi-EKF replay hacks and replay overhaul

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ROMFS/px4fmu_common/init.d-posix/rcS

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