HACK to enable actuators in HITL

Mixers get loaded on normal output modules the first actuator_outputs instance gets sent to the simulator. No lockdown but the rest of HIL stays.
disable uart´s dma
2026-06-20 06:40:35 +08:00 · 2022-06-13 10:19:08 +02:00 · 2022-06-11 13:39:01 -04:00 · 2022-06-10 14:02:41 +02:00 · 2022-06-10 04:20:26 -07:00 · 2022-06-10 01:01:20 -04:00
96 changed files with 6615 additions and 6549 deletions
@@ -0,0 +1,101 @@
+#!/bin/sh
+#
+# @name 6DoF Omnicopter SITL
+#
+# @type Quadrotor Wide
+#
+# @maintainer Jaeyoung Lim <jalim@ethz.ch>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default SYS_CTRL_ALLOC 1
+
+
+param set-default CA_AIRFRAME 0
+
+param set-default CA_ROTOR_COUNT 8
+param set-default CA_R_REV 255
+
+param set-default CA_ROTOR0_PX 0.14435
+param set-default CA_ROTOR0_PY -0.14435
+param set-default CA_ROTOR0_PZ -0.14435
+param set-default CA_ROTOR0_KM 0.05 # CCW
+param set-default CA_ROTOR0_AX -0.788675
+param set-default CA_ROTOR0_AY -0.211325
+param set-default CA_ROTOR0_AZ -0.57735
+
+param set-default CA_ROTOR1_PX -0.14435
+param set-default CA_ROTOR1_PY -0.14435
+param set-default CA_ROTOR1_PZ -0.14435
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_AX 0.211325
+param set-default CA_ROTOR1_AY -0.788675
+param set-default CA_ROTOR1_AZ 0.57735
+
+param set-default CA_ROTOR2_PX 0.14435
+param set-default CA_ROTOR2_PY 0.14435
+param set-default CA_ROTOR2_PZ -0.14435
+param set-default CA_ROTOR2_KM 0.05
+param set-default CA_ROTOR2_AX -0.211325
+param set-default CA_ROTOR2_AY 0.788675
+param set-default CA_ROTOR2_AZ 0.57735
+
+param set-default CA_ROTOR3_PX -0.14435
+param set-default CA_ROTOR3_PY 0.14435
+param set-default CA_ROTOR3_PZ -0.14435
+param set-default CA_ROTOR3_KM 0.05
+param set-default CA_ROTOR3_AX 0.788675
+param set-default CA_ROTOR3_AY 0.211325
+param set-default CA_ROTOR3_AZ -0.57735
+
+param set-default CA_ROTOR4_PX 0.14435
+param set-default CA_ROTOR4_PY -0.14435
+param set-default CA_ROTOR4_PZ 0.14435
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 0.788675
+param set-default CA_ROTOR4_AY 0.211325
+param set-default CA_ROTOR4_AZ -0.57735
+
+param set-default CA_ROTOR5_PX -0.14435
+param set-default CA_ROTOR5_PY -0.14435
+param set-default CA_ROTOR5_PZ 0.14435
+param set-default CA_ROTOR5_KM 0.05
+param set-default CA_ROTOR5_AX -0.211325
+param set-default CA_ROTOR5_AY 0.788675
+param set-default CA_ROTOR5_AZ 0.57735
+
+param set-default CA_ROTOR6_PX 0.14435
+param set-default CA_ROTOR6_PY 0.14435
+param set-default CA_ROTOR6_PZ 0.14435
+param set-default CA_ROTOR6_KM 0.05
+param set-default CA_ROTOR6_AX 0.211325
+param set-default CA_ROTOR6_AY -0.788675
+param set-default CA_ROTOR6_AZ 0.57735
+
+param set-default CA_ROTOR7_PX -0.14435
+param set-default CA_ROTOR7_PY 0.14435
+param set-default CA_ROTOR7_PZ 0.14435
+param set-default CA_ROTOR7_KM 0.05
+param set-default CA_ROTOR7_AX -0.788675
+param set-default CA_ROTOR7_AY -0.211325
+param set-default CA_ROTOR7_AZ -0.57735
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 105
+param set-default PWM_MAIN_FUNC6 106
+param set-default PWM_MAIN_FUNC7 107
+param set-default PWM_MAIN_FUNC8 108
+
+# disable MC desaturation which improves attitude tracking
+param set-default CA_METHOD 0
+
+# disable attitude failure detection
+param set-default FD_FAIL_P 0
+param set-default FD_FAIL_R 0
+
+set MIXER skip
+set MIXER_AUX none
@@ -0,0 +1,23 @@
+#!/bin/sh
+#
+# @name QuadrotorX SITL for SIH
+#
+# @type Quadrotor
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+set MIXER quad_x
+
+# disable some checks to allow to fly:
+# - with usb
+param set-default CBRK_USB_CHK 197848
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default COM_PREARM_MODE 0
+param set-default CBRK_IO_SAFETY 22027
+
+param set SIH_VEHICLE_TYPE 0
@@ -0,0 +1,33 @@
+#!/bin/sh
+#
+# @name Plane SITL for SIH
+#
+# @type Plane
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+
+. ${R}etc/init.d/rc.fw_defaults
+
+set MIXER AERT
+
+# disable some checks to allow to fly:
+# - with usb
+param set-default CBRK_USB_CHK 197848
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default COM_PREARM_MODE 0
+param set-default CBRK_IO_SAFETY 22027
+
+param set-default BAT_N_CELLS 3
+
+param set SIH_T_MAX 6.0
+param set SIH_MASS 0.3
+param set SIH_IXX 0.00402
+param set SIH_IYY 0.0144
+param set SIH_IZZ 0.0177
+param set SIH_IXZ 0.00046
+param set SIH_KDV 0.2
+
+param set SIH_VEHICLE_TYPE 1 	# sih as fixed wing
+param set RWTO_TKOFF 1  # enable takeoff from runway (as opposed to launched)
@@ -0,0 +1,45 @@
+#!/bin/sh
+#
+# @name SIH Tailsitter Duo
+#
+# @type VTOL
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_TYPE 0
+param set-default VT_FW_DIFTHR_EN 1
+param set-default VT_FW_DIFTHR_SC 0.3
+param set-default MPC_MAN_Y_MAX 60
+param set-default MC_PITCH_P 5
+
+param set-default MAV_TYPE 19
+set MAV_TYPE 19
+set MIXER vtol_tailsitter_duo_sat
+
+# disable some checks to allow to fly:
+# - with usb
+param set-default CBRK_USB_CHK 197848
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default COM_PREARM_MODE 0
+param set-default CBRK_IO_SAFETY 22027
+
+param set-default BAT_N_CELLS 3
+
+param set SIH_T_MAX 2.0
+param set SIH_Q_MAX 0.0165
+param set SIH_MASS 0.2
+# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
+param set SIH_IXX 0.00354
+param set SIH_IYY 0.000625
+param set SIH_IZZ 0.00300
+param set SIH_IXZ 0.0
+param set SIH_KDV 0.2
+param set SIH_L_ROLL 0.145
+
+# sih as tailsitter
+param set SIH_VEHICLE_TYPE 2
@@ -35,8 +35,12 @@ px4_add_romfs_files(
 	10016_iris
 	10017_iris_ctrlalloc
 	10018_iris_foggy_lidar
+	10019_omnicopter
 	10020_if750a
 	10030_px4vision
+	10040_quadx
+	10041_airplane
+	10042_xvert
 	1010_iris_opt_flow
 	1010_iris_opt_flow.post
 	1011_iris_irlock
@@ -30,3 +30,12 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud

 # Onboard link to gimbal
 mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote
+
+# To display for SIH sitl
+if [ "$SIM_MODE" = "sihsim" ]; then
+	udp_sihsim_port_local=$((19450+px4_instance))
+	udp_sihsim_port_remote=$((19410+px4_instance))
+	mavlink start -x -u $udp_sihsim_port_local -r 400000 -m custom -o $udp_sihsim_port_remote
+	mavlink stream -r 200 -s HIL_ACTUATOR_CONTROLS -u  $udp_sihsim_port_local
+	mavlink stream -r 25 -s HIL_STATE_QUATERNION -u  $udp_sihsim_port_local
+fi
@@ -154,6 +154,8 @@ param set-default SDLOG_DIRS_MAX 7

 param set-default TRIG_INTERFACE 3

+param set-default SYS_FAILURE_EN 1
+
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
 	COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
@@ -202,8 +204,11 @@ param set IMU_INTEG_RATE 250
 . px4-rc.params

 dataman start
+# start sih in sih_sim mode, otherwise simulator module
+if [ "$SIM_MODE" = "sihsim" ]; then
+	sih start
 # only start the simulator if not in replay mode, as both control the lockstep time
-if ! replay tryapplyparams
+elif ! replay tryapplyparams
 then
  . px4-rc.simulator
 fi
@@ -267,5 +272,3 @@ fi

 mavlink boot_complete
 replay trystart
-
-uorb_explained start
@@ -188,16 +188,7 @@ then

 	if [ $MIXER_AUX_FILE != none ]
 	then
-		# Append aux mixer to main device.
-		if param greater SYS_HITL 0
-		then
-			if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE}
-			then
-				echo "INFO  [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}"
-			else
-				echo "ERROR  [init] Failed appending mixer: ${MIXER_AUX_FILE}"
-			fi
-		fi
+		# Enable actuators in HITL
 		if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ]
 		then
 			if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
@@ -5,12 +5,13 @@ set -e
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 cd "$SCRIPT_DIR/jMAVSim"

-tcp_port=4560
+port=4560
 extra_args=
 baudrate=921600
 device=
 ip="127.0.0.1"
-while getopts ":b:d:p:qsr:f:i:loat" opt; do
+protocol="tcp"
+while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
 	case $opt in
 		b)
 			baudrate=$OPTARG
@@ -18,11 +19,14 @@ while getopts ":b:d:p:qsr:f:i:loat" opt; do
 		d)
 			device="$OPTARG"
 			;;
+		u)
+			protocol="udp"
+			;;
 		i)
 			ip="$OPTARG"
 			;;
 		p)
-			tcp_port=$OPTARG
+			port=$OPTARG
 			;;
 		q)
 			extra_args="$extra_args -qgc"
@@ -53,7 +57,11 @@ while getopts ":b:d:p:qsr:f:i:loat" opt; do
 done

 if [ "$device" == "" ]; then
-	device="-tcp $ip:$tcp_port"
+	if [ "$protocol" == "tcp" ]; then
+		device="-tcp $ip:$port"
+	else
+		device="-udp $port"
+	fi
 else
 	device="-serial $device $baudrate"
 fi
@@ -74,6 +74,9 @@ if [ "$model" == "" ] || [ "$model" == "none" ]; then
 	if [ "$program" == "jsbsim" ]; then
 		echo "empty model, setting rascal as default for jsbsim"
 		model="rascal"
+	elif [ "$program" == "sihsim" ]; then
+		echo "empty model, setting quadx as default for sihsim"
+		model="quadx"
 	else
 		echo "empty model, setting iris as default"
 		model="iris"
@@ -214,6 +217,12 @@ elif [ "$program" == "jsbsim" ] && [ -z "$no_sim" ]; then
 	fi
 	"${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -s "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml" 2> /dev/null &
 	JSBSIM_PID=$!
+elif [ "$program" == "sihsim" ] && [ ! -n "$no_sim" ]; then
+	export SIM_MODE="sihsim"
+	if [ "$model" != "airplane" ] && [ "$model" != "quadx" ] && [ "$model" != "xvert" ]; then
+		echo "Model ${model} not compatible with with sih. sih supports [quadx,airplane,xvert]."
+		exit 1
+	fi
 fi

 pushd "$rootfs" >/dev/null
@@ -86,9 +86,9 @@
 #define DIRECT_PWM_OUTPUT_CHANNELS  8

 /* Power supply control and monitoring GPIOs */
-#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_POWER_IN_A                 /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)

-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_VDD_BRICK1_VALID           GPIO_POWER_IN_A /* Brick 1 Is Chosen */
 #define BOARD_NUMBER_BRICKS             1

 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
@@ -143,7 +143,7 @@
 */
 #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
 #define BOARD_ADC_USB_VALID     BOARD_ADC_USB_CONNECTED
-#define BOARD_ADC_BRICK_VALID   (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK_VALID   (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID))

 #define BOARD_NUM_IO_TIMERS 3
 #define BOARD_DMA_ALLOC_POOL_SIZE 5120 /* This board provides a DMA pool and APIs */
@@ -160,7 +160,7 @@
 		GPIO_CAN2_SILENT_S0,              \
 		GPIO_LEVEL_SHIFTER_OE,            \
 		GPIO_PWM_VOLT_SEL,                \
-		GPIO_nPOWER_IN_A,                 \
+		GPIO_POWER_IN_A,                  \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D0), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D1), \
@@ -198,9 +198,9 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
-CONFIG_UART4_RXBUFSIZE=600
-CONFIG_UART4_RXDMA=y
-CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART4_RXBUFSIZE=1200
+CONFIG_UART4_RXDMA=n
+CONFIG_UART4_TXBUFSIZE=1200
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_RXBUFSIZE=600
 CONFIG_UART7_SERIAL_CONSOLE=y
@@ -225,10 +225,10 @@ CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_RXDMA=y
 CONFIG_USART3_TXBUFSIZE=3000
 CONFIG_USART6_BAUD=57600
-CONFIG_USART6_RXBUFSIZE=600
-CONFIG_USART6_RXDMA=y
-CONFIG_USART6_TXBUFSIZE=1500
-CONFIG_USART6_TXDMA=y
+CONFIG_USART6_RXBUFSIZE=1200
+CONFIG_USART6_RXDMA=n
+CONFIG_USART6_TXBUFSIZE=1200
+CONFIG_USART6_TXDMA=n
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -47,7 +47,7 @@ then
 	fi
 fi

-if ver hwtypecmp V6X04 V6X14
+if ver hwtypecmp V6X04 V6X14 V6X54
 then
 	# Internal SPI bus ICM20649
 	icm20649 -s -R 6 start
@@ -60,7 +60,7 @@ fi
 # Internal SPI bus ICM42688p
 icm42688p -R 6 -s start

-if ver hwtypecmp V6X03 V6X13 V6X04 V6X14
+if ver hwtypecmp V6X03 V6X13 V6X04 V6X14 V6X53 V6X54
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
 	icm42670p -R 10 -s start
@@ -39,6 +39,7 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIH=y
 CONFIG_MODULES_SIMULATOR=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
@@ -124,7 +124,6 @@ set(msg_files
 	optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
-	pasta_information.msg
 	ping.msg
 	pps_capture.msg
 	position_controller_landing_status.msg
@@ -1,33 +0,0 @@
-# Message with information for a dish of pasta
-uint64 timestamp			# [us] time when the topic was published
-
-uint16 customer_table_id		# customer’s table ID to know where to serve
-
-uint8 menu_name				# menu, e.g. Carbonara, Amatriciana
-
-uint8 PASTA_MENU_UNDEFINED = 0		# Undefined: Default value if no value is set
-uint8 PASTA_MENU_CARBONARA = 1		# Carbonara: With Egg, Pecorino and Guanciale!
-uint8 PASTA_MENU_AMATRICIANA = 2	# Amatriciana: With Tomato, Pecorino and Guanciale!
-uint8 PASTA_MENU_AGLIO_E_OLIO = 3	# Aglio E Olio: With Olive oil and Garlic!
-uint8 PASTA_MENU_BOLOGNESE = 4		# Bolognese: With Beef and Tomato!
-
-uint8 cooked_texture			# how cooked the pasta should be, e.g. Al Dente
-
-uint8 PASTA_COOKED_UNDEFINED = 0	# Undefined: Default value if no value is set
-uint8 PASTA_COOKED_AL_DENTE = 1		# Al Dente: https://en.wikipedia.org/wiki/Al_dente
-uint8 PASTA_COOKED_RAW = 2		# Barely cooked
-
-uint8 pasta_type			# type of pasta, e.g. Spaghetti, Lasagne, Rigatoni
-
-uint8 PASTA_TYPE_UNDEFINED = 0		# Undefined: Default value if no value is set
-uint8 PASTA_TYPE_SPAGHETTI = 1		# Spaghetti: Long, stringy pasta
-uint8 PASTA_TYPE_RIGATONI = 2		# Rigatoni: Cylindrical pasta perfect for Carbonara!
-uint8 PASTA_TYPE_LASAGNE = 3		# Lasagne: Flat, big pasta that gets layered on each other
-
-float32 pasta_temperature		# [deg C] temperature of the pasta
-
-# TOPICS pasta_cook pasta_order
-
-# The topic pub/sub flow would be as follows:
-# Customer -> 'pasta_order' -> Waiter
-# Waiter -> 'pasta_cook' -> Chef
@@ -19,7 +19,7 @@ bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
-bool status_safety_off
+bool status_safety_button_event # px4io safety button was pressed for longer than 1 second

 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
 bool alarm_pwm_error
@@ -37,8 +37,8 @@ uint16 VEHICLE_CMD_DO_SET_SERVO = 183			# Set a servo to a desired PWM value. |S
 uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184		# Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185		# Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187		# Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index|
-uint16 VEHICLE_CMD_DO_LAND_START = 189			# Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty|  */
-uint16 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+uint16 VEHICLE_CMD_DO_LAND_START = 189			# Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty|
+uint16 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_REPOSITION = 192
 uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193
 uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195	# Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude|
@@ -117,5 +117,5 @@ uint64 armed_time       # Arming timestamp (microseconds)

 uint64 takeoff_time     # Takeoff timestamp (microseconds)

-bool safety_button_available	# Set to true if a safety button is connected
-bool safety_off					# Set to true if safety is off
+bool safety_button_available # Set to true if a safety button is connected
+bool safety_off # Set to true if safety is off
@@ -3,6 +3,7 @@ uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

 float32 yaw_composite	# composite yaw from GSF (rad)
 float32 yaw_variance	# composite yaw variance from GSF (rad^2)
+bool yaw_composite_valid

 float32[5] yaw		# yaw estimate for each model in the filter bank (rad)
 float32[5] innov_vn	# North velocity innovation for each model in the filter bank (m/s)
@@ -9,7 +9,6 @@ add_custom_command(OUTPUT ${PX4_BINARY_DIR}/logs
 )
 add_custom_target(logs_symlink DEPENDS ${PX4_BINARY_DIR}/logs)
 add_dependencies(px4 logs_symlink)
-
 add_custom_target(run_config
 	COMMAND Tools/sitl_run.sh $<TARGET_FILE:px4> ${config_sitl_debugger} ${config_sitl_viewer} ${config_sitl_model} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
 	WORKING_DIRECTORY ${SITL_WORKING_DIR}
@@ -159,6 +158,7 @@ set(models
 	iris_rplidar
 	iris_vision
 	nxp_cupcar
+	omnicopter
 	plane
 	plane_cam
 	plane_catapult
@@ -324,6 +324,70 @@ foreach(debugger ${debuggers})
 	endforeach()
 endforeach()

+# create targets for sihsim
+set(models_sih
+	none
+	airplane
+	quadx
+	xvert
+)
+
+set(worlds_sih
+	none
+)
+
+foreach(debugger ${debuggers})
+	foreach(model ${models_sih})
+		foreach(world ${worlds_sih})
+			if(world STREQUAL "none")
+				if(debugger STREQUAL "none")
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim")
+					else()
+						set(_targ_name "sihsim_${model}")
+					endif()
+				else()
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim__${debugger}_${world}")
+					else()
+						set(_targ_name "sihsim_${model}_${debugger}_${world}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+					COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+					WORKING_DIRECTORY ${SITL_WORKING_DIR}
+					USES_TERMINAL
+					DEPENDS logs_symlink
+				)
+				list(APPEND all_posix_vmd_make_targets ${_targ_name})
+			else()
+				if(debugger STREQUAL "none")
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim___${world}")
+					else()
+						set(_targ_name "sihsim_${model}__${world}")
+					endif()
+				else()
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim___${debugger}_${world}")
+					else()
+						set(_targ_name "sihsim_${model}_${debugger}_${world}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+					COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+					WORKING_DIRECTORY ${SITL_WORKING_DIR}
+					USES_TERMINAL
+					DEPENDS logs_symlink
+				)
+				list(APPEND all_posix_vmd_make_targets ${_targ_name})
+			endif()
+		endforeach()
+	endforeach()
+endforeach()
+
 # add flighgear targets
 if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
 	set(models
@@ -84,7 +84,7 @@ void LockstepComponents::unregister_component(int component)

 	int components_used_bitset = _components_used_bitset;

-	if (_components_progress_bitset == components_used_bitset && components_used_bitset != 0) {
+	if (_components_progress_bitset == components_used_bitset) {
 		_components_progress_bitset = 0;
 		px4_sem_post(&_components_sem);
 	}
@@ -188,12 +188,6 @@ typedef uint16_t	servo_position_t;
 /** get the maximum PWM value the output will send */
 #define PWM_SERVO_GET_MAX_PWM	_PX4_IOC(_PWM_SERVO_BASE, 19)

-/** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_PX4_IOC(_PWM_SERVO_BASE, 25)
-
-/** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)
-
 /*
 *
 *
@@ -752,8 +752,8 @@ int DShot::custom_command(int argc, char *argv[])
 	};

 	constexpr VerbCommand commands[] = {
-		{"reverse", DShot_cmd_spin_direction_reversed, 10},
-		{"normal", DShot_cmd_spin_direction_normal, 10},
+		{"reverse", DShot_cmd_spin_direction_2, 10},
+		{"normal", DShot_cmd_spin_direction_1, 10},
 		{"save", DShot_cmd_save_settings, 10},
 		{"3d_on", DShot_cmd_3d_mode_on, 10},
 		{"3d_off", DShot_cmd_3d_mode_off, 10},
@@ -492,9 +492,7 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 		break;

 	case PWM_SERVO_SET_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 	case PWM_SERVO_ARM:
 	case PWM_SERVO_DISARM:
 		break;
@@ -717,8 +717,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)

 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 		break;

 	case PWM_SERVO_DISARM:
@@ -74,6 +74,7 @@
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/px4io_status.h>
 #include <uORB/topics/parameter_update.h>

@@ -196,7 +197,8 @@ private:
 	uint16_t		_last_written_arming_c{0};	///< the last written arming state reg

 	uORB::Subscription	_t_actuator_armed{ORB_ID(actuator_armed)};		///< system armed control topic
-	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};		///< vehicle command topic
+	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};	///< vehicle command topic
+	uORB::Subscription	_t_vehicle_status{ORB_ID(vehicle_status)};		///< vehicle status topic

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

@@ -443,15 +445,14 @@ int PX4IO::init()
 		// the startup script to be able to load a new IO
 		// firmware

-		// If IO has already safety off it won't accept going into bootloader mode,
-		// therefore we need to set safety on first.
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-
 		// Now the reboot into bootloader mode should succeed.
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, PX4IO_REBOOT_BL_MAGIC);
 		return -1;
 	}

+	/* Set safety_off to false when FMU boot*/
+	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, 0);
+
 	if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) {
 		_max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS;
 	}
@@ -476,14 +477,6 @@ int PX4IO::init()
 		return ret;
 	}

-	/* set safety to off if circuit breaker enabled */
-	if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
-
-	} else {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-	}
-
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
 		_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
@@ -621,11 +614,6 @@ void PX4IO::Run()

 			update_params();

-			/* Check if the IO safety circuit breaker has been updated */
-			bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
-			/* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled);
-
 			/* Check if the flight termination circuit breaker has been updated */
 			bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
 			/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
@@ -974,15 +962,12 @@ int PX4IO::io_handle_status(uint16_t status)
 	 */

 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
-	    && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+	if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
-				    PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);

 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;

 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {

@@ -999,18 +984,26 @@ int PX4IO::io_handle_status(uint16_t status)
 	}

 	/**
-	 * Get and handle the safety status
+	 * Get and handle the safety button status
 	 */
-	const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+	const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;

-	/* px4io board will change safety_off from false to true and stay like that until the vehicle is rebooted
-	 * where safety will change back to false. Here we are triggering the safety button event once.
-	 * TODO: change px4io firmware to act on the event. This will enable the "force safety on disarming" feature. */
-	if (_previous_safety_off != safety_off) {
+	if (safety_button_pressed) {
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
 		_button_publisher.safetyButtonTriggerEvent();
 	}

-	_previous_safety_off = safety_off;
+	/**
+	 * Inform PX4IO board about safety_off state for LED control
+	 */
+	vehicle_status_s vehicle_status;
+
+	if (_t_vehicle_status.update(&vehicle_status)) {
+		if (_previous_safety_off != vehicle_status.safety_off) {
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
+			_previous_safety_off = vehicle_status.safety_off;
+		}
+	}

 	return ret;
 }
@@ -1033,37 +1026,17 @@ int PX4IO::dsm_bind_ioctl(int dsmMode)
 		return -EINVAL;
 	}

-	// Check if safety was off
-	bool safety_off = (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-	int ret = -1;
-
-	// Put safety on
-	if (safety_off) {
-		// re-enable safety
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, 0);
-
-		// set new status
-		_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-	}
-
 	PX4_INFO("Binding DSM%s RX", (dsmMode == DSM2_BIND_PULSES) ? "2" : ((dsmMode == DSMX_BIND_PULSES) ? "-X" : "-X8"));

-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+	int ret = OK;
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
 	px4_usleep(500000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
 	px4_usleep(72000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
 	px4_usleep(50000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
-	ret = OK;
-
-	// Put safety back off
-	if (safety_off) {
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
-				    PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-		_status |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
-	}
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);

 	if (ret != OK) {
 		PX4_INFO("Binding DSM failed");
@@ -1133,7 +1106,7 @@ int PX4IO::io_get_status()
 		status.status_arm_sync        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC;
 		status.status_init_ok         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK;
 		status.status_failsafe        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE;
-		status.status_safety_off      = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+		status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;

 		// PX4IO_P_STATUS_ALARMS
 		status.alarm_rc_lost       = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
@@ -1644,18 +1617,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;

-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-		PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_OFF");
-		/* force safety swith off */
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
-		break;
-
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
-		PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_ON");
-		/* force safety switch on */
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-		break;
-
 	case DSM_BIND_START:
 		/* bind a DSM receiver */
 		ret = dsm_bind_ioctl(arg);
@@ -1725,15 +1686,10 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)

 		}

-		// re-enable safety
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-
-		if (ret != PX4_OK) {
-			PX4_WARN("IO refused to re-enable safety");
-		}
-
-		// set new status
-		_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
+		/* For Legacy PX4IO Firmware only:
+		 * If IO has already safety off it won't accept going into bootloader mode,
+		 * therefore we need to set safety on first. */
+		io_reg_set(PX4IO_PAGE_SETUP, 14, 22027);

 		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
 		usleep(1);
@@ -2058,29 +2014,6 @@ int PX4IO::custom_command(int argc, char *argv[])
 		return 1;
 	}

-
-	if (!strcmp(verb, "safety_off")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to disable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(verb, "safety_on")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to enable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
 	if (!strcmp(verb, "debug")) {
 		if (argc <= 1) {
 			PX4_ERR("usage: px4io debug LEVEL");
@@ -2162,8 +2095,6 @@ Output driver communicating with the IO co-processor.
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware");
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", true);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_off", "Turn off safety (force)");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_on", "Turn on safety (force)");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("debug", "set IO debug level");
 	PRINT_MODULE_USAGE_ARG("<debug_level>", "0=disabled, 9=max verbosity", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind");
@@ -951,8 +951,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-		// these are no-ops, as no safety switch
 		break;

 	case MIXERIOCRESET:
@@ -47,80 +47,80 @@ static constexpr int LON_DIM = 37;
 // Magnetic declination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.3835,
+// Date: 2022.4219,
 static constexpr const int16_t declination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ {  25991, 24246, 22500, 20755, 19010, 17264, 15519, 13774, 12028, 10283,  8538,  6792,  5047,  3302,  1557,  -189, -1934, -3679, -5425, -7170, -8915,-10660,-12406,-14151,-15896,-17642,-19387,-21133,-22878,-24623,-26369,-28114,-29859, 31227, 29482, 27736, 25991, },
-	/* LAT: -80 */ {  22558, 20425, 18485, 16710, 15069, 13531, 12067, 10655,  9280,  7930,  6598,  5279,  3970,  2665,  1357,    38, -1303, -2674, -4084, -5535, -7029, -8565,-10142,-11761,-13425,-15142,-16927,-18799,-20781,-22899,-25173,-27607,-30176, 30016, 27397, 24889, 22558, },
-	/* LAT: -70 */ {  14975, 13577, 12452, 11491, 10623,  9792,  8951,  8064,  7112,  6093,  5023,  3929,  2844,  1788,   766,  -244, -1281, -2385, -3585, -4884, -6264, -7693, -9140,-10581,-12007,-13423,-14851,-16336,-17958,-19870,-22386,-26179, 30734, 24153, 19624, 16847, 14975, },
-	/* LAT: -60 */ {   8410,  8167,  7888,  7615,  7364,  7112,  6805,  6373,  5759,  4941,  3940,  2829,  1708,   679,  -203,  -962, -1691, -2513, -3514, -4710, -6038, -7407, -8728, -9938,-11003,-11903,-12619,-13114,-13286,-12837,-10739, -3521,  4880,  7637,  8413,  8542,  8410, },
-	/* LAT: -50 */ {   5477,  5514,  5460,  5372,  5300,  5264,  5231,  5105,  4763,  4101,  3092,  1820,   479,  -703, -1576, -2143, -2541, -2980, -3662, -4672, -5914, -7190, -8331, -9234, -9834,-10075, -9883, -9128, -7621, -5261, -2357,   390,  2499,  3923,  4798,  5272,  5477, },
-	/* LAT: -40 */ {   3948,  4042,  4051,  4008,  3949,  3916,  3922,  3912,  3740,  3208,  2187,   747,  -818, -2134, -3001, -3455, -3641, -3701, -3868, -4433, -5406, -6482, -7368, -7909, -8019, -7644, -6757, -5374, -3662, -1959,  -499,   723,  1766,  2631,  3287,  3717,  3948, },
-	/* LAT: -30 */ {   2978,  3065,  3097,  3084,  3027,  2949,  2890,  2857,  2732,  2256,  1223,  -294, -1902, -3158, -3901, -4249, -4331, -4124, -3690, -3465, -3823, -4569, -5269, -5621, -5499, -4914, -3956, -2750, -1540,  -595,    83,   683,  1307,  1909,  2418,  2779,  2978, },
-	/* LAT: -20 */ {   2336,  2382,  2401,  2405,  2364,  2271,  2165,  2089,  1945,  1451,   398, -1089, -2568, -3633, -4169, -4288, -4088, -3533, -2673, -1873, -1592, -1944, -2594, -3064, -3103, -2746, -2111, -1295,  -518,   -28,   235,   536,   977,  1454,  1876,  2183,  2336, },
-	/* LAT: -10 */ {   1942,  1937,  1917,  1916,  1889,  1806,  1698,  1608,  1424,   875,  -184, -1560, -2835, -3668, -3939, -3715, -3142, -2362, -1516,  -760,  -293,  -323,  -791, -1298, -1512, -1424, -1111,  -612,  -114,   128,   177,   337,   707,  1144,  1536,  1823,  1942, },
-	/* LAT:   0 */ {   1729,  1696,  1642,  1635,  1625,  1558,  1455,  1342,  1089,   470,  -569, -1795, -2851, -3441, -3445, -2948, -2182, -1400,  -747,  -210,   204,   318,    41,  -376,  -632,  -688,  -590,  -328,   -38,    52,    -9,    80,   420,   862,  1281,  1600,  1729, },
-	/* LAT:  10 */ {   1594,  1602,  1561,  1579,  1606,  1559,  1439,  1254,   879,   162,  -858, -1934, -2763, -3107, -2907, -2303, -1528,  -815,  -298,    83,   409,   559,   399,    77,  -158,  -266,  -290,  -209,  -104,  -145,  -285,  -258,    43,   497,   977,  1384,  1594, },
-	/* LAT:  20 */ {   1411,  1560,  1621,  1715,  1802,  1783,  1630,  1330,   786,   -69, -1112, -2064, -2664, -2777, -2453, -1853, -1143,  -492,   -31,   280,   539,   686,   597,   354,   154,    35,   -56,  -120,  -200,  -390,  -626,  -685,  -452,    -5,   536,  1054,  1411, },
-	/* LAT:  30 */ {   1111,  1478,  1738,  1963,  2124,  2135,  1949,  1528,   802,  -226, -1344, -2224, -2642, -2583, -2187, -1609,  -954,  -335,   129,   436,   668,   815,   796,   647,   498,   373,   216,     5,  -277,  -651, -1020, -1180, -1021,  -596,   -20,   590,  1111, },
-	/* LAT:  40 */ {    755,  1341,  1836,  2229,  2478,  2523,  2308,  1777,   866,  -361, -1601, -2470, -2795, -2645, -2201, -1609,  -956,  -326,   186,   552,   823,  1022,  1117,  1107,  1031,   883,   614,   204,  -334,  -941, -1462, -1707, -1592, -1176,  -577,    95,   755, },
-	/* LAT:  50 */ {    466,  1213,  1895,  2452,  2819,  2926,  2698,  2045,   904,  -593, -2018, -2937, -3234, -3044, -2555, -1910, -1199,  -500,   117,   624,  1043,  1400,  1681,  1852,  1872,  1685,  1241,   533,  -360, -1263, -1943, -2231, -2107, -1666, -1031,  -298,   466, },
-	/* LAT:  60 */ {    273,  1126,  1932,  2627,  3132,  3345,  3132,  2321,   806, -1155, -2876, -3859, -4114, -3854, -3282, -2536, -1709,  -867,   -58,   695,  1388,  2020,  2564,  2961,  3122,  2937,  2299,  1184,  -230, -1551, -2421, -2732, -2567, -2073, -1379,  -578,   273, },
-	/* LAT:  70 */ {     42,   978,  1876,  2676,  3291,  3585,  3333,  2199,   -41, -2759, -4714, -5542, -5551, -5066, -4295, -3360, -2334, -1266,  -190,   871,  1897,  2865,  3734,  4442,  4891,  4926,  4319,  2850,   658, -1451, -2752, -3197, -3039, -2507, -1756,  -886,    42, },
-	/* LAT:  80 */ {   -650,   272,  1126,  1818,  2203,  2020,   821, -1791, -5004, -7127, -7878, -7749, -7120, -6201, -5109, -3909, -2642, -1335,    -9,  1320,  2635,  3918,  5146,  6285,  7283,  8044,  8389,  7950,  6035,  2276, -1323, -3048, -3419, -3095, -2421, -1572,  -650, },
-	/* LAT:  90 */ { -29901,-28155,-26410,-24665,-22919,-21174,-19428,-17683,-15938,-14193,-12447,-10702, -8957, -7212, -5467, -3722, -1976,  -231,  1514,  3259,  5004,  6750,  8495, 10240, 11986, 13731, 15476, 17222, 18967, 20713, 22458, 24204, 25949, 27695, 29440, 31186,-29901, },
+	/* LAT: -90 */ {  25990, 24245, 22499, 20754, 19009, 17263, 15518, 13773, 12027, 10282,  8537,  6791,  5046,  3301,  1556,  -190, -1935, -3680, -5426, -7171, -8916,-10661,-12407,-14152,-15897,-17643,-19388,-21133,-22879,-24624,-26370,-28115,-29860, 31226, 29481, 27735, 25990, },
+	/* LAT: -80 */ {  22557, 20425, 18484, 16709, 15069, 13530, 12066, 10654,  9279,  7929,  6597,  5278,  3969,  2664,  1357,    38, -1303, -2675, -4084, -5536, -7030, -8566,-10143,-11762,-13426,-15144,-16929,-18801,-20783,-22901,-25175,-27609,-30178, 30014, 27395, 24888, 22557, },
+	/* LAT: -70 */ {  14975, 13577, 12452, 11491, 10623,  9792,  8951,  8064,  7111,  6092,  5022,  3929,  2844,  1788,   766,  -244, -1281, -2386, -3586, -4885, -6264, -7694, -9141,-10583,-12009,-13424,-14852,-16337,-17960,-19872,-22389,-26183, 30730, 24152, 19624, 16847, 14975, },
+	/* LAT: -60 */ {   8411,  8167,  7888,  7616,  7364,  7112,  6805,  6373,  5759,  4940,  3940,  2828,  1708,   679,  -203,  -962, -1691, -2513, -3515, -4710, -6039, -7408, -8729, -9939,-11004,-11904,-12620,-13115,-13287,-12838,-10739, -3518,  4884,  7640,  8415,  8544,  8411, },
+	/* LAT: -50 */ {   5478,  5515,  5460,  5373,  5300,  5264,  5231,  5105,  4762,  4101,  3092,  1820,   478,  -703, -1576, -2142, -2540, -2979, -3662, -4673, -5915, -7191, -8332, -9235, -9835,-10076, -9883, -9128, -7621, -5261, -2357,   391,  2500,  3924,  4799,  5273,  5478, },
+	/* LAT: -40 */ {   3949,  4042,  4051,  4008,  3949,  3916,  3922,  3912,  3740,  3207,  2186,   746,  -819, -2135, -3001, -3454, -3639, -3700, -3867, -4433, -5407, -6483, -7369, -7910, -8019, -7644, -6756, -5373, -3662, -1959,  -498,   723,  1766,  2631,  3287,  3718,  3949, },
+	/* LAT: -30 */ {   2979,  3065,  3097,  3084,  3027,  2949,  2890,  2857,  2731,  2255,  1222,  -295, -1903, -3159, -3901, -4249, -4330, -4122, -3689, -3464, -3824, -4571, -5270, -5622, -5499, -4914, -3955, -2749, -1540,  -595,    83,   683,  1306,  1909,  2418,  2780,  2979, },
+	/* LAT: -20 */ {   2336,  2383,  2402,  2405,  2364,  2271,  2165,  2089,  1944,  1450,   397, -1090, -2570, -3633, -4169, -4288, -4087, -3532, -2671, -1872, -1592, -1945, -2594, -3065, -3103, -2745, -2111, -1294,  -518,   -28,   234,   536,   976,  1454,  1876,  2183,  2336, },
+	/* LAT: -10 */ {   1943,  1937,  1917,  1916,  1889,  1806,  1697,  1607,  1423,   874,  -185, -1562, -2836, -3668, -3939, -3714, -3141, -2361, -1515,  -759,  -292,  -323,  -791, -1298, -1512, -1423, -1110,  -611,  -114,   127,   176,   336,   707,  1144,  1537,  1824,  1943, },
+	/* LAT:   0 */ {   1730,  1697,  1642,  1635,  1625,  1558,  1454,  1342,  1089,   469,  -570, -1796, -2852, -3442, -3444, -2947, -2181, -1399,  -746,  -209,   205,   318,    41,  -376,  -632,  -688,  -590,  -328,   -38,    51,    -9,    79,   420,   862,  1281,  1601,  1730, },
+	/* LAT:  10 */ {   1594,  1602,  1561,  1579,  1606,  1558,  1438,  1253,   878,   161,  -859, -1935, -2764, -3106, -2906, -2302, -1526,  -814,  -298,    84,   410,   560,   399,    77,  -158,  -266,  -289,  -209,  -104,  -146,  -285,  -259,    42,   496,   977,  1384,  1594, },
+	/* LAT:  20 */ {   1411,  1560,  1622,  1715,  1802,  1783,  1630,  1329,   785,   -70, -1113, -2065, -2664, -2777, -2452, -1852, -1141,  -491,   -30,   281,   540,   687,   597,   354,   154,    36,   -56,  -120,  -200,  -390,  -627,  -686,  -452,    -5,   536,  1054,  1411, },
+	/* LAT:  30 */ {   1111,  1478,  1738,  1963,  2123,  2135,  1949,  1528,   801,  -227, -1345, -2224, -2642, -2583, -2186, -1608,  -952,  -334,   130,   436,   668,   816,   796,   648,   498,   373,   216,     5,  -278,  -652, -1021, -1181, -1022,  -596,   -20,   589,  1111, },
+	/* LAT:  40 */ {    754,  1341,  1835,  2228,  2478,  2522,  2308,  1776,   866,  -361, -1601, -2470, -2794, -2644, -2200, -1608,  -955,  -325,   187,   552,   824,  1023,  1118,  1108,  1031,   883,   614,   204,  -335,  -941, -1463, -1708, -1592, -1176,  -578,    95,   754, },
+	/* LAT:  50 */ {    465,  1212,  1894,  2451,  2818,  2925,  2697,  2044,   904,  -593, -2018, -2936, -3233, -3043, -2554, -1908, -1198,  -499,   118,   625,  1044,  1401,  1682,  1853,  1872,  1685,  1241,   533,  -361, -1264, -1944, -2231, -2107, -1666, -1031,  -299,   465, },
+	/* LAT:  60 */ {    271,  1124,  1930,  2625,  3130,  3344,  3131,  2320,   806, -1154, -2874, -3857, -4112, -3852, -3280, -2534, -1707,  -866,   -56,   696,  1389,  2021,  2565,  2962,  3122,  2937,  2299,  1183,  -231, -1552, -2422, -2732, -2568, -2074, -1380,  -579,   271, },
+	/* LAT:  70 */ {     39,   975,  1873,  2673,  3288,  3583,  3332,  2198,   -39, -2755, -4710, -5537, -5547, -5063, -4293, -3357, -2331, -1264,  -188,   873,  1899,  2866,  3735,  4444,  4892,  4926,  4319,  2849,   656, -1453, -2754, -3199, -3040, -2508, -1758,  -888,    39, },
+	/* LAT:  80 */ {   -657,   266,  1119,  1811,  2196,  2014,   818, -1786, -4993, -7115, -7869, -7741, -7113, -6196, -5104, -3905, -2638, -1331,    -5,  1323,  2638,  3921,  5149,  6289,  7286,  8047,  8392,  7952,  6034,  2270, -1332, -3056, -3426, -3101, -2427, -1579,  -657, },
+	/* LAT:  90 */ { -29886,-28141,-26395,-24650,-22905,-21159,-19414,-17669,-15923,-14178,-12433,-10688, -8943, -7197, -5452, -3707, -1962,  -217,  1528,  3274,  5019,  6764,  8509, 10255, 12000, 13745, 15491, 17236, 18982, 20727, 22473, 24218, 25964, 27709, 29455, 31200,-29886, },
 };

 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.3835,
+// Date: 2022.4219,
 static constexpr const int16_t inclination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ { -12573,-12573,-12573,-12573,-12573,-12573,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573, },
-	/* LAT: -80 */ { -13658,-13524,-13363,-13183,-12989,-12787,-12583,-12383,-12192,-12016,-11859,-11724,-11614,-11528,-11464,-11423,-11403,-11406,-11433,-11487,-11571,-11685,-11830,-12003,-12201,-12418,-12647,-12880,-13108,-13321,-13507,-13658,-13762,-13813,-13810,-13756,-13658, },
-	/* LAT: -70 */ { -14106,-13788,-13468,-13145,-12813,-12469,-12114,-11757,-11413,-11104,-10851,-10667,-10555,-10502,-10488,-10492,-10499,-10509,-10532,-10589,-10697,-10871,-11117,-11431,-11803,-12221,-12668,-13133,-13599,-14051,-14469,-14814,-15004,-14951,-14720,-14422,-14106, },
-	/* LAT: -60 */ { -13519,-13166,-12828,-12495,-12151,-11779,-11364,-10909,-10441,-10010, -9680, -9503, -9499, -9636, -9840,-10033,-10159,-10204,-10192,-10177,-10224,-10382,-10670,-11078,-11576,-12131,-12718,-13314,-13902,-14464,-14960,-15250,-15075,-14691,-14285,-13892,-13519, },
-	/* LAT: -50 */ { -12496,-12154,-11824,-11502,-11178,-10832,-10432, -9961, -9431, -8909, -8518, -8391, -8590, -9050, -9614,-10127,-10488,-10652,-10624,-10473,-10331,-10335,-10554,-10969,-11510,-12102,-12689,-13228,-13675,-13973,-14081,-14008,-13805,-13521,-13193,-12846,-12496, },
-	/* LAT: -40 */ { -11240,-10892,-10545,-10200, -9860, -9522, -9161, -8736, -8214, -7649, -7221, -7177, -7635, -8457, -9379,-10209,-10869,-11306,-11448,-11283,-10941,-10668,-10659,-10939,-11400,-11905,-12351,-12674,-12833,-12838,-12748,-12611,-12432,-12200,-11913,-11586,-11240, },
-	/* LAT: -30 */ {  -9602, -9224, -8845, -8455, -8063, -7687, -7330, -6939, -6426, -5814, -5361, -5441, -6207, -7408, -8659, -9755,-10670,-11382,-11783,-11777,-11407,-10899,-10564,-10563,-10813,-11135,-11392,-11506,-11443,-11268,-11093,-10959,-10812,-10605,-10324, -9979, -9602, },
-	/* LAT: -20 */ {  -7371, -6932, -6516, -6088, -5644, -5214, -4822, -4409, -3842, -3156, -2707, -2964, -4071, -5682, -7311, -8681, -9748,-10526,-10966,-11003,-10641,-10028, -9470, -9225, -9272, -9435, -9576, -9594, -9422, -9150, -8956, -8869, -8765, -8557, -8240, -7829, -7371, },
-	/* LAT: -10 */ {  -4415, -3881, -3429, -2993, -2535, -2085, -1670, -1215,  -587,   117,   481,    56, -1270, -3186, -5161, -6772, -7870, -8505, -8771, -8710, -8302, -7619, -6955, -6609, -6569, -6662, -6779, -6804, -6616, -6320, -6165, -6174, -6134, -5917, -5534, -5007, -4415, },
-	/* LAT:   0 */ {   -906,  -285,   178,   580,   998,  1415,  1805,  2250,  2836,  3414,  3618,  3124,  1831,   -78, -2122, -3773, -4784, -5214, -5274, -5104, -4664, -3949, -3244, -2873, -2812, -2882, -3007, -3080, -2951, -2715, -2661, -2804, -2866, -2680, -2255, -1626,  -906, },
-	/* LAT:  10 */ {   2562,  3185,  3618,  3958,  4313,  4680,  5032,  5420,  5873,  6245,  6282,  5795,  4714,  3150,  1460,    84,  -723,  -971,  -881,  -645,  -234,   403,  1036,  1373,  1436,  1391,  1291,  1199,  1242,  1346,  1270,  1004,   812,   885,  1237,  1838,  2562, },
-	/* LAT:  20 */ {   5416,  5943,  6321,  6616,  6929,  7272,  7616,  7964,  8299,  8500,  8413,  7952,  7117,  6022,  4897,  3988,  3458,  3339,  3488,  3731,  4061,  4526,  4986,  5241,  5298,  5280,  5231,  5174,  5163,  5145,  4971,  4639,  4342,  4253,  4425,  4847,  5416, },
-	/* LAT:  30 */ {   7569,  7941,  8258,  8540,  8849,  9196,  9554,  9896, 10173, 10288, 10144,  9723,  9095,  8384,  7724,  7212,  6920,  6878,  7020,  7230,  7477,  7776,  8064,  8238,  8295,  8307,  8309,  8300,  8278,  8196,  7974,  7622,  7267,  7047,  7031,  7225,  7569, },
-	/* LAT:  40 */ {   9266,  9486,  9742, 10027, 10354, 10715, 11083, 11423, 11676, 11758, 11609, 11250, 10777, 10304,  9908,  9624,  9475,  9470,  9580,  9741,  9916, 10100, 10272, 10397, 10475, 10535, 10590, 10623, 10607, 10497, 10255,  9906,  9539,  9254,  9111,  9123,  9266, },
-	/* LAT:  50 */ {  10802, 10923, 11124, 11394, 11717, 12072, 12426, 12743, 12966, 13025, 12886, 12589, 12224, 11879, 11603, 11417, 11323, 11320, 11385, 11488, 11602, 11717, 11834, 11949, 12067, 12189, 12303, 12375, 12366, 12242, 11996, 11669, 11330, 11045, 10855, 10775, 10802, },
-	/* LAT:  60 */ {  12319, 12392, 12543, 12761, 13031, 13332, 13635, 13901, 14077, 14105, 13969, 13720, 13431, 13160, 12939, 12782, 12691, 12660, 12675, 12723, 12790, 12875, 12981, 13113, 13272, 13447, 13612, 13721, 13728, 13608, 13384, 13105, 12826, 12589, 12419, 12328, 12319, },
-	/* LAT:  70 */ {  13758, 13801, 13896, 14039, 14218, 14422, 14631, 14812, 14917, 14897, 14760, 14558, 14340, 14136, 13962, 13827, 13733, 13678, 13659, 13672, 13714, 13785, 13888, 14024, 14191, 14378, 14562, 14703, 14750, 14678, 14517, 14319, 14124, 13959, 13839, 13771, 13758, },
-	/* LAT:  80 */ {  14998, 15011, 15048, 15108, 15185, 15270, 15349, 15394, 15376, 15297, 15184, 15060, 14937, 14823, 14723, 14641, 14579, 14538, 14520, 14523, 14550, 14599, 14670, 14763, 14874, 15001, 15136, 15268, 15375, 15420, 15382, 15295, 15200, 15116, 15052, 15012, 14998, },
+	/* LAT: -90 */ { -12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572, },
+	/* LAT: -80 */ { -13657,-13524,-13363,-13183,-12989,-12787,-12583,-12383,-12192,-12015,-11858,-11724,-11614,-11528,-11464,-11423,-11403,-11405,-11433,-11487,-11570,-11685,-11829,-12003,-12201,-12418,-12647,-12880,-13108,-13321,-13507,-13657,-13762,-13813,-13810,-13756,-13657, },
+	/* LAT: -70 */ { -14106,-13787,-13468,-13145,-12813,-12469,-12114,-11756,-11413,-11104,-10851,-10667,-10555,-10502,-10488,-10492,-10499,-10509,-10532,-10588,-10696,-10870,-11116,-11431,-11803,-12221,-12668,-13133,-13599,-14051,-14469,-14814,-15004,-14950,-14720,-14422,-14106, },
+	/* LAT: -60 */ { -13519,-13165,-12827,-12494,-12151,-11779,-11364,-10909,-10441,-10010, -9680, -9503, -9500, -9636, -9840,-10033,-10159,-10203,-10191,-10177,-10224,-10382,-10670,-11078,-11576,-12132,-12718,-13314,-13902,-14464,-14961,-15251,-15075,-14691,-14285,-13892,-13519, },
+	/* LAT: -50 */ { -12496,-12154,-11823,-11501,-11178,-10831,-10432, -9961, -9431, -8909, -8518, -8391, -8591, -9051, -9615,-10127,-10488,-10652,-10623,-10472,-10330,-10334,-10554,-10969,-11511,-12103,-12689,-13228,-13675,-13973,-14081,-14008,-13805,-13521,-13193,-12846,-12496, },
+	/* LAT: -40 */ { -11240,-10892,-10545,-10200, -9860, -9522, -9161, -8735, -8214, -7649, -7222, -7178, -7636, -8459, -9380,-10210,-10870,-11306,-11448,-11282,-10940,-10668,-10659,-10939,-11400,-11905,-12351,-12674,-12833,-12838,-12748,-12611,-12432,-12200,-11914,-11586,-11240, },
+	/* LAT: -30 */ {  -9602, -9224, -8845, -8455, -8063, -7687, -7330, -6939, -6426, -5814, -5361, -5442, -6209, -7410, -8661, -9757,-10672,-11383,-11784,-11776,-11406,-10899,-10564,-10563,-10814,-11135,-11392,-11505,-11443,-11268,-11093,-10959,-10813,-10606,-10324, -9979, -9602, },
+	/* LAT: -20 */ {  -7371, -6932, -6516, -6088, -5644, -5213, -4822, -4408, -3842, -3156, -2707, -2965, -4074, -5685, -7313, -8683, -9750,-10527,-10967,-11003,-10641,-10027, -9469, -9224, -9272, -9435, -9576, -9594, -9422, -9150, -8956, -8869, -8765, -8557, -8241, -7829, -7371, },
+	/* LAT: -10 */ {  -4415, -3881, -3428, -2992, -2535, -2084, -1670, -1215,  -587,   117,   480,    55, -1273, -3189, -5164, -6774, -7871, -8506, -8772, -8710, -8302, -7618, -6954, -6608, -6569, -6662, -6779, -6803, -6615, -6319, -6165, -6174, -6134, -5918, -5534, -5007, -4415, },
+	/* LAT:   0 */ {   -906,  -285,   179,   581,   999,  1415,  1806,  2250,  2836,  3414,  3617,  3122,  1828,   -81, -2125, -3776, -4785, -5215, -5274, -5104, -4664, -3948, -3243, -2872, -2812, -2881, -3006, -3080, -2950, -2715, -2660, -2804, -2867, -2680, -2256, -1627,  -906, },
+	/* LAT:  10 */ {   2561,  3186,  3618,  3959,  4313,  4680,  5033,  5421,  5873,  6244,  6281,  5793,  4712,  3147,  1457,    82,  -724,  -972,  -881,  -645,  -234,   404,  1037,  1374,  1437,  1392,  1292,  1200,  1243,  1346,  1270,  1003,   812,   884,  1236,  1838,  2561, },
+	/* LAT:  20 */ {   5416,  5943,  6321,  6616,  6929,  7273,  7616,  7964,  8298,  8499,  8412,  7950,  7116,  6020,  4895,  3987,  3457,  3338,  3488,  3731,  4062,  4527,  4987,  5242,  5298,  5281,  5232,  5175,  5164,  5146,  4972,  4639,  4342,  4252,  4424,  4846,  5416, },
+	/* LAT:  30 */ {   7569,  7941,  8258,  8540,  8849,  9196,  9554,  9896, 10173, 10287, 10143,  9722,  9094,  8383,  7722,  7211,  6920,  6878,  7020,  7231,  7477,  7776,  8064,  8238,  8295,  8308,  8310,  8301,  8278,  8196,  7974,  7622,  7267,  7047,  7031,  7225,  7569, },
+	/* LAT:  40 */ {   9266,  9486,  9742, 10027, 10354, 10715, 11083, 11423, 11675, 11758, 11609, 11249, 10776, 10303,  9907,  9624,  9474,  9470,  9580,  9742,  9917, 10101, 10272, 10397, 10475, 10535, 10590, 10623, 10607, 10497, 10256,  9906,  9539,  9254,  9111,  9123,  9266, },
+	/* LAT:  50 */ {  10802, 10923, 11124, 11394, 11717, 12071, 12426, 12743, 12965, 13025, 12886, 12588, 12224, 11878, 11603, 11416, 11323, 11319, 11386, 11488, 11602, 11718, 11834, 11950, 12068, 12190, 12303, 12376, 12367, 12242, 11997, 11669, 11330, 11045, 10855, 10775, 10802, },
+	/* LAT:  60 */ {  12319, 12392, 12542, 12761, 13031, 13332, 13634, 13901, 14077, 14105, 13969, 13720, 13431, 13160, 12939, 12782, 12691, 12660, 12675, 12723, 12790, 12875, 12981, 13113, 13272, 13448, 13612, 13722, 13728, 13608, 13384, 13105, 12826, 12589, 12419, 12328, 12319, },
+	/* LAT:  70 */ {  13758, 13801, 13896, 14038, 14218, 14422, 14630, 14812, 14916, 14897, 14759, 14558, 14340, 14136, 13962, 13827, 13733, 13678, 13659, 13672, 13714, 13785, 13889, 14025, 14191, 14378, 14562, 14703, 14750, 14678, 14517, 14319, 14124, 13959, 13839, 13771, 13758, },
+	/* LAT:  80 */ {  14998, 15010, 15048, 15108, 15184, 15270, 15349, 15394, 15376, 15297, 15184, 15060, 14937, 14823, 14723, 14641, 14579, 14539, 14520, 14524, 14550, 14599, 14671, 14763, 14875, 15001, 15136, 15269, 15376, 15420, 15382, 15296, 15200, 15116, 15052, 15012, 14998, },
 	/* LAT:  90 */ {  15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, },
 };

 // Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.3835,
+// Date: 2022.4219,
 static constexpr const int16_t strength_table[19][37] {
 	//    LONGITUDE:  -180, -170, -160, -150, -140, -130, -120, -110, -100,  -90,  -80,  -70,  -60,  -50,  -40,  -30,  -20,  -10,    0,   10,   20,   30,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130,  140,  150,  160,  170,  180,
 	/* LAT: -90 */ {  5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, },
-	/* LAT: -80 */ {  6059, 5995, 5916, 5825, 5722, 5610, 5492, 5370, 5248, 5128, 5014, 4909, 4815, 4735, 4672, 4627, 4603, 4600, 4622, 4668, 4738, 4832, 4946, 5078, 5222, 5371, 5521, 5664, 5795, 5909, 6002, 6071, 6115, 6135, 6131, 6105, 6059, },
-	/* LAT: -70 */ {  6303, 6170, 6020, 5856, 5677, 5485, 5280, 5065, 4846, 4631, 4428, 4246, 4088, 3957, 3853, 3776, 3729, 3716, 3744, 3820, 3948, 4128, 4356, 4624, 4919, 5226, 5529, 5813, 6063, 6267, 6418, 6514, 6556, 6548, 6499, 6415, 6303, },
-	/* LAT: -60 */ {  6188, 5997, 5795, 5587, 5368, 5134, 4877, 4599, 4306, 4017, 3750, 3522, 3342, 3208, 3109, 3035, 2985, 2965, 2990, 3080, 3249, 3500, 3826, 4212, 4634, 5068, 5489, 5873, 6198, 6447, 6610, 6689, 6691, 6628, 6515, 6364, 6188, },
-	/* LAT: -50 */ {  5845, 5616, 5384, 5153, 4921, 4676, 4406, 4103, 3774, 3442, 3140, 2900, 2738, 2647, 2598, 2564, 2530, 2504, 2509, 2581, 2754, 3043, 3439, 3909, 4416, 4922, 5397, 5816, 6155, 6397, 6536, 6577, 6534, 6424, 6263, 6065, 5845, },
-	/* LAT: -40 */ {  5394, 5149, 4904, 4664, 4430, 4193, 3939, 3655, 3341, 3015, 2716, 2493, 2375, 2349, 2369, 2390, 2395, 2383, 2371, 2399, 2529, 2803, 3220, 3733, 4280, 4804, 5271, 5660, 5952, 6140, 6229, 6232, 6162, 6032, 5851, 5633, 5394, },
-	/* LAT: -30 */ {  4879, 4638, 4400, 4166, 3939, 3721, 3502, 3271, 3015, 2740, 2482, 2299, 2229, 2253, 2320, 2391, 2457, 2507, 2530, 2542, 2609, 2806, 3167, 3654, 4185, 4683, 5102, 5419, 5622, 5722, 5749, 5722, 5643, 5511, 5331, 5115, 4879, },
-	/* LAT: -20 */ {  4321, 4109, 3901, 3696, 3500, 3317, 3149, 2986, 2808, 2610, 2419, 2285, 2244, 2286, 2375, 2486, 2614, 2743, 2832, 2868, 2893, 2988, 3226, 3609, 4059, 4486, 4835, 5071, 5176, 5184, 5154, 5107, 5025, 4897, 4731, 4534, 4321, },
-	/* LAT: -10 */ {  3790, 3630, 3477, 3331, 3195, 3075, 2972, 2880, 2784, 2669, 2547, 2448, 2401, 2425, 2510, 2639, 2795, 2954, 3078, 3141, 3156, 3182, 3304, 3555, 3881, 4202, 4468, 4631, 4667, 4615, 4547, 4483, 4395, 4270, 4121, 3958, 3790, },
-	/* LAT:   0 */ {  3412, 3320, 3236, 3163, 3108, 3070, 3044, 3026, 3002, 2954, 2875, 2781, 2700, 2667, 2708, 2811, 2943, 3079, 3194, 3270, 3301, 3323, 3397, 3553, 3762, 3975, 4155, 4262, 4268, 4201, 4113, 4021, 3909, 3777, 3643, 3520, 3412, },
-	/* LAT:  10 */ {  3283, 3252, 3232, 3228, 3253, 3300, 3356, 3410, 3446, 3436, 3367, 3253, 3126, 3030, 3003, 3043, 3124, 3222, 3323, 3408, 3472, 3534, 3621, 3739, 3874, 4012, 4132, 4202, 4205, 4144, 4034, 3891, 3729, 3570, 3435, 3338, 3283, },
-	/* LAT:  20 */ {  3400, 3403, 3429, 3483, 3576, 3697, 3826, 3944, 4025, 4037, 3964, 3822, 3655, 3515, 3438, 3425, 3460, 3532, 3628, 3725, 3816, 3914, 4025, 4136, 4245, 4356, 4458, 4524, 4535, 4477, 4340, 4138, 3911, 3702, 3539, 3438, 3400, },
-	/* LAT:  30 */ {  3723, 3730, 3785, 3885, 4028, 4200, 4375, 4532, 4640, 4667, 4594, 4439, 4249, 4084, 3978, 3931, 3934, 3985, 4071, 4169, 4266, 4372, 4488, 4606, 4725, 4851, 4972, 5058, 5085, 5028, 4872, 4632, 4359, 4104, 3905, 3777, 3723, },
-	/* LAT:  40 */ {  4222, 4221, 4287, 4411, 4578, 4766, 4949, 5106, 5211, 5236, 5168, 5019, 4832, 4657, 4528, 4452, 4426, 4447, 4508, 4587, 4673, 4770, 4884, 5018, 5169, 5330, 5481, 5591, 5631, 5577, 5422, 5184, 4911, 4651, 4441, 4296, 4222, },
-	/* LAT:  50 */ {  4832, 4825, 4881, 4992, 5139, 5298, 5448, 5569, 5643, 5652, 5588, 5461, 5298, 5134, 4996, 4898, 4843, 4831, 4855, 4905, 4974, 5064, 5183, 5333, 5509, 5693, 5859, 5977, 6022, 5978, 5848, 5652, 5427, 5211, 5031, 4902, 4832, },
-	/* LAT:  60 */ {  5392, 5380, 5409, 5472, 5558, 5653, 5740, 5806, 5839, 5830, 5775, 5681, 5560, 5431, 5312, 5216, 5149, 5115, 5112, 5140, 5196, 5282, 5400, 5547, 5713, 5882, 6029, 6134, 6178, 6156, 6074, 5947, 5801, 5658, 5536, 5445, 5392, },
-	/* LAT:  70 */ {  5726, 5706, 5704, 5716, 5739, 5765, 5788, 5803, 5802, 5783, 5744, 5686, 5615, 5539, 5465, 5401, 5353, 5325, 5320, 5340, 5384, 5454, 5545, 5654, 5771, 5885, 5985, 6058, 6097, 6101, 6072, 6018, 5951, 5881, 5816, 5763, 5726, },
-	/* LAT:  80 */ {  5789, 5772, 5758, 5746, 5736, 5726, 5716, 5705, 5690, 5671, 5649, 5624, 5597, 5569, 5544, 5524, 5510, 5505, 5510, 5525, 5550, 5586, 5628, 5676, 5726, 5774, 5817, 5851, 5876, 5889, 5891, 5884, 5870, 5851, 5830, 5809, 5789, },
+	/* LAT: -80 */ {  6058, 5995, 5916, 5824, 5721, 5610, 5492, 5370, 5248, 5128, 5014, 4908, 4815, 4735, 4672, 4627, 4602, 4600, 4622, 4668, 4738, 4832, 4946, 5078, 5222, 5371, 5521, 5664, 5795, 5909, 6002, 6071, 6115, 6135, 6130, 6104, 6058, },
+	/* LAT: -70 */ {  6303, 6170, 6020, 5855, 5677, 5485, 5280, 5065, 4846, 4630, 4428, 4245, 4087, 3956, 3852, 3776, 3728, 3716, 3744, 3820, 3948, 4128, 4356, 4624, 4919, 5226, 5529, 5813, 6063, 6267, 6418, 6514, 6556, 6548, 6499, 6415, 6303, },
+	/* LAT: -60 */ {  6188, 5996, 5795, 5586, 5368, 5133, 4877, 4598, 4306, 4016, 3749, 3522, 3342, 3207, 3108, 3035, 2984, 2964, 2990, 3080, 3248, 3500, 3826, 4212, 4634, 5068, 5489, 5873, 6198, 6447, 6610, 6689, 6691, 6628, 6515, 6364, 6188, },
+	/* LAT: -50 */ {  5845, 5615, 5383, 5153, 4921, 4676, 4405, 4102, 3774, 3442, 3140, 2899, 2738, 2646, 2598, 2564, 2530, 2504, 2509, 2581, 2754, 3043, 3439, 3910, 4417, 4922, 5398, 5816, 6156, 6397, 6536, 6577, 6534, 6424, 6263, 6065, 5845, },
+	/* LAT: -40 */ {  5394, 5148, 4904, 4664, 4430, 4193, 3938, 3655, 3341, 3014, 2716, 2493, 2375, 2349, 2368, 2390, 2395, 2383, 2371, 2399, 2529, 2803, 3220, 3733, 4280, 4804, 5272, 5660, 5953, 6140, 6229, 6232, 6162, 6032, 5851, 5633, 5394, },
+	/* LAT: -30 */ {  4879, 4638, 4400, 4165, 3939, 3720, 3502, 3270, 3015, 2740, 2482, 2299, 2228, 2253, 2320, 2391, 2457, 2507, 2530, 2542, 2609, 2806, 3167, 3655, 4186, 4683, 5102, 5419, 5623, 5722, 5749, 5722, 5643, 5511, 5331, 5115, 4879, },
+	/* LAT: -20 */ {  4321, 4109, 3901, 3696, 3500, 3317, 3149, 2985, 2808, 2610, 2419, 2285, 2243, 2286, 2375, 2486, 2614, 2742, 2832, 2868, 2893, 2988, 3226, 3610, 4059, 4486, 4835, 5071, 5176, 5184, 5154, 5107, 5025, 4897, 4731, 4534, 4321, },
+	/* LAT: -10 */ {  3790, 3630, 3477, 3331, 3195, 3075, 2972, 2880, 2783, 2669, 2547, 2447, 2401, 2425, 2510, 2639, 2795, 2954, 3078, 3141, 3156, 3182, 3304, 3555, 3881, 4203, 4468, 4631, 4667, 4615, 4547, 4483, 4395, 4270, 4121, 3958, 3790, },
+	/* LAT:   0 */ {  3412, 3319, 3236, 3163, 3108, 3070, 3044, 3025, 3001, 2954, 2875, 2780, 2699, 2667, 2708, 2811, 2943, 3079, 3194, 3270, 3301, 3323, 3397, 3553, 3762, 3975, 4155, 4262, 4268, 4201, 4113, 4021, 3909, 3777, 3643, 3520, 3412, },
+	/* LAT:  10 */ {  3283, 3252, 3232, 3228, 3253, 3300, 3356, 3410, 3445, 3436, 3367, 3252, 3125, 3030, 3003, 3043, 3124, 3222, 3323, 3408, 3472, 3534, 3621, 3739, 3874, 4012, 4132, 4203, 4205, 4144, 4034, 3891, 3729, 3570, 3435, 3338, 3283, },
+	/* LAT:  20 */ {  3399, 3403, 3429, 3483, 3575, 3697, 3826, 3943, 4025, 4037, 3964, 3822, 3655, 3514, 3438, 3425, 3460, 3532, 3628, 3725, 3816, 3914, 4025, 4136, 4245, 4356, 4458, 4524, 4535, 4477, 4340, 4138, 3911, 3702, 3539, 3438, 3399, },
+	/* LAT:  30 */ {  3723, 3730, 3785, 3885, 4028, 4199, 4375, 4531, 4640, 4667, 4594, 4438, 4249, 4084, 3978, 3931, 3934, 3985, 4071, 4169, 4266, 4372, 4488, 4606, 4725, 4852, 4972, 5059, 5085, 5028, 4872, 4633, 4359, 4104, 3905, 3777, 3723, },
+	/* LAT:  40 */ {  4222, 4221, 4286, 4410, 4578, 4765, 4949, 5105, 5210, 5236, 5168, 5019, 4832, 4657, 4528, 4452, 4426, 4448, 4508, 4587, 4673, 4770, 4885, 5018, 5169, 5331, 5482, 5592, 5631, 5578, 5422, 5185, 4911, 4651, 4441, 4296, 4222, },
+	/* LAT:  50 */ {  4832, 4825, 4881, 4992, 5138, 5298, 5448, 5569, 5642, 5651, 5588, 5461, 5298, 5134, 4996, 4898, 4843, 4831, 4855, 4905, 4974, 5065, 5184, 5333, 5509, 5693, 5859, 5978, 6022, 5978, 5848, 5652, 5427, 5211, 5031, 4902, 4832, },
+	/* LAT:  60 */ {  5392, 5380, 5409, 5472, 5558, 5652, 5739, 5805, 5838, 5829, 5775, 5681, 5560, 5431, 5312, 5216, 5149, 5115, 5113, 5140, 5196, 5282, 5400, 5547, 5714, 5882, 6030, 6134, 6178, 6156, 6074, 5947, 5801, 5658, 5536, 5445, 5392, },
+	/* LAT:  70 */ {  5726, 5706, 5704, 5716, 5738, 5765, 5788, 5803, 5802, 5783, 5743, 5686, 5615, 5539, 5465, 5401, 5353, 5325, 5320, 5340, 5384, 5454, 5545, 5654, 5771, 5886, 5985, 6058, 6097, 6101, 6072, 6018, 5951, 5881, 5816, 5763, 5726, },
+	/* LAT:  80 */ {  5789, 5772, 5758, 5746, 5736, 5726, 5716, 5705, 5690, 5671, 5649, 5624, 5597, 5569, 5545, 5524, 5510, 5505, 5510, 5525, 5551, 5586, 5629, 5676, 5726, 5774, 5817, 5851, 5876, 5889, 5891, 5884, 5870, 5851, 5830, 5809, 5789, },
 	/* LAT:  90 */ {  5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, },
 };
@@ -83,7 +83,7 @@ PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);
 * @value 2 Apply the estimated scale in air
 * @group Airspeed Validator
 */
-PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 1);
+PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 2);

 /**
 * Scale of airspeed sensor 1
@@ -92,67 +92,82 @@ private:

 	void Run() override;

-	void update_parameters(bool force = false);
+	bool init_attitude_q();

-	bool init_attq();
+	void update_gps_position();
+
+	void update_magnetometer();
+
+	void update_motion_capture_odometry();
+
+	void update_sensors();
+
+	void update_visual_odometry();
+
+	void update_vehicle_attitude();
+
+	void update_vehicle_local_position();
+
+	void update_parameters(bool force = false);

 	bool update(float dt);

 	// Update magnetic declination (in rads) immediately changing yaw rotation
 	void update_mag_declination(float new_declination);

-
-	const float _eo_max_std_dev = 100.0f;		/**< Maximum permissible standard deviation for estimated orientation */
+	const float _eo_max_std_dev = 100.0f;           /**< Maximum permissible standard deviation for estimated orientation */
 	const float _dt_min = 0.00001f;
 	const float _dt_max = 0.02f;

 	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};

-	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription		_gps_sub{ORB_ID(vehicle_gps_position)};
-	uORB::Subscription		_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription		_vision_odom_sub{ORB_ID(vehicle_visual_odometry)};
-	uORB::Subscription		_mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};
-	uORB::Subscription		_magnetometer_sub{ORB_ID(vehicle_magnetometer)};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

-	uORB::Publication<vehicle_attitude_s>	_att_pub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_magnetometer_sub{ORB_ID(vehicle_magnetometer)};
+	uORB::Subscription _vehicle_mocap_odometry_sub{ORB_ID(vehicle_mocap_odometry)};
+	uORB::Subscription _vehicle_visual_odometry_sub{ORB_ID(vehicle_visual_odometry)};

-	float		_mag_decl{0.0f};
-	float		_bias_max{0.0f};
+	uORB::Publication<vehicle_attitude_s> _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};

-	Vector3f	_gyro;
-	Vector3f	_accel;
-	Vector3f	_mag;
+	Vector3f    _accel{};
+	Vector3f    _gyro{};
+	Vector3f    _gyro_bias{};
+	Vector3f    _rates{};

-	Vector3f	_vision_hdg;
-	Vector3f	_mocap_hdg;
+	Vector3f    _mag{};
+	Vector3f    _mocap_hdg{};
+	Vector3f    _vision_hdg{};

-	Quatf		_q;
-	Vector3f	_rates;
-	Vector3f	_gyro_bias;
+	Vector3f    _pos_acc{};
+	Vector3f    _vel_prev{};

-	Vector3f	_vel_prev;
-	hrt_abstime	_vel_prev_t{0};
+	Quatf       _q{};

-	Vector3f	_pos_acc;
+	hrt_abstime _imu_timestamp{};
+	hrt_abstime _imu_prev_timestamp{};
+	hrt_abstime _vel_prev_timestamp{};

-	hrt_abstime	_last_time{0};
+	float       _bias_max{};
+	float       _mag_decl{};

-	bool		_inited{false};
-	bool		_data_good{false};
-	bool		_ext_hdg_good{false};
+	bool        _data_good{false};
+	bool        _ext_hdg_good{false};
+	bool        _initialized{false};

 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,
-		(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,
-		(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,
+		(ParamFloat<px4::params::ATT_W_ACC>)       _param_att_w_acc,
+		(ParamFloat<px4::params::ATT_W_MAG>)       _param_att_w_mag,
+		(ParamFloat<px4::params::ATT_W_EXT_HDG>)   _param_att_w_ext_hdg,
 		(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,
-		(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,
-		(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
-		(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,
-		(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
-		(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
-		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
+		(ParamFloat<px4::params::ATT_MAG_DECL>)    _param_att_mag_decl,
+		(ParamInt<px4::params::ATT_MAG_DECL_A>)    _param_att_mag_decl_a,
+		(ParamInt<px4::params::ATT_EXT_HDG_M>)     _param_att_ext_hdg_m,
+		(ParamInt<px4::params::ATT_ACC_COMP>)      _param_att_acc_comp,
+		(ParamFloat<px4::params::ATT_BIAS_MAX>)    _param_att_bias_mas,
+		(ParamInt<px4::params::SYS_HAS_MAG>)       _param_sys_has_mag
 	)
 };

@@ -160,25 +175,10 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
-	_vel_prev.zero();
-	_pos_acc.zero();
-
-	_gyro.zero();
-	_accel.zero();
-	_mag.zero();
-
-	_vision_hdg.zero();
-	_mocap_hdg.zero();
-
-	_q.zero();
-	_rates.zero();
-	_gyro_bias.zero();
-
 	update_parameters(true);
 }

-bool
-AttitudeEstimatorQ::init()
+bool AttitudeEstimatorQ::init()
 {
 	if (!_sensors_sub.registerCallback()) {
 		PX4_ERR("callback registration failed");
@@ -188,8 +188,7 @@ AttitudeEstimatorQ::init()
 	return true;
 }

-void
-AttitudeEstimatorQ::Run()
+void AttitudeEstimatorQ::Run()
 {
 	if (should_exit()) {
 		_sensors_sub.unregisterCallback();
@@ -197,14 +196,94 @@ AttitudeEstimatorQ::Run()
 		return;
 	}

+	if (_sensors_sub.updated()) {
+		_data_good = true;
+		_ext_hdg_good = false;
+
+		update_parameters();
+		update_sensors();
+		update_magnetometer();
+		update_visual_odometry();
+		update_motion_capture_odometry();
+		update_gps_position();
+		update_vehicle_local_position();
+		update_vehicle_attitude();
+	}
+}
+
+void AttitudeEstimatorQ::update_gps_position()
+{
+	if (_vehicle_gps_position_sub.updated()) {
+		vehicle_gps_position_s gps;
+
+		if (_vehicle_gps_position_sub.update(&gps)) {
+			if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
+				// set magnetic declination automatically
+				update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
+			}
+		}
+	}
+}
+
+void AttitudeEstimatorQ::update_magnetometer()
+{
+	// Update magnetometer
+	if (_vehicle_magnetometer_sub.updated()) {
+		vehicle_magnetometer_s magnetometer;
+
+		if (_vehicle_magnetometer_sub.update(&magnetometer)) {
+			_mag(0) = magnetometer.magnetometer_ga[0];
+			_mag(1) = magnetometer.magnetometer_ga[1];
+			_mag(2) = magnetometer.magnetometer_ga[2];
+
+			if (_mag.length() < 0.01f) {
+				PX4_ERR("degenerate mag!");
+				return;
+			}
+		}
+	}
+}
+
+void AttitudeEstimatorQ::update_motion_capture_odometry()
+{
+	if (_vehicle_mocap_odometry_sub.updated()) {
+		vehicle_odometry_s mocap;
+
+		if (_vehicle_mocap_odometry_sub.update(&mocap)) {
+			// validation check for mocap attitude data
+			bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
+					       && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
+							       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
+							       fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
+									       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
+
+			if (mocap_att_valid) {
+				Dcmf Rmoc = Quatf(mocap.q);
+				Vector3f v(1.0f, 0.0f, 0.4f);
+
+				// Rmoc is Rwr (robot respect to world) while v is respect to world.
+				// Hence Rmoc must be transposed having (Rwr)' * Vw
+				// Rrw * Vw = vn. This way we have consistency
+				_mocap_hdg = Rmoc.transpose() * v;
+
+				// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
+				if (_param_att_ext_hdg_m.get() == 2) {
+					// Check for timeouts on data
+					_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
+				}
+			}
+		}
+	}
+}
+
+void AttitudeEstimatorQ::update_sensors()
+{
 	sensor_combined_s sensors;

 	if (_sensors_sub.update(&sensors)) {
-
-		update_parameters();
-
-		// Feed validator with recent sensor data
+		// update validator with recent sensor data
 		if (sensors.timestamp > 0) {
+			_imu_timestamp = sensors.timestamp;
 			_gyro(0) = sensors.gyro_rad[0];
 			_gyro(1) = sensors.gyro_rad[1];
 			_gyro(2) = sensors.gyro_rad[2];
@@ -220,148 +299,93 @@ AttitudeEstimatorQ::Run()
 				return;
 			}
 		}
+	}
+}

-		// Update magnetometer
-		if (_magnetometer_sub.updated()) {
-			vehicle_magnetometer_s magnetometer;
+void AttitudeEstimatorQ::update_vehicle_attitude()
+{
+	// time from previous iteration
+	hrt_abstime now = hrt_absolute_time();
+	const float dt = math::constrain((now - _imu_prev_timestamp) / 1e6f, _dt_min, _dt_max);
+	_imu_prev_timestamp = now;

-			if (_magnetometer_sub.copy(&magnetometer)) {
-				_mag(0) = magnetometer.magnetometer_ga[0];
-				_mag(1) = magnetometer.magnetometer_ga[1];
-				_mag(2) = magnetometer.magnetometer_ga[2];
+	if (update(dt)) {
+		vehicle_attitude_s vehicle_attitude{};
+		vehicle_attitude.timestamp_sample = _imu_timestamp;
+		_q.copyTo(vehicle_attitude.q);

-				if (_mag.length() < 0.01f) {
-					PX4_ERR("degenerate mag!");
-					return;
+		/* the instance count is not used here */
+		vehicle_attitude.timestamp = hrt_absolute_time();
+		_vehicle_attitude_pub.publish(vehicle_attitude);
+	}
+}
+
+void AttitudeEstimatorQ::update_vehicle_local_position()
+{
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s lpos;
+
+		if (_vehicle_local_position_sub.update(&lpos)) {
+
+			if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
+			    && lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _initialized) {
+
+				/* position data is actual */
+				const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
+
+				/* velocity updated */
+				if (_vel_prev_timestamp != 0 && lpos.timestamp != _vel_prev_timestamp) {
+					float vel_dt = (lpos.timestamp - _vel_prev_timestamp) / 1e6f;
+					/* calculate acceleration in body frame */
+					_pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt);
 				}
+
+				_vel_prev_timestamp = lpos.timestamp;
+				_vel_prev = vel;
+
+			} else {
+				/* position data is outdated, reset acceleration */
+				_pos_acc.zero();
+				_vel_prev.zero();
+				_vel_prev_timestamp = 0;
 			}
-
-		}
-
-		_data_good = true;
-
-		// Update vision and motion capture heading
-		_ext_hdg_good = false;
-
-		if (_vision_odom_sub.updated()) {
-			vehicle_odometry_s vision;
-
-			if (_vision_odom_sub.copy(&vision)) {
-				// validation check for vision attitude data
-				bool vision_att_valid = PX4_ISFINITE(vision.q[0])
-							&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
-									vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
-									fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
-											vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
-
-				if (vision_att_valid) {
-					Dcmf Rvis = Quatf(vision.q);
-					Vector3f v(1.0f, 0.0f, 0.4f);
-
-					// Rvis is Rwr (robot respect to world) while v is respect to world.
-					// Hence Rvis must be transposed having (Rwr)' * Vw
-					// Rrw * Vw = vn. This way we have consistency
-					_vision_hdg = Rvis.transpose() * v;
-
-					// vision external heading usage (ATT_EXT_HDG_M 1)
-					if (_param_att_ext_hdg_m.get() == 1) {
-						// Check for timeouts on data
-						_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
-					}
-				}
-			}
-		}
-
-		if (_mocap_odom_sub.updated()) {
-			vehicle_odometry_s mocap;
-
-			if (_mocap_odom_sub.copy(&mocap)) {
-				// validation check for mocap attitude data
-				bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
-						       && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
-								       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
-								       fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
-										       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
-
-				if (mocap_att_valid) {
-					Dcmf Rmoc = Quatf(mocap.q);
-					Vector3f v(1.0f, 0.0f, 0.4f);
-
-					// Rmoc is Rwr (robot respect to world) while v is respect to world.
-					// Hence Rmoc must be transposed having (Rwr)' * Vw
-					// Rrw * Vw = vn. This way we have consistency
-					_mocap_hdg = Rmoc.transpose() * v;
-
-					// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
-					if (_param_att_ext_hdg_m.get() == 2) {
-						// Check for timeouts on data
-						_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
-					}
-				}
-			}
-		}
-
-		if (_gps_sub.updated()) {
-			vehicle_gps_position_s gps;
-
-			if (_gps_sub.copy(&gps)) {
-				if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
-					/* set magnetic declination automatically */
-					update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
-				}
-			}
-		}
-
-		if (_local_position_sub.updated()) {
-			vehicle_local_position_s lpos;
-
-			if (_local_position_sub.copy(&lpos)) {
-
-				if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
-				    && lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
-
-					/* position data is actual */
-					const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
-
-					/* velocity updated */
-					if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
-						float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
-						/* calculate acceleration in body frame */
-						_pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt);
-					}
-
-					_vel_prev_t = lpos.timestamp;
-					_vel_prev = vel;
-
-				} else {
-					/* position data is outdated, reset acceleration */
-					_pos_acc.zero();
-					_vel_prev.zero();
-					_vel_prev_t = 0;
-				}
-			}
-		}
-
-		/* time from previous iteration */
-		hrt_abstime now = hrt_absolute_time();
-		const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
-		_last_time = now;
-
-		if (update(dt)) {
-			vehicle_attitude_s att = {};
-			att.timestamp_sample = sensors.timestamp;
-			_q.copyTo(att.q);
-
-			/* the instance count is not used here */
-			att.timestamp = hrt_absolute_time();
-			_att_pub.publish(att);
-
 		}
 	}
 }

-void
-AttitudeEstimatorQ::update_parameters(bool force)
+void AttitudeEstimatorQ::update_visual_odometry()
+{
+	if (_vehicle_visual_odometry_sub.updated()) {
+		vehicle_odometry_s vision;
+
+		if (_vehicle_visual_odometry_sub.update(&vision)) {
+			// validation check for vision attitude data
+			bool vision_att_valid = PX4_ISFINITE(vision.q[0])
+						&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
+								vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
+								fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
+										vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
+
+			if (vision_att_valid) {
+				Dcmf Rvis = Quatf(vision.q);
+				Vector3f v(1.0f, 0.0f, 0.4f);
+
+				// Rvis is Rwr (robot respect to world) while v is respect to world.
+				// Hence Rvis must be transposed having (Rwr)' * Vw
+				// Rrw * Vw = vn. This way we have consistency
+				_vision_hdg = Rvis.transpose() * v;
+
+				// vision external heading usage (ATT_EXT_HDG_M 1)
+				if (_param_att_ext_hdg_m.get() == 1) {
+					// Check for timeouts on data
+					_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
+				}
+			}
+		}
+	}
+}
+
+void AttitudeEstimatorQ::update_parameters(bool force)
 {
 	// check for parameter updates
 	if (_parameter_update_sub.updated() || force) {
@@ -388,8 +412,7 @@ AttitudeEstimatorQ::update_parameters(bool force)
 	}
 }

-bool
-AttitudeEstimatorQ::init_attq()
+bool AttitudeEstimatorQ::init_attitude_q()
 {
 	// Rotation matrix can be easily constructed from acceleration and mag field vectors
 	// 'k' is Earth Z axis (Down) unit vector in body frame
@@ -421,25 +444,24 @@ AttitudeEstimatorQ::init_attq()
 	if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
 	    PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
 	    _q.length() > 0.95f && _q.length() < 1.05f) {
-		_inited = true;
+		_initialized = true;

 	} else {
-		_inited = false;
+		_initialized = false;
 	}

-	return _inited;
+	return _initialized;
 }

-bool
-AttitudeEstimatorQ::update(float dt)
+bool AttitudeEstimatorQ::update(float dt)
 {
-	if (!_inited) {
+	if (!_initialized) {

 		if (!_data_good) {
 			return false;
 		}

-		return init_attq();
+		return init_attitude_q();
 	}

 	Quatf q_last = _q;
@@ -543,11 +565,10 @@ AttitudeEstimatorQ::update(float dt)
 	return true;
 }

-void
-AttitudeEstimatorQ::update_mag_declination(float new_declination)
+void AttitudeEstimatorQ::update_mag_declination(float new_declination)
 {
 	// Apply initial declination or trivial rotations without changing estimation
-	if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
+	if (!_initialized || fabsf(new_declination - _mag_decl) < 0.0001f) {
 		_mag_decl = new_declination;

 	} else {
@@ -558,14 +579,12 @@ AttitudeEstimatorQ::update_mag_declination(float new_declination)
 	}
 }

-int
-AttitudeEstimatorQ::custom_command(int argc, char *argv[])
+int AttitudeEstimatorQ::custom_command(int argc, char *argv[])
 {
 	return print_usage("unknown command");
 }

-int
-AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
+int AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
 {
 	AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();

@@ -588,8 +607,7 @@ AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }

-int
-AttitudeEstimatorQ::print_usage(const char *reason)
+int AttitudeEstimatorQ::print_usage(const char *reason)
 {
 	if (reason) {
 		PX4_WARN("%s\n", reason);
@@ -127,8 +127,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		}

 		if (hil_enabled) {
-			/* enforce lockdown in HIL */
-			armed.lockdown = true;
+			// Enable actuators in HITL
 			status_flags.system_sensors_initialized = true;

 			/* recover from a prearm fail */
@@ -2298,21 +2298,22 @@ Commander::run()
 		}

 		/* safety button */
-		bool safety_updated = _safety.safetyButtonHandler();
+		const bool safety_changed = _safety.safetyButtonHandler();
 		_vehicle_status.safety_button_available = _safety.isButtonAvailable();
 		_vehicle_status.safety_off = _safety.isSafetyOff();

-		if (safety_updated) {
-
+		if (safety_changed) {
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.isButtonAvailable(), _safety.isSafetyOff(),
 					 _safety.isButtonAvailable(), _vehicle_status);

 			// Notify the user if the status of the safety button changes
-			if (_safety.isSafetyOff()) {
-				set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
+			if (!_safety.isSafetyDisabled()) {
+				if (_safety.isSafetyOff()) {
+					set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);

-			} else {
-				tune_neutral(true);
+				} else {
+					tune_neutral(true);
+				}
 			}

 			_status_changed = true;
@@ -2428,10 +2429,8 @@ Commander::run()
 			bool auto_disarm = _actuator_armed.manual_lockdown;

 			// auto disarm if locked down to avoid user confusion
-			//  skipped in HITL where lockdown is enabled for safety
-			if (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) {
-				auto_disarm |= _actuator_armed.lockdown;
-			}
+			// Enable actuators in HITL
+			auto_disarm |= _armed.lockdown;

 			_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());

@@ -3805,7 +3804,7 @@ void Commander::avoidance_check()

 void Commander::battery_status_check()
 {
-	int battery_required_count{0};
+	size_t battery_required_count = 0;
 	bool battery_has_fault = false;
 	// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
 	// option is to check if ANY of them have a warning, and specifically find which one has the most
@@ -3830,7 +3829,7 @@ void Commander::battery_status_check()

 		if (_arm_state_machine.isArmed()) {

-			if ((_last_connected_batteries & (1 << index)) && !battery.connected) {
+			if (_last_connected_batteries[index] && !battery.connected) {
 				mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
 				events::send<uint8_t>(events::ID("commander_battery_disconnected"), {events::Log::Emergency, events::LogInternal::Warning},
 						      "Battery {1} disconnected. Land now!", index + 1);
@@ -3847,13 +3846,7 @@ void Commander::battery_status_check()
 			}
 		}

-		if (battery.connected) {
-			_last_connected_batteries |= 1 << index;
-
-		} else {
-			_last_connected_batteries &= ~(1 << index);
-		}
-
+		_last_connected_batteries.set(index, battery.connected);
 		_last_battery_mode[index] = battery.mode;

 		if (battery.connected) {
@@ -3958,7 +3951,7 @@ void Commander::battery_status_check()
 		// All connected batteries are regularly being published
 		(hrt_elapsed_time(&oldest_update) < 5_s)
 		// There is at least one connected battery (in any slot)
-		&& (math::countSetBits(_last_connected_batteries) >= battery_required_count)
+		&& (_last_connected_batteries.count() >= battery_required_count)
 		// No currently-connected batteries have any warning
 		&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)
 		// No currently-connected batteries have any fault
@@ -41,6 +41,7 @@
 #include "state_machine_helper.h"
 #include "worker_thread.hpp"

+#include <containers/Bitset.hpp>
 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
 #include <lib/mathlib/mathlib.h>
@@ -347,7 +348,7 @@ private:

 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime	_battery_failsafe_timestamp{0};
-	uint8_t		_last_connected_batteries{0};
+	px4::Bitset<battery_status_s::MAX_INSTANCES> _last_connected_batteries;
 	uint32_t	_last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};
 	uint16_t	_last_battery_fault[battery_status_s::MAX_INSTANCES] {};
 	uint8_t		_last_battery_mode[battery_status_s::MAX_INSTANCES] {};
@@ -402,7 +403,7 @@ private:
 	vehicle_status_s        _vehicle_status{};
 	vehicle_status_flags_s  _vehicle_status_flags{};

-	Safety _safety{};
+	Safety _safety;

 	WorkerThread _worker_thread;

@@ -42,20 +42,18 @@ using namespace time_literals;

 Safety::Safety()
 {
-	/*
-	 * Safety can be turned off with the CBRK_IO_SAFETY parameter.
-	 */
+	// Safety can be turned off with the CBRK_IO_SAFETY parameter.
 	_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
+
+	if (_safety_disabled) {
+		_button_available = true;
+		_safety_off = true;
+	}
 }

 bool Safety::safetyButtonHandler()
 {
-	if (_safety_disabled) {
-		_button_available = true;
-		_safety_off = true;
-
-	} else {
-
+	if (!_safety_disabled) {
 		if (!_button_available && _safety_button_sub.advertised()) {
 			_button_available = true;
 		}
@@ -67,10 +65,8 @@ bool Safety::safetyButtonHandler()
 		}
 	}

-	bool safety_changed = _previous_safety_off != _safety_off;
-
+	const bool safety_changed = _previous_safety_off != _safety_off;
 	_previous_safety_off = _safety_off;
-
 	return safety_changed;
 }

@@ -43,23 +43,21 @@

 class Safety
 {
-
 public:
-
 	Safety();
 	~Safety() = default;

 	bool safetyButtonHandler();
 	void activateSafety();
-	bool isButtonAvailable() { return _button_available;}
-	bool isSafetyOff() { return _safety_off;}
+	bool isButtonAvailable() { return _button_available; }
+	bool isSafetyOff() { return _safety_off; }
+	bool isSafetyDisabled() { return _safety_disabled; }

 private:
-
 	uORB::Subscription _safety_button_sub{ORB_ID::safety_button};

-	bool _button_available{false};		//<! Set to true if a safety button is connected
-	bool _safety_off{false};			//<! Set to true if safety is off
-	bool _previous_safety_off{false};	//<! Previous safety value
-	bool _safety_disabled{false};		//<! Set to true if safety is disabled
+	bool _button_available{false};///< Set to true if a safety button is connected
+	bool _safety_off{false}; ///< Set to true if safety is off
+	bool _previous_safety_off{false}; ///< Previous safety value
+	bool _safety_disabled{false}; ///< Set to true if safety is disabled
 };
@@ -927,7 +927,7 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0);
 /**
 * Enable force safety
 *
- * Force safety when the vehicle disarms. Not supported when safety button used over PX4IO board.
+ * Force safety when the vehicle disarms
 *
 * @boolean
 * @group Commander
@@ -187,13 +187,6 @@ static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
 		goto Out;
 	}

-	/* tell IO to switch off safety without using the safety switch */
-	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
-		return_code = PX4_ERROR;
-		goto Out;
-	}
-
 	calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");

 	timeout_start = hrt_absolute_time();
@@ -233,9 +226,6 @@ static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
 Out:

 	if (fd != -1) {
-		if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
-			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
-		}

 		if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
 			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
@@ -221,8 +221,8 @@ void FailureDetector::updateAttitudeStatus()
 		const float max_roll(fabsf(math::radians(max_roll_deg)));
 		const float max_pitch(fabsf(math::radians(max_pitch_deg)));

-		const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
-		const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
+		const bool roll_status = (max_roll > FLT_EPSILON) && (fabsf(roll) > max_roll);
+		const bool pitch_status = (max_pitch > FLT_EPSILON) && (fabsf(pitch) > max_pitch);

 		hrt_abstime time_now = hrt_absolute_time();

@@ -384,7 +384,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,

 			// Check for telemetry timeout
 			const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
-			const bool esc_timed_out = telemetry_age > 100_ms;  // TODO: magic number
+			const bool esc_timed_out = telemetry_age > 300_ms;

 			const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc);
 			const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc);
@@ -399,35 +399,40 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 			}

 			// Check if ESC current is too low
-			float esc_throttle = 0.f;
-
-			if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
-				esc_throttle = fabsf(actuator_motors.control[i_esc]);
+			if (cur_esc_report.esc_current > FLT_EPSILON) {
+				_motor_failure_escs_have_current = true;
 			}

-			const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
-			const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
-						     _param_fd_motor_current2throttle_thres.get();
+			if (_motor_failure_escs_have_current) {
+				float esc_throttle = 0.f;

-			if (throttle_above_threshold && current_too_low && !esc_timed_out) {
-				if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
-					_motor_failure_undercurrent_start_time[i_esc] = time_now;
+				if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
+					esc_throttle = fabsf(actuator_motors.control[i_esc]);
 				}

-			} else {
-				if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
-					_motor_failure_undercurrent_start_time[i_esc] = 0;
+				const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
+				const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
+							     _param_fd_motor_current2throttle_thres.get();
+
+				if (throttle_above_threshold && current_too_low && !esc_timed_out) {
+					if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
+						_motor_failure_undercurrent_start_time[i_esc] = time_now;
+					}
+
+				} else {
+					if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
+						_motor_failure_undercurrent_start_time[i_esc] = 0;
+					}
 				}
+
+				if (_motor_failure_undercurrent_start_time[i_esc] != 0
+				    && (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
+				    && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
+					// Set flag
+					_motor_failure_esc_under_current_mask |= (1 << i_esc);
+
+				} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
 			}
-
-			if (_motor_failure_undercurrent_start_time[i_esc] != 0
-			    && (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
-			    && (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
-				// Set flag
-				_motor_failure_esc_under_current_mask |= (1 << i_esc);
-
-			} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
-
 		}

 		bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
@@ -129,6 +129,7 @@ private:
 	uint8_t _motor_failure_esc_valid_current_mask{};  // ESC 1-8, true if ESC telemetry was valid at some point
 	uint8_t _motor_failure_esc_timed_out_mask{};      // ESC telemetry no longer available -> failure
 	uint8_t _motor_failure_esc_under_current_mask{};  // ESC drawing too little current -> failure
+	bool _motor_failure_escs_have_current{false}; // true if some ESC had non-zero current (some don't support it)
 	hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};

 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -53,7 +53,7 @@
 *
 * Setting this parameter to 0 disables the check
 *
- * @min 60
+ * @min 0
 * @max 180
 * @unit deg
 * @group Failure Detector
@@ -71,7 +71,7 @@ PARAM_DEFINE_INT32(FD_FAIL_R, 60);
 *
 * Setting this parameter to 0 disables the check
 *
- * @min 60
+ * @min 0
 * @max 180
 * @unit deg
 * @group Failure Detector
@@ -556,6 +556,18 @@ mixer:
                            label: 'Direction CCW'
                            function: 'spin-dir'
                            show_as: 'true-if-positive'
+                          - name: 'CA_ROTOR${i}_AX'
+                            label: 'Axis X'
+                            function: 'axisx'
+                            advanced: true
+                          - name: 'CA_ROTOR${i}_AY'
+                            label: 'Axis Y'
+                            function: 'axisy'
+                            advanced: true
+                          - name: 'CA_ROTOR${i}_AZ'
+                            label: 'Axis Z'
+                            function: 'axisz'
+                            advanced: true

            1: # Fixed Wing
                title: 'Fixed Wing'
@@ -23,7 +23,6 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
 	_delta_vel = imu_sample.delta_vel;
 	_delta_ang_dt = imu_sample.delta_ang_dt;
 	_delta_vel_dt = imu_sample.delta_vel_dt;
-	_run_ekf_gsf = run_EKF;
 	_true_airspeed = airspeed;

 	// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
@@ -60,7 +59,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
 	}

 	// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
-	if (_run_ekf_gsf && _vel_data_updated) {
+	if (run_EKF && _vel_data_updated) {
 		if (!_ekf_gsf_vel_fuse_started) {
 			initialiseEKFGSF();
 			ahrsAlignYaw();
@@ -111,7 +110,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
 			}
 		}

-	} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
+	} else if (_ekf_gsf_vel_fuse_started && !run_EKF) {
 		// wait to fly again
 		_ekf_gsf_vel_fuse_started = false;
 	}
@@ -108,7 +108,6 @@ private:
 	} _ekf_gsf[N_MODELS_EKFGSF] {};

 	bool _vel_data_updated{};	// true when velocity data has been updated
-	bool _run_ekf_gsf{};		// true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data
 	Vector2f _vel_NE{};        // NE velocity observations (m/s)
 	float _vel_accuracy{};     // 1-sigma accuracy of velocity observations (m/s)
 	bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active
@@ -338,6 +338,9 @@ public:
 	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
 			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);

+	// Returns true if the output of the yaw emergency estimator can be used for a reset
+	bool isYawEmergencyEstimateAvailable() const;
+
 	const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }

 	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
@@ -1062,9 +1065,6 @@ private:
 	// Returns true if the reset was successful
 	bool resetYawToEKFGSF();

-	// Returns true if the output of the yaw emergency estimator can be used for a reset
-	bool isYawEmergencyEstimateAvailable() const;
-
 	void resetGpsDriftCheckFilters();

 	bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status) const
@@ -194,6 +194,10 @@ void Ekf::resetHaglRng()
 void Ekf::stopHaglRngFusion()
 {
 	_hagl_sensor_status.flags.range_finder = false;
+	_hagl_innov = 0.f;
+	_hagl_innov_var = 0.f;
+	_hagl_test_ratio = 0.f;
+	_innov_check_fail_status.flags.reject_hagl = false;
 }

 void Ekf::fuseHaglRng()
@@ -290,6 +294,10 @@ void Ekf::startHaglFlowFusion()
 void Ekf::stopHaglFlowFusion()
 {
 	_hagl_sensor_status.flags.flow = false;
+	_hagl_innov = 0.f;
+	_hagl_innov_var = 0.f;
+	_hagl_test_ratio = 0.f;
+	_innov_check_fail_status.flags.reject_hagl = false;
 }

 void Ekf::resetHaglFlow()
@@ -877,6 +877,11 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 		_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
 		_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);

+		_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
+		_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
+		_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
+		_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
+
 		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);

 	} else if (_ekf.control_status_flags().in_air != _ekf.control_status_prev_flags().in_air) {
@@ -1384,6 +1389,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 			       yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve,
 			       yaw_est_test_data.weight)) {

+		yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable();
 		yaw_est_test_data.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
 		yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();

@@ -102,10 +102,29 @@ bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s

 bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
 {
-	const float hgt_innov = fmaxf(fabsf(innov.gps_vpos), fmaxf(fabs(innov.ev_vpos),
-				      fabs(innov.rng_vpos)));    // only temporary solution
-	const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
-	return checkInnovFailed(hgt_innov_lpf, hgt_innov, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	bool has_failed = false;
+
+	if (_is_using_baro_hgt_aiding) {
+		const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	if (_is_using_gps_hgt_aiding) {
+		const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	if (_is_using_rng_hgt_aiding) {
+		const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	if (_is_using_ev_hgt_aiding) {
+		const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
+		has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
+	}
+
+	return has_failed;
 }

 bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
@@ -127,6 +146,10 @@ void PreFlightChecker::reset()
 	_is_using_flow_aiding = false;
 	_is_using_ev_pos_aiding = false;
 	_is_using_ev_vel_aiding = false;
+	_is_using_baro_hgt_aiding = false;
+	_is_using_gps_hgt_aiding = false;
+	_is_using_rng_hgt_aiding = false;
+	_is_using_ev_hgt_aiding = false;
 	_has_heading_failed = false;
 	_has_horiz_vel_failed = false;
 	_has_vert_vel_failed = false;
@@ -134,7 +157,10 @@ void PreFlightChecker::reset()
 	_filter_vel_n_innov.reset();
 	_filter_vel_e_innov.reset();
 	_filter_vel_d_innov.reset();
-	_filter_hgt_innov.reset();
+	_filter_baro_hgt_innov.reset();
+	_filter_gps_hgt_innov.reset();
+	_filter_rng_hgt_innov.reset();
+	_filter_ev_hgt_innov.reset();
 	_filter_heading_innov.reset();
 	_filter_flow_x_innov.reset();
 	_filter_flow_y_innov.reset();
@@ -79,6 +79,11 @@ public:
 	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
 	void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }

+	void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
+	void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
+	void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
+	void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
+
 	bool hasHeadingFailed() const { return _has_heading_failed; }
 	bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
 	bool hasVertVelFailed() const { return _has_vert_vel_failed; }
@@ -149,15 +154,25 @@ private:
 	bool _is_using_ev_pos_aiding{};
 	bool _is_using_ev_vel_aiding{};

+	bool _is_using_baro_hgt_aiding{};
+	bool _is_using_gps_hgt_aiding{};
+	bool _is_using_rng_hgt_aiding{};
+	bool _is_using_ev_hgt_aiding{};
+
 	// Low-pass filters for innovation pre-flight checks
 	InnovationLpf _filter_vel_n_innov;	///< Preflight low pass filter N axis velocity innovations (m/sec)
 	InnovationLpf _filter_vel_e_innov;	///< Preflight low pass filter E axis velocity innovations (m/sec)
 	InnovationLpf _filter_vel_d_innov;	///< Preflight low pass filter D axis velocity innovations (m/sec)
-	InnovationLpf _filter_hgt_innov;	///< Preflight low pass filter height innovation (m)
 	InnovationLpf _filter_heading_innov;	///< Preflight low pass filter heading innovation magntitude (rad)
 	InnovationLpf _filter_flow_x_innov;	///< Preflight low pass filter optical flow innovation (rad)
 	InnovationLpf _filter_flow_y_innov;	///< Preflight low pass filter optical flow innovation (rad)

+	// Preflight low pass filter height innovation (m)
+	InnovationLpf _filter_baro_hgt_innov;
+	InnovationLpf _filter_gps_hgt_innov;
+	InnovationLpf _filter_rng_hgt_innov;
+	InnovationLpf _filter_ev_hgt_innov;
+
 	// Preflight low pass filter time constant inverse (1/sec)
 	static constexpr float _innov_lpf_tau_inv = 0.2f;
 	// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
@@ -2,45 +2,45 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 10000,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 90000,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 190000,0.979,-0.00865,-0.0138,0.205,-0.000572,-0.00011,-0.00742,-2.83e-05,-2.2e-05,-0.0351,-2.69e-14,7.98e-14,2.03e-15,2.99e-11,-2.9e-11,1.29e-09,0.203,0.0109,0.434,0,0,0,0,0,0.000143,0.00247,0.00247,0.00324,25,25,0.563,100,100,0.8,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-290000,0.979,-0.00873,-0.0141,0.205,0.000802,-0.000466,-0.0239,-6.92e-05,-2.29e-05,-0.0538,5.71e-12,-9.05e-13,-1.79e-13,1.07e-09,-1.03e-09,4.57e-08,0.203,0.0109,0.434,0,0,0,0,0,9.44e-05,0.00253,0.00253,0.00213,25,25,0.562,101,101,0.337,1e-06,1e-06,9.97e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-390000,0.979,-0.00874,-0.0144,0.205,0.00384,-0.000207,-0.0425,0.000107,7.76e-06,-0.0658,-2.92e-11,-1.07e-10,-1.39e-12,8.73e-09,-8.37e-09,3.69e-07,0.203,0.0109,0.434,0,0,0,0,0,7.22e-05,0.00263,0.00263,0.00162,24.8,24.8,0.557,0.29,0.29,0.209,1e-06,1e-06,9.88e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
-490000,0.979,-0.00876,-0.0148,0.205,0.00668,-0.00219,-0.0639,0.000628,-0.000111,-0.0812,-2.87e-10,-5.71e-10,-3.06e-12,2.75e-08,-2.63e-08,1.15e-06,0.203,0.0109,0.434,0,0,0,0,0,6.04e-05,0.00279,0.00279,0.00135,24.9,24.9,0.544,0.739,0.739,0.159,1e-06,1e-06,9.73e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+290000,0.979,-0.00873,-0.0141,0.205,0.000802,-0.000466,-0.0239,-6.92e-05,-2.29e-05,-0.0538,5.71e-12,-9.05e-13,-1.79e-13,1.07e-09,-1.03e-09,4.57e-08,0.203,0.0109,0.434,0,0,0,0,0,9.45e-05,0.00253,0.00253,0.00213,25,25,0.562,101,101,0.337,1e-06,1e-06,9.97e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+390000,0.979,-0.00874,-0.0144,0.205,0.00384,-0.000206,-0.0425,0.000107,7.77e-06,-0.0658,-2.92e-11,-1.07e-10,-1.39e-12,8.73e-09,-8.37e-09,3.69e-07,0.203,0.0109,0.434,0,0,0,0,0,7.23e-05,0.00263,0.00263,0.00162,24.8,24.8,0.557,0.29,0.29,0.209,1e-06,1e-06,9.88e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+490000,0.979,-0.00876,-0.0148,0.205,0.00668,-0.00219,-0.0639,0.000628,-0.00011,-0.0812,-2.87e-10,-5.71e-10,-3.06e-12,2.75e-08,-2.63e-08,1.15e-06,0.203,0.0109,0.434,0,0,0,0,0,6.04e-05,0.00279,0.00279,0.00135,24.9,24.9,0.544,0.739,0.739,0.159,1e-06,1e-06,9.73e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 590000,0.979,-0.00877,-0.015,0.205,0.00456,-0.00282,-0.0851,0.000297,-0.000135,-0.0938,-6.02e-09,-7.53e-09,5.54e-11,6.35e-08,-5.84e-08,2.02e-06,0.203,0.0109,0.434,0,0,0,0,0,5.37e-05,0.00299,0.00299,0.0012,7.8,7.8,0.527,0.267,0.267,0.141,1e-06,1e-06,9.49e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 690000,0.979,-0.0088,-0.0153,0.205,0.00888,-0.00425,-0.0976,0.000996,-0.000484,-0.103,-6.06e-09,-7.57e-09,5.59e-11,6.46e-08,-5.94e-08,2.07e-06,0.203,0.0109,0.434,0,0,0,0,0,5.01e-05,0.00324,0.00324,0.00111,7.87,7.87,0.497,0.556,0.556,0.13,1e-06,1e-06,9.16e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 790000,0.979,-0.0088,-0.0156,0.205,0.00942,-0.00187,-0.11,0.000559,-0.000205,-0.114,-3.56e-08,-3.46e-08,4.58e-10,1.03e-07,-1.01e-07,2.23e-06,0.203,0.0109,0.434,0,0,0,0,0,4.82e-05,0.00353,0.00353,0.00106,2.63,2.63,0.46,0.218,0.218,0.125,9.99e-07,9.99e-07,8.75e-07,4e-06,4e-06,3.99e-06,0,0,0,0,0,0,0,0
 890000,0.979,-0.00883,-0.0159,0.205,0.0126,-0.00133,-0.128,0.00162,-0.000368,-0.13,-3.77e-08,-3.7e-08,4.82e-10,1.4e-07,-1.35e-07,3.74e-06,0.203,0.0109,0.434,0,0,0,0,0,4.75e-05,0.00388,0.00388,0.00104,2.72,2.72,0.423,0.363,0.363,0.128,9.99e-07,9.99e-07,8.27e-07,4e-06,4e-06,3.99e-06,0,0,0,0,0,0,0,0
-990000,0.979,-0.00877,-0.0162,0.205,0.0132,0.000542,-0.143,0.00107,-0.000148,-0.144,-1.29e-07,-1.78e-07,1.19e-09,2.67e-07,-2.2e-07,4.15e-06,0.203,0.0109,0.434,0,0,0,0,0,4.74e-05,0.00424,0.00424,0.00103,1.25,1.25,0.379,0.178,0.178,0.127,9.97e-07,9.97e-07,7.73e-07,4e-06,4e-06,3.98e-06,0,0,0,0,0,0,0,0
+990000,0.979,-0.00877,-0.0162,0.205,0.0132,0.000543,-0.143,0.00107,-0.000148,-0.144,-1.29e-07,-1.78e-07,1.19e-09,2.67e-07,-2.2e-07,4.15e-06,0.203,0.0109,0.434,0,0,0,0,0,4.74e-05,0.00424,0.00424,0.00103,1.25,1.25,0.379,0.178,0.178,0.127,9.97e-07,9.97e-07,7.73e-07,4e-06,4e-06,3.98e-06,0,0,0,0,0,0,0,0
 1090000,0.979,-0.00889,-0.0164,0.205,0.0231,-0.00141,-0.157,0.00285,-0.000128,-0.16,-1.3e-07,-1.8e-07,1.2e-09,2.81e-07,-2.33e-07,4.72e-06,0.203,0.0109,0.434,0,0,0,0,0,4.76e-05,0.00469,0.00469,0.00103,1.38,1.38,0.335,0.265,0.265,0.125,9.97e-07,9.97e-07,7.15e-07,4e-06,4e-06,3.97e-06,0,0,0,0,0,0,0,0
-1190000,0.979,-0.00881,-0.0166,0.205,0.0251,-0.00245,-0.171,0.00218,-0.000138,-0.176,-4.35e-07,-7.71e-07,3.59e-09,6.52e-07,-4.25e-07,4.71e-06,0.203,0.0109,0.434,0,0,0,0,0,4.78e-05,0.00503,0.00503,0.00104,0.846,0.847,0.299,0.151,0.151,0.128,9.88e-07,9.88e-07,6.56e-07,3.99e-06,3.99e-06,3.95e-06,0,0,0,0,0,0,0,0
-1290000,0.979,-0.00869,-0.0168,0.205,0.0347,-0.00229,-0.186,0.00524,-0.000424,-0.194,-4.35e-07,-7.71e-07,3.59e-09,6.52e-07,-4.25e-07,4.71e-06,0.203,0.0109,0.434,0,0,0,0,0,4.82e-05,0.00557,0.00557,0.00104,1.02,1.02,0.262,0.214,0.214,0.125,9.88e-07,9.88e-07,5.97e-07,3.99e-06,3.99e-06,3.94e-06,0,0,0,0,0,0,0,0
+1190000,0.979,-0.00881,-0.0166,0.205,0.0251,-0.00245,-0.171,0.00218,-0.000138,-0.176,-4.35e-07,-7.71e-07,3.59e-09,6.52e-07,-4.25e-07,4.71e-06,0.203,0.0109,0.434,0,0,0,0,0,4.79e-05,0.00503,0.00503,0.00104,0.846,0.847,0.299,0.151,0.151,0.128,9.88e-07,9.88e-07,6.56e-07,3.99e-06,3.99e-06,3.95e-06,0,0,0,0,0,0,0,0
+1290000,0.979,-0.00869,-0.0168,0.205,0.0347,-0.00229,-0.186,0.00524,-0.000423,-0.194,-4.35e-07,-7.71e-07,3.59e-09,6.52e-07,-4.25e-07,4.71e-06,0.203,0.0109,0.434,0,0,0,0,0,4.82e-05,0.00557,0.00557,0.00104,1.02,1.02,0.262,0.214,0.214,0.125,9.88e-07,9.88e-07,5.97e-07,3.99e-06,3.99e-06,3.94e-06,0,0,0,0,0,0,0,0
 1390000,0.979,-0.00868,-0.017,0.205,0.0454,-0.00312,-0.2,0.0093,-0.000696,-0.214,-4.35e-07,-7.71e-07,3.59e-09,6.52e-07,-4.25e-07,4.71e-06,0.203,0.0109,0.434,0,0,0,0,0,4.84e-05,0.00616,0.00615,0.00104,1.25,1.25,0.23,0.3,0.3,0.122,9.88e-07,9.88e-07,5.41e-07,3.99e-06,3.99e-06,3.92e-06,0,0,0,0,0,0,0,0
-1490000,0.979,-0.00855,-0.017,0.205,0.0434,-0.00331,-0.214,0.00801,-0.000538,-0.234,-1.42e-06,-2.63e-06,1.57e-08,1.6e-06,-9.28e-07,4.67e-06,0.203,0.0109,0.434,0,0,0,0,0,4.83e-05,0.00635,0.00635,0.00103,1.01,1.01,0.202,0.188,0.188,0.118,9.62e-07,9.62e-07,4.87e-07,3.99e-06,3.99e-06,3.89e-06,0,0,0,0,0,0,0,0
-1590000,0.979,-0.00863,-0.0173,0.205,0.0533,-0.00359,-0.228,0.0128,-0.00094,-0.256,-1.42e-06,-2.63e-06,1.57e-08,1.6e-06,-9.28e-07,4.67e-06,0.203,0.0109,0.434,0,0,0,0,0,4.83e-05,0.007,0.007,0.00102,1.3,1.3,0.182,0.268,0.268,0.118,9.62e-07,9.62e-07,4.38e-07,3.99e-06,3.99e-06,3.87e-06,0,0,0,0,0,0,0,0
-1690000,0.979,-0.00854,-0.017,0.205,0.0494,-0.00235,-0.244,0.0101,-0.000622,-0.28,-3.52e-06,-6.61e-06,4.8e-08,3.3e-06,-1.82e-06,4.59e-06,0.203,0.0109,0.434,0,0,0,0,0,4.77e-05,0.00675,0.00675,0.00101,1.1,1.1,0.163,0.179,0.179,0.114,9.06e-07,9.06e-07,3.92e-07,3.98e-06,3.98e-06,3.83e-06,0,0,0,0,0,0,0,0
-1790000,0.979,-0.00865,-0.0173,0.205,0.0604,-0.00313,-0.257,0.0157,-0.000918,-0.305,-3.52e-06,-6.61e-06,4.8e-08,3.3e-06,-1.82e-06,4.59e-06,0.203,0.0109,0.434,0,0,0,0,0,4.75e-05,0.00743,0.00743,0.000993,1.44,1.44,0.147,0.262,0.262,0.11,9.06e-07,9.06e-07,3.51e-07,3.98e-06,3.98e-06,3.8e-06,0,0,0,0,0,0,0,0
-1890000,0.979,-0.00832,-0.0166,0.205,0.0501,7.35e-05,-0.269,0.0114,-0.000469,-0.331,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.63e-05,0.00657,0.00657,0.000975,1.18,1.18,0.137,0.178,0.178,0.109,8.15e-07,8.16e-07,3.15e-07,3.97e-06,3.97e-06,3.76e-06,0,0,0,0,0,0,0,0
-1990000,0.979,-0.0084,-0.0169,0.205,0.0585,0.000588,-0.282,0.0168,-0.000494,-0.359,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.58e-05,0.00722,0.00722,0.000955,1.54,1.54,0.126,0.266,0.266,0.105,8.15e-07,8.16e-07,2.82e-07,3.97e-06,3.97e-06,3.72e-06,0,0,0,0,0,0,0,0
-2090000,0.979,-0.00819,-0.0161,0.205,0.0446,0.0016,-0.296,0.0111,-0.000118,-0.388,-1.04e-05,-2.08e-05,1.73e-07,7.87e-06,-4.05e-06,4.31e-06,0.203,0.0109,0.434,0,0,0,0,0,4.42e-05,0.00588,0.00588,0.000934,1.18,1.18,0.118,0.177,0.177,0.102,7.03e-07,7.03e-07,2.52e-07,3.96e-06,3.96e-06,3.67e-06,0,0,0,0,0,0,0,0
-2190000,0.979,-0.00811,-0.0164,0.205,0.0523,0.00169,-0.293,0.0162,0.000102,-0.4,-1.03e-05,-2.05e-05,1.71e-07,7.17e-06,-3.42e-06,-2.46e-05,0.203,0.0109,0.434,0,0,0,0,0,4.36e-05,0.00645,0.00645,0.000912,1.51,1.51,0.113,0.267,0.267,0.101,7.03e-07,7.03e-07,2.27e-07,3.96e-06,3.96e-06,3.62e-06,0,0,0,0,0,0,0,0
-2290000,0.979,-0.00794,-0.0156,0.205,0.037,0.00274,-0.275,0.0102,0.000239,-0.394,-1.33e-05,-2.77e-05,2.28e-07,7.55e-06,-2.91e-06,-8.4e-05,0.203,0.0109,0.434,0,0,0,0,0,4.18e-05,0.00497,0.00497,0.00089,1.09,1.09,0.107,0.173,0.173,0.0977,5.9e-07,5.91e-07,2.04e-07,3.95e-06,3.95e-06,3.56e-06,0,0,0,0,0,0,0,0
-2390000,0.979,-0.00796,-0.0158,0.205,0.0446,0.00236,-0.263,0.0146,0.000477,-0.391,-1.32e-05,-2.73e-05,2.24e-07,6.19e-06,-1.67e-06,-0.000142,0.203,0.0109,0.434,0,0,0,0,0,4.11e-05,0.00545,0.00545,0.000867,1.38,1.39,0.103,0.259,0.259,0.0949,5.9e-07,5.91e-07,1.84e-07,3.95e-06,3.95e-06,3.49e-06,0,0,0,0,0,0,0,0
+1490000,0.979,-0.00855,-0.017,0.205,0.0434,-0.00331,-0.214,0.00801,-0.000537,-0.234,-1.42e-06,-2.63e-06,1.57e-08,1.6e-06,-9.28e-07,4.67e-06,0.203,0.0109,0.434,0,0,0,0,0,4.83e-05,0.00635,0.00635,0.00103,1.01,1.01,0.202,0.188,0.188,0.118,9.62e-07,9.62e-07,4.87e-07,3.99e-06,3.99e-06,3.89e-06,0,0,0,0,0,0,0,0
+1590000,0.979,-0.00863,-0.0173,0.205,0.0533,-0.00359,-0.228,0.0128,-0.000939,-0.256,-1.42e-06,-2.63e-06,1.57e-08,1.6e-06,-9.28e-07,4.67e-06,0.203,0.0109,0.434,0,0,0,0,0,4.83e-05,0.007,0.007,0.00102,1.3,1.3,0.182,0.268,0.268,0.118,9.62e-07,9.62e-07,4.38e-07,3.99e-06,3.99e-06,3.87e-06,0,0,0,0,0,0,0,0
+1690000,0.979,-0.00854,-0.017,0.205,0.0494,-0.00234,-0.244,0.0101,-0.000621,-0.28,-3.52e-06,-6.61e-06,4.8e-08,3.3e-06,-1.82e-06,4.59e-06,0.203,0.0109,0.434,0,0,0,0,0,4.78e-05,0.00675,0.00675,0.00101,1.1,1.1,0.163,0.179,0.179,0.114,9.06e-07,9.06e-07,3.92e-07,3.98e-06,3.98e-06,3.83e-06,0,0,0,0,0,0,0,0
+1790000,0.979,-0.00865,-0.0173,0.205,0.0604,-0.00312,-0.257,0.0157,-0.000917,-0.305,-3.52e-06,-6.61e-06,4.8e-08,3.3e-06,-1.82e-06,4.59e-06,0.203,0.0109,0.434,0,0,0,0,0,4.75e-05,0.00743,0.00743,0.000993,1.44,1.44,0.147,0.262,0.262,0.11,9.06e-07,9.06e-07,3.51e-07,3.98e-06,3.98e-06,3.8e-06,0,0,0,0,0,0,0,0
+1890000,0.979,-0.00832,-0.0166,0.205,0.0501,7.74e-05,-0.269,0.0114,-0.000468,-0.331,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.63e-05,0.00657,0.00657,0.000975,1.18,1.18,0.137,0.178,0.178,0.109,8.15e-07,8.16e-07,3.15e-07,3.97e-06,3.97e-06,3.76e-06,0,0,0,0,0,0,0,0
+1990000,0.979,-0.00839,-0.0169,0.205,0.0585,0.000592,-0.282,0.0168,-0.000492,-0.359,-6.76e-06,-1.3e-05,1.05e-07,5.58e-06,-2.98e-06,4.46e-06,0.203,0.0109,0.434,0,0,0,0,0,4.58e-05,0.00722,0.00722,0.000955,1.54,1.54,0.126,0.266,0.266,0.105,8.15e-07,8.16e-07,2.82e-07,3.97e-06,3.97e-06,3.72e-06,0,0,0,0,0,0,0,0
+2090000,0.979,-0.00819,-0.0161,0.205,0.0446,0.00161,-0.296,0.0111,-0.000117,-0.388,-1.04e-05,-2.08e-05,1.73e-07,7.87e-06,-4.05e-06,4.31e-06,0.203,0.0109,0.434,0,0,0,0,0,4.42e-05,0.00588,0.00588,0.000934,1.18,1.18,0.118,0.177,0.177,0.102,7.03e-07,7.03e-07,2.52e-07,3.96e-06,3.96e-06,3.67e-06,0,0,0,0,0,0,0,0
+2190000,0.979,-0.00811,-0.0164,0.205,0.0523,0.00169,-0.293,0.0162,0.000104,-0.4,-1.03e-05,-2.05e-05,1.71e-07,7.17e-06,-3.42e-06,-2.46e-05,0.203,0.0109,0.434,0,0,0,0,0,4.36e-05,0.00645,0.00645,0.000912,1.51,1.51,0.113,0.267,0.267,0.101,7.03e-07,7.03e-07,2.27e-07,3.96e-06,3.96e-06,3.62e-06,0,0,0,0,0,0,0,0
+2290000,0.979,-0.00794,-0.0156,0.205,0.037,0.00274,-0.275,0.0102,0.00024,-0.394,-1.33e-05,-2.77e-05,2.28e-07,7.55e-06,-2.91e-06,-8.4e-05,0.203,0.0109,0.434,0,0,0,0,0,4.18e-05,0.00497,0.00497,0.00089,1.09,1.09,0.107,0.173,0.173,0.0977,5.9e-07,5.91e-07,2.04e-07,3.95e-06,3.95e-06,3.56e-06,0,0,0,0,0,0,0,0
+2390000,0.979,-0.00796,-0.0158,0.205,0.0446,0.00236,-0.263,0.0146,0.000478,-0.391,-1.32e-05,-2.73e-05,2.24e-07,6.19e-06,-1.67e-06,-0.000142,0.203,0.0109,0.434,0,0,0,0,0,4.11e-05,0.00545,0.00545,0.000867,1.38,1.39,0.103,0.259,0.259,0.0949,5.9e-07,5.91e-07,1.84e-07,3.95e-06,3.95e-06,3.49e-06,0,0,0,0,0,0,0,0
 2490000,0.979,-0.00769,-0.015,0.205,0.0284,0.00337,-0.259,0.00872,0.000394,-0.4,-1.57e-05,-3.37e-05,2.7e-07,6.44e-06,-1.33e-06,-0.000178,0.203,0.0109,0.434,0,0,0,0,0,3.94e-05,0.00411,0.00411,0.000845,0.963,0.963,0.101,0.166,0.166,0.0946,4.92e-07,4.93e-07,1.66e-07,3.95e-06,3.95e-06,3.43e-06,0,0,0,0,0,0,0,0
-2590000,0.979,-0.00779,-0.0152,0.205,0.0323,0.00208,-0.247,0.012,0.000675,-0.397,-1.56e-05,-3.34e-05,2.68e-07,5.1e-06,-9.65e-08,-0.000238,0.203,0.0109,0.434,0,0,0,0,0,3.86e-05,0.00452,0.00452,0.000823,1.21,1.21,0.0981,0.245,0.245,0.0923,4.92e-07,4.93e-07,1.5e-07,3.95e-06,3.95e-06,3.35e-06,0,0,0,0,0,0,0,0
-2690000,0.979,-0.00772,-0.0146,0.205,0.0208,0.00295,-0.235,0.00706,0.000467,-0.394,-1.73e-05,-3.82e-05,2.99e-07,4.12e-06,1.04e-06,-0.000299,0.203,0.0109,0.434,0,0,0,0,0,3.7e-05,0.00342,0.00342,0.000802,0.836,0.836,0.096,0.157,0.157,0.0903,4.12e-07,4.12e-07,1.36e-07,3.95e-06,3.95e-06,3.26e-06,0,0,0,0,0,0,0,0
-2790000,0.979,-0.00762,-0.0147,0.205,0.0256,0.00351,-0.226,0.00965,0.000813,-0.393,-1.72e-05,-3.79e-05,2.97e-07,2.86e-06,2.2e-06,-0.000356,0.203,0.0109,0.434,0,0,0,0,0,3.62e-05,0.00376,0.00376,0.000781,1.05,1.05,0.0943,0.228,0.228,0.0885,4.12e-07,4.12e-07,1.23e-07,3.95e-06,3.95e-06,3.17e-06,0,0,0,0,0,0,0,0
-2890000,0.979,-0.00759,-0.0143,0.205,0.0181,0.00184,-0.222,0.00574,0.000506,-0.399,-1.85e-05,-4.16e-05,3.18e-07,1.91e-06,3.08e-06,-0.000398,0.203,0.0109,0.434,0,0,0,0,0,3.48e-05,0.0029,0.0029,0.000761,0.727,0.727,0.0942,0.148,0.148,0.0889,3.47e-07,3.47e-07,1.12e-07,3.95e-06,3.95e-06,3.08e-06,0,0,0,0,0,0,0,0
+2590000,0.979,-0.00779,-0.0152,0.205,0.0323,0.00208,-0.247,0.012,0.000676,-0.397,-1.56e-05,-3.34e-05,2.68e-07,5.1e-06,-9.65e-08,-0.000238,0.203,0.0109,0.434,0,0,0,0,0,3.86e-05,0.00452,0.00452,0.000823,1.21,1.21,0.0981,0.245,0.245,0.0923,4.92e-07,4.93e-07,1.5e-07,3.95e-06,3.95e-06,3.35e-06,0,0,0,0,0,0,0,0
+2690000,0.979,-0.00772,-0.0146,0.205,0.0208,0.00295,-0.235,0.00706,0.000468,-0.394,-1.73e-05,-3.82e-05,2.99e-07,4.12e-06,1.04e-06,-0.000299,0.203,0.0109,0.434,0,0,0,0,0,3.7e-05,0.00342,0.00342,0.000802,0.836,0.836,0.096,0.157,0.157,0.0903,4.12e-07,4.12e-07,1.36e-07,3.95e-06,3.95e-06,3.26e-06,0,0,0,0,0,0,0,0
+2790000,0.979,-0.00762,-0.0147,0.205,0.0256,0.00351,-0.226,0.00965,0.000814,-0.393,-1.72e-05,-3.79e-05,2.97e-07,2.86e-06,2.2e-06,-0.000356,0.203,0.0109,0.434,0,0,0,0,0,3.62e-05,0.00376,0.00376,0.000781,1.05,1.05,0.0943,0.228,0.228,0.0885,4.12e-07,4.12e-07,1.23e-07,3.95e-06,3.95e-06,3.17e-06,0,0,0,0,0,0,0,0
+2890000,0.979,-0.00759,-0.0143,0.205,0.0181,0.00184,-0.222,0.00574,0.000507,-0.399,-1.85e-05,-4.16e-05,3.18e-07,1.91e-06,3.08e-06,-0.000398,0.203,0.0109,0.434,0,0,0,0,0,3.48e-05,0.0029,0.0029,0.000761,0.727,0.727,0.0942,0.148,0.148,0.0889,3.47e-07,3.47e-07,1.12e-07,3.95e-06,3.95e-06,3.08e-06,0,0,0,0,0,0,0,0
 2990000,0.979,-0.00756,-0.0144,0.205,0.0208,0.00137,-0.207,0.00795,0.000696,-0.392,-1.84e-05,-4.14e-05,3.17e-07,3.11e-07,4.56e-06,-0.000471,0.203,0.0109,0.434,0,0,0,0,0,3.41e-05,0.00319,0.00319,0.000741,0.904,0.904,0.0929,0.211,0.211,0.0875,3.47e-07,3.47e-07,1.02e-07,3.95e-06,3.95e-06,2.98e-06,0,0,0,0,0,0,0,0
-3090000,0.979,-0.0075,-0.0141,0.205,0.017,-0.000483,-0.203,0.00489,0.000335,-0.395,-1.94e-05,-4.43e-05,3.34e-07,-9.68e-07,5.59e-06,-0.000516,0.203,0.0109,0.434,0,0,0,0,0,3.29e-05,0.00252,0.00252,0.000722,0.638,0.638,0.0919,0.139,0.139,0.0862,2.94e-07,2.94e-07,9.36e-08,3.95e-06,3.95e-06,2.87e-06,0,0,0,0,0,0,0,0
+3090000,0.979,-0.0075,-0.0141,0.205,0.017,-0.000481,-0.203,0.00489,0.000335,-0.395,-1.94e-05,-4.43e-05,3.34e-07,-9.68e-07,5.59e-06,-0.000516,0.203,0.0109,0.434,0,0,0,0,0,3.3e-05,0.00252,0.00252,0.000722,0.638,0.638,0.0919,0.139,0.139,0.0862,2.94e-07,2.94e-07,9.36e-08,3.95e-06,3.95e-06,2.87e-06,0,0,0,0,0,0,0,0
 3190000,0.979,-0.00749,-0.0143,0.205,0.0201,-0.00147,-0.195,0.00684,0.00019,-0.398,-1.94e-05,-4.42e-05,3.34e-07,-2.01e-06,6.55e-06,-0.000563,0.203,0.0109,0.434,0,0,0,0,0,3.23e-05,0.00277,0.00277,0.000704,0.791,0.791,0.092,0.196,0.196,0.087,2.94e-07,2.94e-07,8.58e-08,3.95e-06,3.95e-06,2.78e-06,0,0,0,0,0,0,0,0
-3290000,0.979,-0.00737,-0.0139,0.205,0.0145,-0.00116,-0.183,0.00435,1.62e-06,-0.394,-2.04e-05,-4.65e-05,3.52e-07,-3.87e-06,8.03e-06,-0.000628,0.203,0.0109,0.434,0,0,0,0,0,3.12e-05,0.00222,0.00222,0.000686,0.567,0.567,0.0909,0.131,0.131,0.086,2.49e-07,2.49e-07,7.87e-08,3.94e-06,3.94e-06,2.66e-06,0,0,0,0,0,0,0,0
-3390000,0.979,-0.00721,-0.014,0.205,0.0157,0.000219,-0.174,0.00588,-6.31e-05,-0.391,-2.04e-05,-4.64e-05,3.52e-07,-5.16e-06,9.2e-06,-0.000686,0.203,0.0109,0.434,0,0,0,0,0,3.05e-05,0.00244,0.00244,0.000669,0.702,0.702,0.0898,0.183,0.183,0.0851,2.49e-07,2.49e-07,7.24e-08,3.94e-06,3.94e-06,2.54e-06,0,0,0,0,0,0,0,0
-3490000,0.979,-0.00715,-0.014,0.205,0.0195,0.00301,-0.172,0.00774,9.49e-05,-0.398,-2.04e-05,-4.64e-05,3.51e-07,-5.81e-06,9.79e-06,-0.000716,0.203,0.0109,0.434,0,0,0,0,0,2.99e-05,0.00268,0.00268,0.000653,0.86,0.86,0.0898,0.254,0.254,0.0861,2.49e-07,2.49e-07,6.68e-08,3.94e-06,3.94e-06,2.44e-06,0,0,0,0,0,0,0,0
+3290000,0.979,-0.00737,-0.0139,0.205,0.0145,-0.00116,-0.183,0.00435,1.95e-06,-0.394,-2.04e-05,-4.65e-05,3.52e-07,-3.87e-06,8.03e-06,-0.000628,0.203,0.0109,0.434,0,0,0,0,0,3.12e-05,0.00222,0.00222,0.000686,0.567,0.567,0.0909,0.131,0.131,0.086,2.49e-07,2.49e-07,7.87e-08,3.94e-06,3.94e-06,2.66e-06,0,0,0,0,0,0,0,0
+3390000,0.979,-0.00721,-0.014,0.205,0.0157,0.00022,-0.174,0.00588,-6.27e-05,-0.391,-2.04e-05,-4.64e-05,3.52e-07,-5.16e-06,9.2e-06,-0.000686,0.203,0.0109,0.434,0,0,0,0,0,3.05e-05,0.00244,0.00244,0.000669,0.702,0.702,0.0898,0.183,0.183,0.0851,2.49e-07,2.49e-07,7.24e-08,3.94e-06,3.94e-06,2.54e-06,0,0,0,0,0,0,0,0
+3490000,0.979,-0.00714,-0.014,0.205,0.0195,0.00302,-0.172,0.00774,9.55e-05,-0.398,-2.04e-05,-4.64e-05,3.51e-07,-5.81e-06,9.79e-06,-0.000716,0.203,0.0109,0.434,0,0,0,0,0,2.99e-05,0.00268,0.00268,0.000653,0.86,0.86,0.0898,0.254,0.254,0.0861,2.49e-07,2.49e-07,6.68e-08,3.94e-06,3.94e-06,2.44e-06,0,0,0,0,0,0,0,0
 3590000,0.979,-0.00699,-0.0136,0.205,0.0157,0.00279,-0.167,0.00528,0.00032,-0.399,-2.12e-05,-4.83e-05,3.66e-07,-7.38e-06,1.09e-05,-0.000762,0.203,0.0109,0.434,0,0,0,0,0,2.89e-05,0.00218,0.00218,0.000637,0.632,0.632,0.0884,0.171,0.171,0.0853,2.11e-07,2.11e-07,6.16e-08,3.94e-06,3.94e-06,2.31e-06,0,0,0,0,0,0,0,0
-3690000,0.979,-0.00698,-0.0137,0.205,0.0169,0.00374,-0.156,0.00703,0.00061,-0.396,-2.12e-05,-4.83e-05,3.66e-07,-8.6e-06,1.21e-05,-0.000818,0.203,0.0109,0.434,0,0,0,0,0,2.83e-05,0.00239,0.00239,0.000622,0.773,0.773,0.0868,0.236,0.236,0.0845,2.11e-07,2.11e-07,5.7e-08,3.94e-06,3.94e-06,2.19e-06,0,0,0,0,0,0,0,0
-3790000,0.979,-0.00688,-0.0135,0.205,0.0111,0.00704,-0.152,0.00459,0.00084,-0.398,-2.17e-05,-5e-05,3.75e-07,-1.01e-05,1.3e-05,-0.000855,0.203,0.0109,0.434,0,0,0,0,0,2.75e-05,0.00196,0.00196,0.000607,0.575,0.575,0.0864,0.161,0.161,0.0856,1.78e-07,1.78e-07,5.28e-08,3.94e-06,3.94e-06,2.09e-06,0,0,0,0,0,0,0,0
+3690000,0.979,-0.00698,-0.0137,0.205,0.0169,0.00374,-0.156,0.00703,0.000611,-0.396,-2.12e-05,-4.83e-05,3.66e-07,-8.6e-06,1.21e-05,-0.000818,0.203,0.0109,0.434,0,0,0,0,0,2.83e-05,0.00239,0.00239,0.000622,0.773,0.773,0.0868,0.236,0.236,0.0845,2.11e-07,2.11e-07,5.7e-08,3.94e-06,3.94e-06,2.19e-06,0,0,0,0,0,0,0,0
+3790000,0.979,-0.00687,-0.0135,0.205,0.0111,0.00704,-0.152,0.00459,0.000841,-0.398,-2.17e-05,-5e-05,3.75e-07,-1.01e-05,1.3e-05,-0.000855,0.203,0.0109,0.434,0,0,0,0,0,2.75e-05,0.00196,0.00196,0.000607,0.575,0.575,0.0864,0.161,0.161,0.0856,1.78e-07,1.78e-07,5.28e-08,3.94e-06,3.94e-06,2.09e-06,0,0,0,0,0,0,0,0
 3890000,0.979,-0.00681,-0.0136,0.205,0.0112,0.00817,-0.145,0.0058,0.00164,-0.399,-2.17e-05,-5e-05,3.75e-07,-1.09e-05,1.38e-05,-0.000894,0.203,0.0109,0.434,0,0,0,0,0,2.7e-05,0.00214,0.00214,0.000594,0.702,0.702,0.0846,0.221,0.221,0.0848,1.78e-07,1.78e-07,4.91e-08,3.94e-06,3.94e-06,1.97e-06,0,0,0,0,0,0,0,0
 3990000,0.979,-0.00682,-0.0134,0.205,0.00939,0.00749,-0.14,0.00376,0.00142,-0.397,-2.19e-05,-5.15e-05,3.75e-07,-1.26e-05,1.47e-05,-0.000939,0.203,0.0109,0.434,0,0,0,0,0,2.62e-05,0.00176,0.00176,0.00058,0.528,0.528,0.0826,0.153,0.153,0.0841,1.49e-07,1.49e-07,4.56e-08,3.93e-06,3.93e-06,1.85e-06,0,0,0,0,0,0,0,0
-4090000,0.979,-0.00676,-0.0133,0.205,0.0113,0.00776,-0.129,0.00492,0.00221,-0.393,-2.19e-05,-5.14e-05,3.75e-07,-1.37e-05,1.57e-05,-0.00099,0.203,0.0109,0.434,0,0,0,0,0,2.57e-05,0.00191,0.00191,0.000567,0.642,0.643,0.0806,0.208,0.208,0.0833,1.49e-07,1.49e-07,4.25e-08,3.93e-06,3.93e-06,1.74e-06,0,0,0,0,0,0,0,0
+4090000,0.979,-0.00675,-0.0133,0.205,0.0113,0.00776,-0.129,0.00492,0.00221,-0.393,-2.19e-05,-5.14e-05,3.75e-07,-1.37e-05,1.57e-05,-0.00099,0.203,0.0109,0.434,0,0,0,0,0,2.57e-05,0.00191,0.00191,0.000567,0.642,0.643,0.0806,0.208,0.208,0.0833,1.49e-07,1.49e-07,4.25e-08,3.93e-06,3.93e-06,1.74e-06,0,0,0,0,0,0,0,0
 4190000,0.979,-0.0069,-0.0131,0.205,0.00845,0.00589,-0.128,0.00325,0.00165,-0.4,-2.19e-05,-5.28e-05,3.71e-07,-1.47e-05,1.6e-05,-0.00101,0.203,0.0109,0.434,0,0,0,0,0,2.5e-05,0.00158,0.00158,0.000555,0.486,0.486,0.0795,0.145,0.145,0.0842,1.23e-07,1.23e-07,3.96e-08,3.92e-06,3.92e-06,1.64e-06,0,0,0,0,0,0,0,0
 4290000,0.979,-0.00694,-0.0132,0.205,0.011,0.00743,-0.121,0.00431,0.00232,-0.397,-2.19e-05,-5.28e-05,3.71e-07,-1.56e-05,1.69e-05,-0.00105,0.203,0.0109,0.434,0,0,0,0,0,2.45e-05,0.00171,0.00171,0.000543,0.59,0.59,0.0773,0.196,0.196,0.0834,1.23e-07,1.23e-07,3.7e-08,3.92e-06,3.92e-06,1.54e-06,0,0,0,0,0,0,0,0
 4390000,0.979,-0.00689,-0.013,0.205,0.0102,0.00495,-0.112,0.00307,0.00162,-0.392,-2.18e-05,-5.39e-05,3.65e-07,-1.73e-05,1.77e-05,-0.00109,0.203,0.0109,0.434,0,0,0,0,0,2.39e-05,0.00141,0.00141,0.000532,0.449,0.449,0.0749,0.138,0.138,0.0825,1.02e-07,1.02e-07,3.46e-08,3.92e-06,3.92e-06,1.43e-06,0,0,0,0,0,0,0,0
@@ -49,20 +49,20 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 4690000,0.979,-0.0069,-0.0129,0.205,0.00855,0.00579,-0.0979,0.0039,0.00225,-0.394,-2.16e-05,-5.49e-05,3.6e-07,-1.96e-05,1.91e-05,-0.00117,0.203,0.0109,0.434,0,0,0,0,0,2.25e-05,0.00135,0.00135,0.0005,0.498,0.498,0.0687,0.177,0.177,0.0814,8.3e-08,8.31e-08,2.85e-08,3.91e-06,3.91e-06,1.17e-06,0,0,0,0,0,0,0,0
 4790000,0.979,-0.00681,-0.0128,0.205,0.00171,0.00455,-0.0963,0.00239,0.00159,-0.398,-2.15e-05,-5.58e-05,3.55e-07,-2.06e-05,1.93e-05,-0.00119,0.203,0.0109,0.434,0,0,0,0,0,2.2e-05,0.0011,0.0011,0.00049,0.38,0.38,0.0672,0.126,0.126,0.082,6.74e-08,6.75e-08,2.68e-08,3.9e-06,3.9e-06,1.1e-06,0,0,0,0,0,0,0,0
 4890000,0.979,-0.00674,-0.0128,0.205,0.00197,0.00281,-0.087,0.00259,0.00195,-0.392,-2.15e-05,-5.58e-05,3.55e-07,-2.13e-05,2e-05,-0.00122,0.203,0.0109,0.434,0,0,0,0,0,2.16e-05,0.00119,0.00119,0.000481,0.456,0.456,0.0647,0.168,0.168,0.0809,6.74e-08,6.75e-08,2.53e-08,3.9e-06,3.9e-06,1.02e-06,0,0,0,0,0,0,0,0
-4990000,0.979,-0.00674,-0.0127,0.205,0.00228,0.00292,-0.0813,0.00158,0.00129,-0.392,-2.13e-05,-5.64e-05,3.49e-07,-2.22e-05,2.03e-05,-0.00124,0.203,0.0109,0.434,0,0,0,0,0,2.11e-05,0.000973,0.000973,0.000472,0.349,0.349,0.0622,0.121,0.121,0.0798,5.45e-08,5.46e-08,2.38e-08,3.9e-06,3.9e-06,9.49e-07,0,0,0,0,0,0,0,0
-5090000,0.979,-0.0066,-0.0127,0.205,0.00206,0.00403,-0.0839,0.00181,0.00159,-0.4,-2.13e-05,-5.64e-05,3.49e-07,-2.22e-05,2.03e-05,-0.00124,0.203,0.0109,0.434,0,0,0,0,0,2.07e-05,0.00104,0.00104,0.000463,0.416,0.416,0.0607,0.159,0.159,0.0803,5.45e-08,5.46e-08,2.24e-08,3.9e-06,3.9e-06,8.89e-07,0,0,0,0,0,0,0,0
+4990000,0.979,-0.00673,-0.0127,0.205,0.00228,0.00292,-0.0813,0.00157,0.00129,-0.392,-2.13e-05,-5.64e-05,3.49e-07,-2.22e-05,2.03e-05,-0.00124,0.203,0.0109,0.434,0,0,0,0,0,2.11e-05,0.000973,0.000973,0.000472,0.349,0.349,0.0622,0.121,0.121,0.0798,5.45e-08,5.46e-08,2.38e-08,3.9e-06,3.9e-06,9.49e-07,0,0,0,0,0,0,0,0
+5090000,0.979,-0.0066,-0.0127,0.205,0.00206,0.00403,-0.0839,0.00181,0.0016,-0.4,-2.13e-05,-5.64e-05,3.49e-07,-2.22e-05,2.03e-05,-0.00124,0.203,0.0109,0.434,0,0,0,0,0,2.07e-05,0.00104,0.00104,0.000463,0.416,0.416,0.0607,0.159,0.159,0.0803,5.45e-08,5.46e-08,2.24e-08,3.9e-06,3.9e-06,8.89e-07,0,0,0,0,0,0,0,0
 5190000,0.979,-0.00647,-0.0126,0.205,-0.00151,0.00553,-0.0801,0.00104,0.00119,-0.399,-2.12e-05,-5.68e-05,3.44e-07,-2.3e-05,2.06e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,2.03e-05,0.000856,0.000856,0.000454,0.32,0.32,0.0583,0.115,0.115,0.0791,4.39e-08,4.4e-08,2.12e-08,3.89e-06,3.89e-06,8.26e-07,0,0,0,0,0,0,0,0
-5290000,0.979,-0.00636,-0.0126,0.205,-0.000921,0.00698,-0.0777,0.000963,0.00181,-0.407,-2.12e-05,-5.68e-05,3.44e-07,-2.3e-05,2.06e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,2e-05,0.000914,0.000914,0.000446,0.379,0.379,0.056,0.152,0.152,0.078,4.39e-08,4.4e-08,2e-08,3.89e-06,3.89e-06,7.67e-07,0,0,0,0,0,0,0,0
-5390000,0.979,-0.00633,-0.0126,0.205,-0.00393,0.00825,-0.0811,0.000425,0.00145,-0.415,-2.09e-05,-5.7e-05,3.37e-07,-2.32e-05,2.03e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.96e-05,0.000753,0.000753,0.000438,0.292,0.292,0.0538,0.111,0.111,0.0768,3.54e-08,3.54e-08,1.9e-08,3.88e-06,3.88e-06,7.13e-07,0,0,0,0,0,0,0,0
-5490000,0.979,-0.00631,-0.0126,0.205,-0.00347,0.0114,-0.0811,2.33e-05,0.00242,-0.423,-2.09e-05,-5.7e-05,3.37e-07,-2.32e-05,2.03e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.92e-05,0.000801,0.000801,0.000431,0.345,0.345,0.0523,0.144,0.144,0.0771,3.54e-08,3.54e-08,1.8e-08,3.88e-06,3.88e-06,6.68e-07,0,0,0,0,0,0,0,0
+5290000,0.979,-0.00636,-0.0126,0.206,-0.000922,0.00698,-0.0777,0.000963,0.00181,-0.407,-2.12e-05,-5.68e-05,3.44e-07,-2.3e-05,2.06e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,2e-05,0.000914,0.000914,0.000446,0.379,0.379,0.056,0.152,0.152,0.078,4.39e-08,4.4e-08,2e-08,3.89e-06,3.89e-06,7.67e-07,0,0,0,0,0,0,0,0
+5390000,0.979,-0.00633,-0.0126,0.206,-0.00393,0.00825,-0.0811,0.000425,0.00145,-0.415,-2.09e-05,-5.7e-05,3.37e-07,-2.32e-05,2.03e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.96e-05,0.000753,0.000753,0.000438,0.292,0.292,0.0538,0.111,0.111,0.0768,3.54e-08,3.54e-08,1.9e-08,3.88e-06,3.88e-06,7.13e-07,0,0,0,0,0,0,0,0
+5490000,0.979,-0.00631,-0.0126,0.206,-0.00347,0.0114,-0.0811,2.31e-05,0.00242,-0.423,-2.09e-05,-5.7e-05,3.37e-07,-2.32e-05,2.03e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.92e-05,0.000801,0.000801,0.000431,0.345,0.345,0.0523,0.144,0.144,0.0771,3.54e-08,3.54e-08,1.8e-08,3.88e-06,3.88e-06,6.68e-07,0,0,0,0,0,0,0,0
 5590000,0.979,-0.00631,-0.0126,0.205,-0.00445,0.015,-0.0822,-0.000327,0.00377,-0.431,-2.09e-05,-5.7e-05,3.37e-07,-2.32e-05,2.03e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.89e-05,0.000851,0.000851,0.000423,0.405,0.405,0.0502,0.187,0.187,0.0759,3.54e-08,3.54e-08,1.7e-08,3.88e-06,3.88e-06,6.21e-07,0,0,0,0,0,0,0,0
 5690000,0.979,-0.00636,-0.0126,0.205,-0.00414,0.0155,-0.086,-0.000649,0.00344,-0.439,-2.04e-05,-5.72e-05,3.25e-07,-2.33e-05,1.99e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.85e-05,0.000703,0.000703,0.000416,0.314,0.314,0.0482,0.137,0.137,0.0747,2.85e-08,2.85e-08,1.62e-08,3.88e-06,3.88e-06,5.77e-07,0,0,0,0,0,0,0,0
-5790000,0.979,-0.00622,-0.0125,0.205,-0.00438,0.0179,-0.0875,-0.00107,0.00511,-0.448,-2.04e-05,-5.72e-05,3.25e-07,-2.33e-05,1.99e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.82e-05,0.000745,0.000745,0.000409,0.367,0.367,0.0468,0.177,0.177,0.0749,2.85e-08,2.85e-08,1.53e-08,3.88e-06,3.88e-06,5.42e-07,0,0,0,0,0,0,0,0
-5890000,0.979,-0.00624,-0.0126,0.205,-0.0019,0.0166,-0.0869,-0.000853,0.00444,-0.457,-1.98e-05,-5.73e-05,3.09e-07,-2.34e-05,1.93e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.79e-05,0.000618,0.000619,0.000402,0.285,0.285,0.0449,0.131,0.131,0.0737,2.29e-08,2.29e-08,1.46e-08,3.87e-06,3.87e-06,5.04e-07,0,0,0,0,0,0,0,0
+5790000,0.979,-0.00621,-0.0125,0.205,-0.00438,0.0179,-0.0875,-0.00107,0.00511,-0.448,-2.04e-05,-5.72e-05,3.25e-07,-2.33e-05,1.99e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.82e-05,0.000745,0.000745,0.000409,0.367,0.367,0.0468,0.177,0.177,0.0749,2.85e-08,2.85e-08,1.53e-08,3.88e-06,3.88e-06,5.42e-07,0,0,0,0,0,0,0,0
+5890000,0.979,-0.00624,-0.0126,0.205,-0.0019,0.0166,-0.0869,-0.000854,0.00444,-0.457,-1.98e-05,-5.73e-05,3.09e-07,-2.34e-05,1.93e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.79e-05,0.000618,0.000619,0.000402,0.285,0.285,0.0449,0.131,0.131,0.0737,2.29e-08,2.29e-08,1.46e-08,3.87e-06,3.87e-06,5.04e-07,0,0,0,0,0,0,0,0
 5990000,0.979,-0.00621,-0.0127,0.205,-0.00154,0.0181,-0.0868,-0.000999,0.00615,-0.465,-1.98e-05,-5.73e-05,3.09e-07,-2.34e-05,1.93e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.76e-05,0.000653,0.000653,0.000396,0.332,0.332,0.0431,0.167,0.167,0.0725,2.29e-08,2.29e-08,1.39e-08,3.87e-06,3.87e-06,4.7e-07,0,0,0,0,0,0,0,0
-6090000,0.979,-0.00631,-0.0126,0.205,-0.00193,0.0157,-0.0884,-0.000744,0.00497,-0.474,-1.91e-05,-5.75e-05,2.93e-07,-2.36e-05,1.87e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.73e-05,0.000546,0.000546,0.00039,0.259,0.259,0.0419,0.124,0.124,0.0726,1.85e-08,1.85e-08,1.32e-08,3.87e-06,3.87e-06,4.41e-07,0,0,0,0,0,0,0,0
+6090000,0.979,-0.00631,-0.0126,0.205,-0.00194,0.0157,-0.0885,-0.000744,0.00497,-0.474,-1.91e-05,-5.75e-05,2.93e-07,-2.36e-05,1.87e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.73e-05,0.000546,0.000546,0.00039,0.259,0.259,0.0419,0.124,0.124,0.0726,1.85e-08,1.85e-08,1.32e-08,3.87e-06,3.87e-06,4.41e-07,0,0,0,0,0,0,0,0
 6190000,0.979,-0.00633,-0.0126,0.205,-0.00319,0.0177,-0.0899,-0.000947,0.00666,-0.483,-1.91e-05,-5.75e-05,2.93e-07,-2.36e-05,1.87e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.7e-05,0.000575,0.000575,0.000383,0.3,0.3,0.0402,0.159,0.159,0.0714,1.85e-08,1.85e-08,1.26e-08,3.87e-06,3.87e-06,4.12e-07,0,0,0,0,0,0,0,0
-6290000,0.979,-0.00641,-0.0126,0.205,-0.00474,0.0159,-0.0915,-0.000858,0.00526,-0.492,-1.85e-05,-5.76e-05,2.76e-07,-2.38e-05,1.8e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.67e-05,0.000484,0.000484,0.000378,0.235,0.235,0.0385,0.119,0.119,0.0703,1.49e-08,1.5e-08,1.2e-08,3.87e-06,3.87e-06,3.84e-07,0,0,0,0,0,0,0,0
+6290000,0.979,-0.00641,-0.0126,0.205,-0.00475,0.0159,-0.0915,-0.000858,0.00526,-0.492,-1.85e-05,-5.76e-05,2.76e-07,-2.38e-05,1.8e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.67e-05,0.000484,0.000484,0.000378,0.235,0.235,0.0385,0.119,0.119,0.0703,1.49e-08,1.5e-08,1.2e-08,3.87e-06,3.87e-06,3.84e-07,0,0,0,0,0,0,0,0
 6390000,0.979,-0.00635,-0.0125,0.205,-0.00313,0.0175,-0.0933,-0.00127,0.00693,-0.501,-1.85e-05,-5.76e-05,2.76e-07,-2.38e-05,1.8e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.65e-05,0.000508,0.000508,0.000372,0.272,0.272,0.0375,0.15,0.15,0.0704,1.49e-08,1.5e-08,1.15e-08,3.87e-06,3.87e-06,3.62e-07,0,0,0,0,0,0,0,0
 6490000,0.979,-0.00642,-0.0125,0.205,-0.00506,0.0124,-0.0944,-0.00109,0.00526,-0.511,-1.78e-05,-5.78e-05,2.61e-07,-2.39e-05,1.74e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.62e-05,0.000431,0.000431,0.000366,0.214,0.214,0.036,0.113,0.113,0.0692,1.21e-08,1.21e-08,1.09e-08,3.86e-06,3.86e-06,3.38e-07,0,0,0,0,0,0,0,0
 6590000,0.979,-0.00637,-0.0125,0.205,-0.0063,0.0146,-0.0978,-0.00162,0.00655,-0.52,-1.78e-05,-5.78e-05,2.61e-07,-2.39e-05,1.74e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.6e-05,0.000451,0.000451,0.000361,0.246,0.246,0.0345,0.143,0.143,0.0681,1.21e-08,1.21e-08,1.05e-08,3.86e-06,3.86e-06,3.16e-07,0,0,0,0,0,0,0,0
@@ -71,12 +71,12 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 6890000,0.979,-0.00633,-0.0123,0.205,-0.00685,0.011,-0.103,-0.002,0.00482,-0.551,-1.68e-05,-5.79e-05,2.34e-07,-2.4e-05,1.62e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.52e-05,0.000347,0.000347,0.000346,0.177,0.177,0.0311,0.103,0.103,0.0659,8.07e-09,8.07e-09,9.16e-09,3.86e-06,3.86e-06,2.62e-07,0,0,0,0,0,0,0,0
 6990000,0.979,-0.00628,-0.0123,0.205,-0.00731,0.0117,-0.104,-0.00275,0.00595,-0.561,-1.68e-05,-5.79e-05,2.34e-07,-2.4e-05,1.62e-05,-0.00126,0.203,0.0109,0.434,0,0,0,0,0,1.5e-05,0.000362,0.000362,0.000341,0.203,0.203,0.0299,0.129,0.129,0.0648,8.07e-09,8.07e-09,8.78e-09,3.86e-06,3.86e-06,2.46e-07,0,0,0,0,0,0,0,0
 7090000,0.982,-0.00648,-0.0121,0.188,-0.00737,0.0134,-0.106,-0.00225,0.00469,-0.572,-1.63e-05,-5.79e-05,2.22e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,9.14e-05,0.000314,0.000314,0.00244,0.16,0.16,0.0291,0.0991,0.0991,0.0649,6.62e-09,6.63e-09,8.49e-09,3.86e-06,3.86e-06,2.33e-07,0,0,0,0,0,0,0,0
-7190000,0.982,-0.00644,-0.0122,0.188,-0.00814,0.0147,-0.106,-0.00306,0.00616,-0.582,-1.63e-05,-5.79e-05,2.15e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,4.82e-05,0.000314,0.000314,0.00128,0.161,0.161,0.028,0.122,0.122,0.0638,6.62e-09,6.63e-09,8.49e-09,3.86e-06,3.86e-06,2.19e-07,0,0,0,0,0,0,0,0
+7190000,0.982,-0.00644,-0.0122,0.188,-0.00815,0.0147,-0.106,-0.00306,0.00616,-0.582,-1.63e-05,-5.79e-05,2.15e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,4.82e-05,0.000314,0.000314,0.00128,0.161,0.161,0.028,0.122,0.122,0.0638,6.62e-09,6.63e-09,8.49e-09,3.86e-06,3.86e-06,2.19e-07,0,0,0,0,0,0,0,0
 7290000,0.982,-0.00642,-0.0121,0.188,-0.00769,0.0181,-0.108,-0.00391,0.00776,-0.593,-1.63e-05,-5.79e-05,2.39e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,3.28e-05,0.000315,0.000315,0.000872,0.167,0.167,0.027,0.148,0.148,0.0628,6.62e-09,6.62e-09,8.49e-09,3.86e-06,3.86e-06,2.06e-07,0,0,0,0,0,0,0,0
 7390000,0.982,-0.0063,-0.0121,0.188,-0.00979,0.0203,-0.109,-0.00478,0.00973,-0.604,-1.63e-05,-5.79e-05,2.53e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.56e-05,0.000316,0.000315,0.000681,0.176,0.176,0.0263,0.177,0.177,0.0628,6.62e-09,6.62e-09,8.48e-09,3.86e-06,3.86e-06,1.96e-07,0,0,0,0,0,0,0,0
-7490000,0.982,-0.00631,-0.0122,0.187,-0.00782,0.0224,-0.109,-0.00562,0.0119,-0.615,-1.63e-05,-5.79e-05,3.44e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.05e-05,0.000317,0.000317,0.000545,0.188,0.188,0.0253,0.21,0.21,0.0618,6.61e-09,6.62e-09,8.48e-09,3.86e-06,3.86e-06,1.85e-07,0,0,0,0,0,0,0,0
+7490000,0.982,-0.00631,-0.0122,0.187,-0.00782,0.0224,-0.109,-0.00563,0.0119,-0.615,-1.63e-05,-5.79e-05,3.44e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.05e-05,0.000317,0.000317,0.000545,0.188,0.188,0.0253,0.21,0.21,0.0618,6.61e-09,6.62e-09,8.48e-09,3.86e-06,3.86e-06,1.85e-07,0,0,0,0,0,0,0,0
 7590000,0.982,-0.00639,-0.0121,0.187,-0.00719,0.0246,-0.11,-0.00638,0.0142,-0.626,-1.63e-05,-5.79e-05,3.53e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,1.72e-05,0.000318,0.000318,0.000455,0.205,0.205,0.0244,0.247,0.247,0.0609,6.61e-09,6.62e-09,8.48e-09,3.86e-06,3.86e-06,1.74e-07,0,0,0,0,0,0,0,0
-7690000,0.982,-0.00641,-0.0122,0.187,-0.00789,0.0277,-0.114,-0.00713,0.0168,-0.637,-1.63e-05,-5.79e-05,3.31e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,1.5e-05,0.00032,0.00032,0.000398,0.225,0.225,0.0239,0.289,0.289,0.0608,6.61e-09,6.62e-09,8.47e-09,3.86e-06,3.86e-06,1.66e-07,0,0,0,0,0,0,0,0
+7690000,0.982,-0.00641,-0.0122,0.187,-0.0079,0.0277,-0.114,-0.00714,0.0168,-0.637,-1.63e-05,-5.79e-05,3.31e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,1.5e-05,0.00032,0.00032,0.000398,0.225,0.225,0.0239,0.289,0.289,0.0608,6.61e-09,6.62e-09,8.47e-09,3.86e-06,3.86e-06,1.66e-07,0,0,0,0,0,0,0,0
 7790000,0.982,-0.00629,-0.0122,0.187,-0.00667,0.0293,-0.116,-0.00784,0.0196,-0.648,-1.63e-05,-5.79e-05,2.72e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,1.32e-05,0.000322,0.000322,0.000348,0.249,0.249,0.023,0.335,0.335,0.0599,6.6e-09,6.61e-09,8.46e-09,3.86e-06,3.86e-06,1.57e-07,0,0,0,0,0,0,0,0
 7890000,0.982,-0.00631,-0.0123,0.187,-0.0084,0.033,-0.117,-0.00867,0.0227,-0.66,-1.63e-05,-5.79e-05,2.4e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,1.18e-05,0.000325,0.000325,0.00031,0.277,0.277,0.0222,0.388,0.389,0.059,6.6e-09,6.61e-09,8.45e-09,3.86e-06,3.86e-06,1.49e-07,0,0,0,0,0,0,0,0
 7990000,0.982,-0.00625,-0.0122,0.187,-0.00851,0.0351,-0.116,-0.00949,0.0259,-0.672,-1.63e-05,-5.79e-05,2.62e-07,-2.4e-05,1.56e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,1.06e-05,0.000328,0.000328,0.00028,0.308,0.308,0.0215,0.447,0.447,0.0581,6.59e-09,6.59e-09,8.44e-09,3.86e-06,3.86e-06,1.41e-07,0,0,0,0,0,0,0,0
@@ -85,18 +85,18 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 8290000,0.982,-0.00616,-0.0121,0.188,-0.00542,0.0471,-0.119,-0.0116,0.038,-0.707,-1.63e-05,-5.79e-05,-2.37e-07,-2.4e-05,1.57e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,8.44e-06,0.000338,0.000338,0.000222,0.425,0.425,0.0197,0.677,0.677,0.0564,6.57e-09,6.57e-09,8.37e-09,3.86e-06,3.86e-06,1.21e-07,0,0,0,0,0,0,0,0
 8390000,0.982,-0.00611,-0.0121,0.188,-0.00798,0.0484,-0.12,-0.0121,0.0424,-0.719,-1.62e-05,-5.79e-05,-2.89e-07,-2.4e-05,1.58e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,7.98e-06,0.000341,0.000341,0.000209,0.47,0.47,0.0193,0.771,0.771,0.0564,6.54e-09,6.54e-09,8.35e-09,3.85e-06,3.86e-06,1.16e-07,0,0,0,0,0,0,0,0
 8490000,0.982,-0.00601,-0.0121,0.188,-0.00863,0.0521,-0.121,-0.013,0.0474,-0.731,-1.62e-05,-5.79e-05,-5.22e-08,-2.4e-05,1.58e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,7.53e-06,0.000346,0.000346,0.000197,0.521,0.521,0.0187,0.883,0.883,0.0556,6.54e-09,6.54e-09,8.32e-09,3.85e-06,3.86e-06,1.1e-07,0,0,0,0,0,0,0,0
-8590000,0.982,-0.00598,-0.0122,0.188,-0.00795,0.0543,-0.12,-0.0137,0.0523,-0.743,-1.62e-05,-5.79e-05,-1.68e-07,-2.4e-05,1.6e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,7.13e-06,0.000349,0.000349,0.000187,0.572,0.572,0.0181,1,1,0.0548,6.5e-09,6.51e-09,8.28e-09,3.85e-06,3.85e-06,1.05e-07,0,0,0,0,0,0,0,0
+8590000,0.982,-0.00598,-0.0122,0.188,-0.00795,0.0543,-0.12,-0.0137,0.0522,-0.743,-1.62e-05,-5.79e-05,-1.68e-07,-2.4e-05,1.6e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,7.13e-06,0.000349,0.000349,0.000187,0.572,0.572,0.0181,1,1,0.0548,6.5e-09,6.51e-09,8.28e-09,3.85e-06,3.85e-06,1.05e-07,0,0,0,0,0,0,0,0
 8690000,0.982,-0.006,-0.0123,0.187,-0.00841,0.0562,-0.123,-0.0146,0.0578,-0.755,-1.62e-05,-5.79e-05,-8.68e-08,-2.4e-05,1.6e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,6.85e-06,0.000354,0.000354,0.000179,0.631,0.631,0.0177,1.14,1.14,0.0548,6.5e-09,6.51e-09,8.24e-09,3.85e-06,3.85e-06,1.01e-07,0,0,0,0,0,0,0,0
-8790000,0.982,-0.00598,-0.0122,0.188,-0.00733,0.0584,-0.125,-0.0153,0.0627,-0.767,-1.62e-05,-5.79e-05,-3.43e-07,-2.4e-05,1.62e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,6.55e-06,0.000358,0.000358,0.000172,0.687,0.687,0.0172,1.29,1.29,0.0541,6.46e-09,6.47e-09,8.2e-09,3.85e-06,3.85e-06,9.63e-08,0,0,0,0,0,0,0,0
+8790000,0.982,-0.00598,-0.0122,0.188,-0.00733,0.0584,-0.125,-0.0153,0.0627,-0.767,-1.62e-05,-5.79e-05,-3.43e-07,-2.4e-05,1.62e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,6.56e-06,0.000358,0.000358,0.000172,0.687,0.687,0.0172,1.29,1.29,0.0541,6.46e-09,6.47e-09,8.2e-09,3.85e-06,3.85e-06,9.63e-08,0,0,0,0,0,0,0,0
 8890000,0.982,-0.00603,-0.0122,0.187,-0.00777,0.0615,-0.125,-0.016,0.0688,-0.78,-1.62e-05,-5.79e-05,-5.23e-08,-2.4e-05,1.62e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,6.3e-06,0.000363,0.000364,0.000165,0.754,0.754,0.0167,1.47,1.47,0.0533,6.46e-09,6.47e-09,8.14e-09,3.85e-06,3.85e-06,9.2e-08,0,0,0,0,0,0,0,0
 8990000,0.982,-0.00596,-0.0122,0.187,-0.00898,0.065,-0.124,-0.0166,0.0739,-0.792,-1.61e-05,-5.79e-05,3.46e-07,-2.39e-05,1.66e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,6.13e-06,0.000366,0.000366,0.00016,0.814,0.814,0.0164,1.64,1.64,0.0533,6.41e-09,6.42e-09,8.1e-09,3.85e-06,3.85e-06,8.84e-08,0,0,0,0,0,0,0,0
 9090000,0.982,-0.00595,-0.0122,0.187,-0.00912,0.0695,-0.127,-0.0175,0.0806,-0.805,-1.61e-05,-5.79e-05,8.08e-07,-2.39e-05,1.66e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.93e-06,0.000373,0.000373,0.000155,0.888,0.888,0.0159,1.86,1.86,0.0526,6.41e-09,6.42e-09,8.03e-09,3.85e-06,3.85e-06,8.46e-08,0,0,0,0,0,0,0,0
 9190000,0.982,-0.00596,-0.0123,0.187,-0.0058,0.0708,-0.127,-0.0179,0.086,-0.818,-1.6e-05,-5.79e-05,1.21e-06,-2.38e-05,1.72e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.77e-06,0.000375,0.000375,0.000151,0.95,0.95,0.0155,2.07,2.07,0.0519,6.35e-09,6.36e-09,7.97e-09,3.84e-06,3.84e-06,8.1e-08,0,0,0,0,0,0,0,0
 9290000,0.982,-0.00581,-0.0121,0.187,-0.00399,0.0736,-0.128,-0.0183,0.0932,-0.831,-1.6e-05,-5.79e-05,1.34e-06,-2.38e-05,1.72e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.62e-06,0.000381,0.000381,0.000147,1.03,1.03,0.0151,2.34,2.34,0.0512,6.35e-09,6.36e-09,7.9e-09,3.84e-06,3.84e-06,7.77e-08,0,0,0,0,0,0,0,0
 9390000,0.982,-0.00573,-0.0121,0.187,-0.00411,0.0746,-0.128,-0.0182,0.0981,-0.843,-1.59e-05,-5.79e-05,8.93e-07,-2.36e-05,1.8e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.53e-06,0.000382,0.000382,0.000145,1.09,1.09,0.0148,2.58,2.58,0.0512,6.28e-09,6.28e-09,7.83e-09,3.84e-06,3.84e-06,7.49e-08,0,0,0,0,0,0,0,0
-9490000,0.982,-0.00575,-0.0122,0.187,-0.00469,0.0765,-0.129,-0.0187,0.106,-0.856,-1.59e-05,-5.8e-05,1.08e-06,-2.36e-05,1.8e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.42e-06,0.000389,0.000389,0.000142,1.18,1.18,0.0144,2.91,2.91,0.0506,6.28e-09,6.28e-09,7.75e-09,3.84e-06,3.84e-06,7.19e-08,0,0,0,0,0,0,0,0
+9490000,0.982,-0.00575,-0.0122,0.187,-0.00469,0.0765,-0.129,-0.0187,0.106,-0.856,-1.59e-05,-5.8e-05,1.08e-06,-2.36e-05,1.8e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.42e-06,0.000389,0.000389,0.000142,1.18,1.18,0.0144,2.91,2.91,0.0506,6.28e-09,6.29e-09,7.75e-09,3.84e-06,3.84e-06,7.19e-08,0,0,0,0,0,0,0,0
 9590000,0.982,-0.00582,-0.0122,0.187,-0.00493,0.076,-0.13,-0.0186,0.11,-0.869,-1.59e-05,-5.8e-05,1.59e-07,-2.34e-05,1.91e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.32e-06,0.000388,0.000388,0.000139,1.24,1.24,0.014,3.17,3.17,0.05,6.2e-09,6.2e-09,7.66e-09,3.83e-06,3.83e-06,6.91e-08,0,0,0,0,0,0,0,0
-9690000,0.982,-0.00582,-0.0121,0.187,-0.00548,0.0785,-0.128,-0.0191,0.117,-0.882,-1.59e-05,-5.8e-05,-8.9e-08,-2.34e-05,1.91e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.27e-06,0.000395,0.000395,0.000138,1.33,1.33,0.0138,3.56,3.56,0.05,6.2e-09,6.2e-09,7.58e-09,3.83e-06,3.83e-06,6.68e-08,0,0,0,0,0,0,0,0
+9690000,0.982,-0.00582,-0.0121,0.187,-0.00548,0.0785,-0.128,-0.0191,0.117,-0.882,-1.59e-05,-5.8e-05,-8.91e-08,-2.34e-05,1.91e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.27e-06,0.000395,0.000395,0.000138,1.33,1.33,0.0138,3.56,3.56,0.05,6.2e-09,6.2e-09,7.58e-09,3.83e-06,3.83e-06,6.68e-08,0,0,0,0,0,0,0,0
 9790000,0.982,-0.00579,-0.0121,0.187,-0.00462,0.0816,-0.131,-0.0196,0.125,-0.895,-1.59e-05,-5.79e-05,-1.02e-06,-2.35e-05,1.92e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.2e-06,0.000403,0.000403,0.000136,1.43,1.43,0.0135,3.99,3.99,0.0493,6.2e-09,6.2e-09,7.48e-09,3.83e-06,3.83e-06,6.42e-08,0,0,0,0,0,0,0,0
 9890000,0.982,-0.00579,-0.012,0.187,-0.00241,0.0807,-0.13,-0.0192,0.128,-0.908,-1.58e-05,-5.8e-05,-8.34e-07,-2.31e-05,2.05e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,5.13e-06,0.0004,0.0004,0.000134,1.48,1.48,0.0131,4.29,4.29,0.0487,6.11e-09,6.11e-09,7.38e-09,3.82e-06,3.82e-06,6.19e-08,0,0,0,0,0,0,0,0
 9990000,0.982,-0.00574,-0.0121,0.188,-0.00111,0.0851,-0.0989,-0.0191,0.141,-0.849,-1.58e-05,-5.79e-05,-1.61e-06,-2.38e-05,1.92e-05,-0.00131,0.204,0.00201,0.435,0,0,0,0,0,5.11e-06,0.000408,0.000408,0.000134,1.59,1.59,0.0129,4.79,4.79,0.0488,6.11e-09,6.11e-09,7.29e-09,3.82e-06,3.82e-06,5.99e-08,0,0,0,0,0,0,0,0
@@ -108,7 +108,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 10590000,0.982,-0.00546,-0.0119,0.188,-0.00216,0.00452,0.0811,-0.00118,-0.00544,-0.499,-1.56e-05,-5.78e-05,-3.51e-06,-2.31e-05,1.48e-05,-0.00154,0.204,0.00201,0.435,0,0,0,0,0,4.95e-06,0.000423,0.000423,0.00013,0.13,0.13,0.169,0.131,0.131,0.0486,5.89e-09,5.89e-09,6.6e-09,3.77e-06,3.77e-06,5e-08,0,0,0,0,0,0,0,0
 10690000,0.982,-0.00539,-0.012,0.188,-0.00152,0.00539,0.124,-0.00136,-0.00494,-0.448,-1.56e-05,-5.77e-05,-3.85e-06,-2.32e-05,1.41e-05,-0.00155,0.204,0.00201,0.435,0,0,0,0,0,4.96e-06,0.000432,0.000433,0.00013,0.139,0.139,0.164,0.137,0.137,0.054,5.89e-09,5.89e-09,6.49e-09,3.77e-06,3.77e-06,5e-08,0,0,0,0,0,0,0,0
 10790000,0.982,-0.00546,-0.0121,0.188,0.000355,0.00222,0.135,-0.000823,-0.00466,-0.405,-1.54e-05,-5.78e-05,-4.01e-06,-2.23e-05,1.69e-05,-0.00157,0.204,0.00201,0.435,0,0,0,0,0,4.94e-06,0.000427,0.000428,0.00013,0.0961,0.0961,0.123,0.0913,0.0913,0.0538,5.83e-09,5.84e-09,6.36e-09,3.75e-06,3.75e-06,5e-08,0,0,0,0,0,0,0,0
-10890000,0.982,-0.00542,-0.0122,0.188,-2.72e-05,0.00548,0.17,-0.000799,-0.00432,-0.357,-1.54e-05,-5.78e-05,-3.14e-06,-2.24e-05,1.65e-05,-0.00158,0.204,0.00201,0.435,0,0,0,0,0,4.93e-06,0.000437,0.000437,0.00013,0.108,0.108,0.116,0.0974,0.0974,0.0583,5.83e-09,5.84e-09,6.23e-09,3.75e-06,3.75e-06,5e-08,0,0,0,0,0,0,0,0
+10890000,0.982,-0.00542,-0.0122,0.188,-2.71e-05,0.00548,0.17,-0.000799,-0.00432,-0.357,-1.54e-05,-5.78e-05,-3.14e-06,-2.24e-05,1.65e-05,-0.00158,0.204,0.00201,0.435,0,0,0,0,0,4.93e-06,0.000437,0.000437,0.00013,0.108,0.108,0.116,0.0974,0.0974,0.0583,5.83e-09,5.84e-09,6.23e-09,3.75e-06,3.75e-06,5e-08,0,0,0,0,0,0,0,0
 10990000,0.982,-0.00545,-0.0123,0.188,-0.00035,0.0117,0.174,-0.000548,-0.00303,-0.323,-1.53e-05,-5.78e-05,-3.53e-06,-2.2e-05,1.79e-05,-0.00159,0.204,0.00201,0.435,0,0,0,0,0,4.94e-06,0.000419,0.000419,0.00013,0.0849,0.0849,0.0921,0.0728,0.0728,0.0579,5.72e-09,5.73e-09,6.11e-09,3.72e-06,3.72e-06,5e-08,0,0,0,0,0,0,0,0
 11090000,0.982,-0.00557,-0.0123,0.188,0.000205,0.0165,0.205,-0.000592,-0.00167,-0.274,-1.53e-05,-5.78e-05,-4.65e-06,-2.21e-05,1.77e-05,-0.0016,0.204,0.00201,0.435,0,0,0,0,0,4.93e-06,0.000429,0.000429,0.00013,0.0999,0.0999,0.0863,0.0791,0.0791,0.0614,5.72e-09,5.73e-09,5.98e-09,3.72e-06,3.72e-06,5e-08,0,0,0,0,0,0,0,0
 11190000,0.982,-0.00578,-0.0123,0.188,0.00185,0.0168,0.207,0.000862,-0.0017,-0.243,-1.49e-05,-5.77e-05,-5.28e-06,-2.3e-05,2.44e-05,-0.0016,0.204,0.00201,0.435,0,0,0,0,0,4.91e-06,0.000398,0.000398,0.00013,0.0823,0.0823,0.0701,0.0627,0.0627,0.0596,5.56e-09,5.56e-09,5.85e-09,3.68e-06,3.68e-06,5e-08,0,0,0,0,0,0,0,0
@@ -131,11 +131,11 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 12890000,0.982,-0.00709,-0.0118,0.188,0.0093,-0.00198,0.172,0.00492,-0.0039,-0.03,-1.18e-05,-5.88e-05,-3.11e-06,-9.75e-06,5.67e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.78e-06,0.000212,0.000212,0.000128,0.0728,0.0728,0.013,0.0568,0.0568,0.0504,4.03e-09,4.04e-09,3.79e-09,3.47e-06,3.47e-06,5e-08,0,0,0,0,0,0,0,0
 12990000,0.982,-0.00707,-0.0117,0.187,0.00752,-0.000567,0.167,0.00348,-0.00299,-0.0311,-1.19e-05,-5.9e-05,-2.38e-06,-9.31e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.76e-06,0.000199,0.000199,0.000127,0.0598,0.0598,0.012,0.0485,0.0485,0.0488,3.9e-09,3.9e-09,3.68e-09,3.47e-06,3.47e-06,5e-08,0,0,0,0,0,0,0,0
 13090000,0.982,-0.00709,-0.0116,0.187,0.00841,-0.000178,0.162,0.00426,-0.00297,-0.0293,-1.19e-05,-5.91e-05,-1.45e-06,-9.36e-06,5.63e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.74e-06,0.000205,0.000205,0.000127,0.0683,0.0683,0.0113,0.0565,0.0565,0.0481,3.9e-09,3.9e-09,3.58e-09,3.47e-06,3.47e-06,5.01e-08,0,0,0,0,0,0,0,0
-13190000,0.982,-0.00712,-0.0115,0.187,0.00364,-0.00222,0.156,0.000927,-0.00401,-0.0314,-1.18e-05,-5.94e-05,-8.43e-07,-8.67e-06,5.61e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.71e-06,0.000193,0.000193,0.000126,0.0565,0.0565,0.0105,0.0484,0.0484,0.0467,3.76e-09,3.76e-09,3.47e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
+13190000,0.982,-0.00712,-0.0115,0.187,0.00364,-0.00222,0.156,0.000927,-0.00401,-0.0314,-1.18e-05,-5.94e-05,-8.43e-07,-8.66e-06,5.61e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.71e-06,0.000193,0.000193,0.000126,0.0565,0.0565,0.0105,0.0484,0.0484,0.0467,3.76e-09,3.76e-09,3.47e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13290000,0.982,-0.00714,-0.0116,0.187,0.00329,-0.00289,0.152,0.00122,-0.00424,-0.0287,-1.18e-05,-5.94e-05,-7.39e-07,-8.76e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.71e-06,0.000199,0.000199,0.000126,0.0643,0.0643,0.0101,0.0562,0.0562,0.0465,3.76e-09,3.76e-09,3.39e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13390000,0.982,-0.00708,-0.0117,0.187,0.00251,-0.00135,0.148,0.000831,-0.00327,-0.0311,-1.19e-05,-5.94e-05,-1.02e-06,-9.1e-06,5.63e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.68e-06,0.00019,0.00019,0.000125,0.0536,0.0536,0.00943,0.0482,0.0482,0.0452,3.63e-09,3.63e-09,3.29e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13490000,0.982,-0.00706,-0.0116,0.187,0.00298,0.000544,0.145,0.00112,-0.00324,-0.0338,-1.19e-05,-5.94e-05,-7.29e-07,-9.25e-06,5.64e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.66e-06,0.000196,0.000196,0.000125,0.0607,0.0607,0.00904,0.0559,0.0559,0.0445,3.63e-09,3.63e-09,3.19e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
-13590000,0.982,-0.00707,-0.0117,0.187,0.00735,-1.5e-05,0.143,0.00393,-0.00275,-0.0368,-1.18e-05,-5.91e-05,-9.06e-07,-8.96e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.66e-06,0.000187,0.000187,0.000125,0.0509,0.0509,0.00862,0.0481,0.0481,0.0439,3.49e-09,3.5e-09,3.11e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
+13590000,0.982,-0.00707,-0.0117,0.187,0.00735,-1.5e-05,0.143,0.00393,-0.00275,-0.0368,-1.18e-05,-5.91e-05,-9.05e-07,-8.96e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.66e-06,0.000187,0.000187,0.000125,0.0509,0.0509,0.00862,0.0481,0.0481,0.0439,3.49e-09,3.5e-09,3.11e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13690000,0.982,-0.00702,-0.0116,0.187,0.00735,-0.00124,0.142,0.00465,-0.00281,-0.0336,-1.18e-05,-5.91e-05,-3.87e-07,-9.06e-06,5.62e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.63e-06,0.000193,0.000193,0.000124,0.0576,0.0576,0.00832,0.0556,0.0556,0.0432,3.49e-09,3.5e-09,3.02e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13790000,0.982,-0.00697,-0.0118,0.187,0.0142,0.00254,0.138,0.00814,-0.000595,-0.0374,-1.19e-05,-5.87e-05,-5.78e-07,-7.59e-06,5.66e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.6e-06,0.000186,0.000186,0.000123,0.0487,0.0487,0.00794,0.0479,0.0479,0.0421,3.36e-09,3.36e-09,2.93e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
 13890000,0.982,-0.00688,-0.0117,0.187,0.0153,0.00344,0.138,0.00959,-0.00029,-0.0338,-1.18e-05,-5.87e-05,5.07e-08,-7.71e-06,5.66e-05,-0.00162,0.204,0.00201,0.435,0,0,0,0,0,4.59e-06,0.000191,0.000191,0.000123,0.055,0.055,0.00777,0.0553,0.0553,0.0419,3.36e-09,3.36e-09,2.85e-09,3.46e-06,3.46e-06,5.01e-08,0,0,0,0,0,0,0,0
@@ -146,7 +146,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 14390000,0.982,-0.00696,-0.0112,0.187,0.0116,-0.00279,0.125,0.00836,-0.00271,-0.0408,-1.16e-05,-5.94e-05,-6.25e-07,-1.23e-05,5.58e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.47e-06,0.000183,0.000183,0.00012,0.0437,0.0437,0.00689,0.0473,0.0473,0.0385,2.93e-09,2.94e-09,2.47e-09,3.45e-06,3.45e-06,5e-08,0,0,0,0,0,0,0,0
 14490000,0.982,-0.00707,-0.0112,0.187,0.0101,-0.00237,0.128,0.00939,-0.00298,-0.0397,-1.16e-05,-5.94e-05,-1.26e-06,-1.25e-05,5.6e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.44e-06,0.000188,0.000188,0.000119,0.0492,0.0492,0.00683,0.0543,0.0543,0.038,2.94e-09,2.94e-09,2.4e-09,3.45e-06,3.45e-06,5e-08,0,0,0,0,0,0,0,0
 14590000,0.982,-0.00714,-0.011,0.187,0.00796,-0.00265,0.123,0.00589,-0.00379,-0.0461,-1.17e-05,-5.99e-05,-1.33e-06,-1.7e-05,5.65e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.43e-06,0.000182,0.000182,0.000118,0.0425,0.0425,0.00675,0.0471,0.0471,0.0376,2.79e-09,2.79e-09,2.34e-09,3.44e-06,3.44e-06,5e-08,0,0,0,0,0,0,0,0
-14690000,0.982,-0.00713,-0.011,0.187,0.00716,-0.00247,0.122,0.00662,-0.00403,-0.0474,-1.17e-05,-5.99e-05,-1.04e-06,-1.74e-05,5.67e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.39e-06,0.000187,0.000187,0.000118,0.048,0.048,0.00673,0.054,0.054,0.0372,2.79e-09,2.79e-09,2.27e-09,3.44e-06,3.44e-06,5e-08,0,0,0,0,0,0,0,0
+14690000,0.982,-0.00713,-0.011,0.187,0.00716,-0.00247,0.122,0.00662,-0.00403,-0.0474,-1.17e-05,-5.99e-05,-1.04e-06,-1.74e-05,5.67e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.4e-06,0.000187,0.000187,0.000118,0.048,0.048,0.00673,0.054,0.054,0.0372,2.79e-09,2.79e-09,2.27e-09,3.44e-06,3.44e-06,5e-08,0,0,0,0,0,0,0,0
 14790000,0.982,-0.00706,-0.011,0.187,0.00889,0.00413,0.12,0.00529,0.0011,-0.0485,-1.24e-05,-5.98e-05,-2.58e-07,-1.68e-05,6.35e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.36e-06,0.000181,0.000181,0.000117,0.0416,0.0416,0.00665,0.0469,0.0469,0.0365,2.63e-09,2.64e-09,2.2e-09,3.43e-06,3.43e-06,5e-08,0,0,0,0,0,0,0,0
 14890000,0.982,-0.00697,-0.0109,0.187,0.00755,0.00188,0.123,0.00606,0.00141,-0.0479,-1.24e-05,-5.99e-05,3.09e-07,-1.72e-05,6.37e-05,-0.00161,0.204,0.00201,0.435,0,0,0,0,0,4.35e-06,0.000186,0.000186,0.000116,0.0469,0.0469,0.0067,0.0537,0.0537,0.0365,2.63e-09,2.64e-09,2.15e-09,3.43e-06,3.43e-06,5e-08,0,0,0,0,0,0,0,0
 14990000,0.982,-0.00712,-0.0108,0.187,0.00629,0.000129,0.123,0.00475,-0.000331,-0.0499,-1.22e-05,-6.02e-05,6.56e-07,-2.05e-05,6.25e-05,-0.0016,0.204,0.00201,0.435,0,0,0,0,0,4.32e-06,0.00018,0.00018,0.000116,0.0408,0.0408,0.00664,0.0467,0.0467,0.0359,2.48e-09,2.48e-09,2.09e-09,3.41e-06,3.41e-06,5e-08,0,0,0,0,0,0,0,0
@@ -164,11 +164,11 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 16190000,0.982,-0.00725,-0.0106,0.187,-0.00201,-0.00325,0.0995,0.00249,-0.00292,-0.0851,-1.23e-05,-6.1e-05,1.39e-06,-3.8e-05,6.99e-05,-0.00156,0.204,0.00201,0.435,0,0,0,0,0,3.99e-06,0.000164,0.000164,0.000107,0.0379,0.0379,0.00723,0.046,0.046,0.0334,1.58e-09,1.58e-09,1.51e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0
 16290000,0.982,-0.00727,-0.0105,0.187,-0.00182,-0.00463,0.0979,0.00225,-0.00335,-0.0867,-1.23e-05,-6.1e-05,1.56e-06,-3.88e-05,7.04e-05,-0.00155,0.204,0.00201,0.435,0,0,0,0,0,3.96e-06,0.000168,0.000168,0.000106,0.0426,0.0426,0.00734,0.0525,0.0525,0.0334,1.58e-09,1.58e-09,1.46e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0
 16390000,0.982,-0.00723,-0.0105,0.186,0.000586,-0.0041,0.0957,0.0033,-0.00256,-0.0913,-1.24e-05,-6.08e-05,1.95e-06,-3.7e-05,7.17e-05,-0.00155,0.204,0.00201,0.435,0,0,0,0,0,3.93e-06,0.000161,0.000161,0.000105,0.0375,0.0375,0.00736,0.0459,0.0459,0.0331,1.44e-09,1.45e-09,1.43e-09,3.27e-06,3.28e-06,5e-08,0,0,0,0,0,0,0,0
-16490000,0.982,-0.00735,-0.0105,0.186,0.00249,-0.0056,0.0969,0.00347,-0.0031,-0.089,-1.24e-05,-6.08e-05,1.81e-06,-3.73e-05,7.18e-05,-0.00154,0.204,0.00201,0.435,0,0,0,0,0,3.91e-06,0.000164,0.000164,0.000105,0.0421,0.0421,0.0075,0.0524,0.0524,0.0334,1.44e-09,1.45e-09,1.39e-09,3.27e-06,3.28e-06,5e-08,0,0,0,0,0,0,0,0
+16490000,0.982,-0.00735,-0.0105,0.186,0.00249,-0.0056,0.0969,0.00347,-0.0031,-0.089,-1.24e-05,-6.08e-05,1.81e-06,-3.73e-05,7.18e-05,-0.00154,0.204,0.00201,0.435,0,0,0,0,0,3.91e-06,0.000164,0.000164,0.000105,0.0421,0.0421,0.0075,0.0524,0.0524,0.0334,1.45e-09,1.45e-09,1.39e-09,3.27e-06,3.28e-06,5e-08,0,0,0,0,0,0,0,0
 16590000,0.982,-0.00731,-0.0105,0.186,0.00657,-0.00581,0.0981,0.00305,-0.0024,-0.0936,-1.25e-05,-6.08e-05,1.83e-06,-3.86e-05,7.45e-05,-0.00154,0.204,0.00201,0.435,0,0,0,0,0,3.88e-06,0.000157,0.000157,0.000104,0.037,0.037,0.00751,0.0459,0.0459,0.0332,1.32e-09,1.32e-09,1.35e-09,3.25e-06,3.25e-06,5e-08,0,0,0,0,0,0,0,0
 16690000,0.982,-0.00736,-0.0104,0.186,0.00787,-0.0103,0.0965,0.00376,-0.00318,-0.0963,-1.25e-05,-6.09e-05,2.07e-06,-3.96e-05,7.52e-05,-0.00153,0.204,0.00201,0.435,0,0,0,0,0,3.85e-06,0.00016,0.00016,0.000103,0.0415,0.0415,0.00763,0.0523,0.0523,0.0332,1.32e-09,1.32e-09,1.32e-09,3.25e-06,3.25e-06,5e-08,0,0,0,0,0,0,0,0
 16790000,0.982,-0.00718,-0.0103,0.186,0.00874,-0.00925,0.0931,0.00289,-0.00227,-0.1,-1.28e-05,-6.1e-05,2.24e-06,-4.21e-05,7.98e-05,-0.00153,0.204,0.00201,0.435,0,0,0,0,0,3.83e-06,0.000153,0.000153,0.000103,0.0365,0.0365,0.00767,0.0458,0.0458,0.0333,1.2e-09,1.2e-09,1.29e-09,3.23e-06,3.23e-06,5e-08,0,0,0,0,0,0,0,0
-16890000,0.982,-0.00715,-0.0103,0.186,0.00751,-0.00956,0.0924,0.00366,-0.00317,-0.105,-1.28e-05,-6.1e-05,2.73e-06,-4.34e-05,8.07e-05,-0.00152,0.204,0.00201,0.435,0,0,0,0,0,3.79e-06,0.000156,0.000156,0.000102,0.0409,0.0409,0.00778,0.0522,0.0522,0.0333,1.2e-09,1.2e-09,1.25e-09,3.23e-06,3.23e-06,5e-08,0,0,0,0,0,0,0,0
+16890000,0.982,-0.00715,-0.0103,0.186,0.00751,-0.00956,0.0924,0.00366,-0.00317,-0.105,-1.28e-05,-6.1e-05,2.73e-06,-4.34e-05,8.07e-05,-0.00152,0.204,0.00201,0.435,0,0,0,0,0,3.8e-06,0.000156,0.000156,0.000102,0.0409,0.0409,0.00778,0.0522,0.0522,0.0333,1.2e-09,1.2e-09,1.25e-09,3.23e-06,3.23e-06,5e-08,0,0,0,0,0,0,0,0
 16990000,0.982,-0.00716,-0.0104,0.186,0.00722,-0.00906,0.0896,0.00354,-0.00234,-0.112,-1.3e-05,-6.08e-05,2.84e-06,-4.18e-05,8.33e-05,-0.00152,0.204,0.00201,0.435,0,0,0,0,0,3.76e-06,0.000149,0.000149,0.000101,0.036,0.036,0.00778,0.0457,0.0457,0.0332,1.09e-09,1.09e-09,1.22e-09,3.21e-06,3.21e-06,5e-08,0,0,0,0,0,0,0,0
 17090000,0.982,-0.00725,-0.0103,0.186,0.00859,-0.0116,0.0874,0.00433,-0.00342,-0.116,-1.3e-05,-6.08e-05,2.72e-06,-4.27e-05,8.38e-05,-0.00151,0.204,0.00201,0.435,0,0,0,0,0,3.73e-06,0.000152,0.000152,0.0001,0.0402,0.0402,0.00789,0.0521,0.0521,0.0332,1.09e-09,1.09e-09,1.19e-09,3.21e-06,3.21e-06,5e-08,0,0,0,0,0,0,0,0
 17190000,0.982,-0.00737,-0.0102,0.186,0.00754,-0.0169,0.0868,0.00281,-0.00703,-0.117,-1.28e-05,-6.1e-05,2.2e-06,-4.52e-05,8.2e-05,-0.00151,0.204,0.00201,0.435,0,0,0,0,0,3.72e-06,0.000145,0.000145,0.0001,0.0354,0.0354,0.00791,0.0457,0.0457,0.0334,9.89e-10,9.89e-10,1.16e-09,3.19e-06,3.19e-06,5e-08,0,0,0,0,0,0,0,0
@@ -179,7 +179,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 17690000,0.982,-0.00718,-0.01,0.186,-0.00195,-0.0143,0.0755,0.00162,-0.00671,-0.137,-1.37e-05,-6.09e-05,2.04e-06,-4.89e-05,9.79e-05,-0.00148,0.204,0.00201,0.435,0,0,0,0,0,3.58e-06,0.00014,0.00014,9.65e-05,0.038,0.038,0.0082,0.0518,0.0518,0.0337,8.11e-10,8.11e-10,1.03e-09,3.15e-06,3.15e-06,5e-08,0,0,0,0,0,0,0,0
 17790000,0.982,-0.00713,-0.01,0.186,0.00092,-0.0127,0.0737,0.0028,-0.00569,-0.136,-1.4e-05,-6.06e-05,2.08e-06,-4.51e-05,0.000102,-0.00147,0.204,0.00201,0.435,0,0,0,0,0,3.56e-06,0.000134,0.000134,9.62e-05,0.0335,0.0335,0.00821,0.0455,0.0455,0.0339,7.34e-10,7.34e-10,1.01e-09,3.13e-06,3.13e-06,5e-08,0,0,0,0,0,0,0,0
 17890000,0.983,-0.00706,-0.0101,0.186,0.00291,-0.0145,0.0716,0.00303,-0.00706,-0.136,-1.4e-05,-6.06e-05,2.2e-06,-4.58e-05,0.000103,-0.00147,0.204,0.00201,0.435,0,0,0,0,0,3.53e-06,0.000136,0.000136,9.54e-05,0.0372,0.0372,0.00829,0.0517,0.0517,0.034,7.34e-10,7.34e-10,9.81e-10,3.13e-06,3.13e-06,5e-08,0,0,0,0,0,0,0,0
-17990000,0.983,-0.00684,-0.0101,0.186,0.00245,-0.00801,0.0687,0.00239,-0.00147,-0.141,-1.46e-05,-6.04e-05,2.15e-06,-4.39e-05,0.000113,-0.00147,0.204,0.00201,0.435,0,0,0,0,0,3.5e-06,0.000131,0.000131,9.46e-05,0.0328,0.0328,0.00825,0.0454,0.0454,0.0339,6.63e-10,6.64e-10,9.57e-10,3.12e-06,3.12e-06,5e-08,0,0,0,0,0,0,0,0
+17990000,0.983,-0.00684,-0.0101,0.186,0.00245,-0.00801,0.0687,0.00239,-0.00147,-0.141,-1.46e-05,-6.04e-05,2.15e-06,-4.39e-05,0.000113,-0.00147,0.204,0.00201,0.435,0,0,0,0,0,3.5e-06,0.000131,0.000131,9.46e-05,0.0328,0.0328,0.00825,0.0454,0.0454,0.0339,6.64e-10,6.64e-10,9.57e-10,3.12e-06,3.12e-06,5e-08,0,0,0,0,0,0,0,0
 18090000,0.983,-0.00692,-0.0101,0.186,0.00174,-0.00844,0.0665,0.00263,-0.00232,-0.147,-1.46e-05,-6.04e-05,2.09e-06,-4.49e-05,0.000114,-0.00146,0.204,0.00201,0.435,0,0,0,0,0,3.48e-06,0.000133,0.000133,9.43e-05,0.0364,0.0364,0.00837,0.0515,0.0515,0.0343,6.64e-10,6.64e-10,9.37e-10,3.12e-06,3.12e-06,5e-08,0,0,0,0,0,0,0,0
 18190000,0.983,-0.00692,-0.0101,0.186,0.00185,-0.00749,0.0644,0.00334,-0.00165,-0.154,-1.47e-05,-6.03e-05,2.57e-06,-4.41e-05,0.000116,-0.00145,0.204,0.00201,0.435,0,0,0,0,0,3.45e-06,0.000127,0.000127,9.35e-05,0.0321,0.0321,0.00832,0.0453,0.0453,0.0342,6e-10,6e-10,9.14e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0
 18290000,0.983,-0.00695,-0.0101,0.186,0.00252,-0.00787,0.0614,0.00349,-0.00239,-0.16,-1.47e-05,-6.03e-05,2.58e-06,-4.51e-05,0.000117,-0.00145,0.204,0.00201,0.435,0,0,0,0,0,3.43e-06,0.000129,0.000129,9.28e-05,0.0356,0.0356,0.00839,0.0514,0.0514,0.0343,6e-10,6e-10,8.92e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0
@@ -208,8 +208,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 20590000,0.982,-0.0066,-0.0102,0.186,-0.0132,-0.00243,0.0212,0.00589,-0.000237,-0.275,-1.51e-05,-5.93e-05,9.66e-07,-4.71e-05,0.000135,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,2.96e-06,0.000101,0.0001,8e-05,0.0239,0.0239,0.00821,0.0436,0.0436,0.0355,1.98e-10,1.98e-10,5.44e-10,2.99e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
 20690000,0.982,-0.00651,-0.0102,0.186,-0.0151,-0.00114,0.0214,0.00449,-0.000362,-0.278,-1.51e-05,-5.93e-05,7.37e-07,-4.76e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.95e-06,0.000101,0.000101,7.97e-05,0.0261,0.0261,0.00829,0.0489,0.0489,0.0359,1.98e-10,1.98e-10,5.34e-10,2.99e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
 20790000,0.982,-0.00591,-0.0102,0.186,-0.0174,0.00133,0.00537,0.00381,-0.000264,-0.283,-1.51e-05,-5.92e-05,8.08e-07,-4.67e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.93e-06,9.92e-05,9.91e-05,7.91e-05,0.0234,0.0234,0.0082,0.0434,0.0434,0.0356,1.83e-10,1.83e-10,5.23e-10,2.98e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
-20890000,0.982,0.00321,-0.00615,0.186,-0.0238,0.0134,-0.114,0.00169,0.000517,-0.293,-1.51e-05,-5.92e-05,8.03e-07,-4.69e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.9e-06,9.98e-05,9.98e-05,7.86e-05,0.0258,0.0258,0.00823,0.0487,0.0487,0.0357,1.83e-10,1.83e-10,5.13e-10,2.98e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
-20990000,0.982,0.00653,-0.00273,0.186,-0.0345,0.0313,-0.254,0.00122,0.00104,-0.311,-1.49e-05,-5.92e-05,7.54e-07,-4.47e-05,0.000132,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.88e-06,9.78e-05,9.77e-05,7.8e-05,0.0234,0.0234,0.00814,0.0433,0.0433,0.0354,1.69e-10,1.69e-10,5.03e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
+20890000,0.982,0.00321,-0.00615,0.186,-0.0238,0.0134,-0.114,0.00169,0.000517,-0.293,-1.51e-05,-5.92e-05,8.04e-07,-4.69e-05,0.000135,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.9e-06,9.98e-05,9.98e-05,7.86e-05,0.0258,0.0258,0.00823,0.0487,0.0487,0.0357,1.83e-10,1.83e-10,5.13e-10,2.98e-06,2.99e-06,5e-08,0,0,0,0,0,0,0,0
+20990000,0.982,0.00653,-0.00273,0.186,-0.0345,0.0313,-0.254,0.00122,0.00104,-0.311,-1.49e-05,-5.92e-05,7.54e-07,-4.47e-05,0.000132,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.89e-06,9.78e-05,9.77e-05,7.8e-05,0.0234,0.0234,0.00814,0.0433,0.0433,0.0354,1.69e-10,1.69e-10,5.03e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
 21090000,0.982,0.00494,-0.00311,0.187,-0.0482,0.0474,-0.373,-0.00291,0.00502,-0.345,-1.49e-05,-5.92e-05,7.37e-07,-4.48e-05,0.000132,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.88e-06,9.84e-05,9.83e-05,7.77e-05,0.0258,0.0258,0.00822,0.0485,0.0485,0.0358,1.69e-10,1.69e-10,4.94e-10,2.98e-06,2.98e-06,5e-08,0,0,0,0,0,0,0,0
 21190000,0.982,0.00206,-0.00479,0.187,-0.0517,0.0513,-0.5,-0.00221,0.00397,-0.384,-1.46e-05,-5.9e-05,7.96e-07,-3.99e-05,0.000122,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,2.87e-06,9.62e-05,9.62e-05,7.72e-05,0.0234,0.0234,0.00813,0.0432,0.0432,0.0356,1.56e-10,1.56e-10,4.84e-10,2.97e-06,2.97e-06,5e-08,0,0,0,0,0,0,0,0
 21290000,0.982,-0.000107,-0.00612,0.187,-0.0521,0.055,-0.631,-0.0074,0.00931,-0.446,-1.46e-05,-5.9e-05,4.98e-07,-4e-05,0.000122,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.85e-06,9.68e-05,9.68e-05,7.66e-05,0.0257,0.0257,0.00816,0.0483,0.0483,0.0356,1.56e-10,1.56e-10,4.75e-10,2.97e-06,2.97e-06,5e-08,0,0,0,0,0,0,0,0
@@ -224,7 +224,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 22190000,0.982,-0.00579,-0.00887,0.187,-0.00629,0.0139,-1.38,0.00479,0.0134,-1.5,-1.39e-05,-5.84e-05,1.44e-06,-2.65e-05,0.000111,-0.00135,0.204,0.00201,0.435,0,0,0,0,0,2.73e-06,8.73e-05,8.73e-05,7.28e-05,0.0214,0.0214,0.00802,0.0427,0.0427,0.0355,1.08e-10,1.08e-10,4.02e-10,2.91e-06,2.92e-06,5e-08,0,0,0,0,0,0,0,0
 22290000,0.982,-0.0065,-0.00912,0.187,-0.00118,0.00872,-1.38,0.0044,0.0145,-1.65,-1.39e-05,-5.84e-05,1.34e-06,-2.71e-05,0.000112,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.71e-06,8.77e-05,8.76e-05,7.23e-05,0.0231,0.0231,0.00806,0.0476,0.0476,0.0355,1.08e-10,1.08e-10,3.95e-10,2.91e-06,2.92e-06,5e-08,0,0,0,0,0,0,0,0
 22390000,0.982,-0.00686,-0.00928,0.187,0.00414,-0.000935,-1.38,0.0118,0.00462,-1.79,-1.38e-05,-5.85e-05,1.14e-06,-3.13e-05,0.000111,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.71e-06,8.58e-05,8.58e-05,7.2e-05,0.0206,0.0206,0.00802,0.0425,0.0425,0.0356,1.01e-10,1.01e-10,3.88e-10,2.9e-06,2.91e-06,5e-08,0,0,0,0,0,0,0,0
-22490000,0.982,-0.007,-0.00974,0.187,0.008,-0.00688,-1.39,0.0124,0.00419,-1.93,-1.38e-05,-5.85e-05,9.92e-07,-3.15e-05,0.000111,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.69e-06,8.62e-05,8.61e-05,7.15e-05,0.0222,0.0222,0.00806,0.0474,0.0474,0.0356,1.01e-10,1.01e-10,3.81e-10,2.9e-06,2.91e-06,5e-08,0,0,0,0,0,0,0,0
+22490000,0.982,-0.007,-0.00974,0.187,0.008,-0.00689,-1.39,0.0124,0.00419,-1.93,-1.38e-05,-5.85e-05,9.92e-07,-3.15e-05,0.000111,-0.00134,0.204,0.00201,0.435,0,0,0,0,0,2.69e-06,8.62e-05,8.61e-05,7.15e-05,0.0222,0.0222,0.00806,0.0474,0.0474,0.0356,1.01e-10,1.01e-10,3.81e-10,2.9e-06,2.91e-06,5e-08,0,0,0,0,0,0,0,0
 22590000,0.982,-0.00693,-0.0104,0.187,0.0176,-0.0159,-1.38,0.0245,-0.00484,-2.08,-1.37e-05,-5.84e-05,1.16e-06,-2.88e-05,0.00011,-0.00133,0.204,0.00201,0.435,0,0,0,0,0,2.68e-06,8.45e-05,8.44e-05,7.11e-05,0.0199,0.0199,0.00799,0.0423,0.0423,0.0353,9.46e-11,9.47e-11,3.75e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 22690000,0.982,-0.00684,-0.0106,0.188,0.0197,-0.0203,-1.39,0.0264,-0.00669,-2.22,-1.37e-05,-5.84e-05,1.07e-06,-2.94e-05,0.000111,-0.00133,0.204,0.00201,0.435,0,0,0,0,0,2.67e-06,8.48e-05,8.48e-05,7.08e-05,0.0215,0.0215,0.00807,0.0471,0.0471,0.0357,9.47e-11,9.48e-11,3.69e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 22790000,0.982,-0.00684,-0.011,0.187,0.026,-0.0282,-1.39,0.0367,-0.0168,-2.37,-1.36e-05,-5.84e-05,9.31e-07,-3.16e-05,0.000112,-0.00133,0.204,0.00201,0.435,0,0,0,0,0,2.65e-06,8.33e-05,8.33e-05,7.04e-05,0.0193,0.0193,0.00799,0.0421,0.0421,0.0354,8.92e-11,8.92e-11,3.62e-10,2.89e-06,2.89e-06,5e-08,0,0,0,0,0,0,0,0
@@ -245,11 +245,11 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 24290000,0.983,-0.00994,-0.0143,0.185,0.0783,-0.075,0.0531,0.146,-0.107,-3.94,-1.41e-05,-5.84e-05,1.53e-06,-3.25e-05,0.000116,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.41e-06,7.93e-05,7.93e-05,6.51e-05,0.0164,0.0164,0.00802,0.0442,0.0442,0.0355,6.36e-11,6.36e-11,2.83e-10,2.87e-06,2.87e-06,5e-08,0,0,0,0,0,0,0,0
 24390000,0.983,-0.00909,-0.0133,0.185,0.0718,-0.0694,0.0696,0.152,-0.107,-3.93,-1.42e-05,-5.84e-05,1.72e-06,-3.2e-05,0.000107,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.39e-06,7.92e-05,7.91e-05,6.47e-05,0.0151,0.0151,0.00795,0.0399,0.0399,0.0353,6.11e-11,6.12e-11,2.79e-10,2.86e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
 24490000,0.983,-0.00933,-0.0134,0.185,0.0672,-0.0664,0.0675,0.159,-0.114,-3.93,-1.42e-05,-5.84e-05,1.97e-06,-3.18e-05,0.000107,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.38e-06,7.94e-05,7.93e-05,6.43e-05,0.0163,0.0163,0.00799,0.0439,0.0439,0.0353,6.12e-11,6.13e-11,2.74e-10,2.86e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
-24590000,0.983,-0.01,-0.0135,0.185,0.0638,-0.0622,0.0634,0.163,-0.111,-3.92,-1.44e-05,-5.84e-05,1.95e-06,-3.4e-05,9.94e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.38e-06,7.93e-05,7.92e-05,6.41e-05,0.015,0.015,0.00797,0.0396,0.0396,0.0354,5.89e-11,5.9e-11,2.7e-10,2.86e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
+24590000,0.983,-0.01,-0.0135,0.185,0.0638,-0.0622,0.0634,0.163,-0.111,-3.92,-1.44e-05,-5.84e-05,1.95e-06,-3.4e-05,9.94e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.38e-06,7.93e-05,7.92e-05,6.41e-05,0.015,0.015,0.00797,0.0396,0.0396,0.0354,5.89e-11,5.89e-11,2.7e-10,2.86e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
 24690000,0.983,-0.0105,-0.0133,0.185,0.0616,-0.0616,0.0629,0.169,-0.117,-3.91,-1.44e-05,-5.84e-05,1.95e-06,-3.4e-05,9.94e-05,-0.00126,0.204,0.00201,0.435,0,0,0,0,0,2.37e-06,7.95e-05,7.94e-05,6.38e-05,0.0161,0.0161,0.00801,0.0436,0.0436,0.0354,5.9e-11,5.9e-11,2.66e-10,2.86e-06,2.86e-06,5e-08,0,0,0,0,0,0,0,0
 24790000,0.983,-0.0105,-0.0126,0.185,0.0583,-0.0592,0.0546,0.171,-0.113,-3.91,-1.46e-05,-5.84e-05,1.98e-06,-3.42e-05,9.31e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.35e-06,7.94e-05,7.94e-05,6.34e-05,0.0149,0.0149,0.00794,0.0394,0.0394,0.0352,5.69e-11,5.69e-11,2.62e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 24890000,0.983,-0.0104,-0.0121,0.185,0.0567,-0.0625,0.0441,0.177,-0.119,-3.91,-1.46e-05,-5.84e-05,2.06e-06,-3.44e-05,9.34e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.34e-06,7.96e-05,7.96e-05,6.3e-05,0.016,0.016,0.00799,0.0434,0.0434,0.0352,5.7e-11,5.7e-11,2.58e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
-24990000,0.983,-0.0101,-0.0119,0.185,0.0477,-0.058,0.037,0.177,-0.111,-3.91,-1.49e-05,-5.83e-05,2.05e-06,-3.62e-05,8.56e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.33e-06,7.96e-05,7.96e-05,6.28e-05,0.0148,0.0148,0.00796,0.0393,0.0393,0.0353,5.5e-11,5.5e-11,2.54e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
+24990000,0.983,-0.0101,-0.0119,0.185,0.0477,-0.058,0.037,0.177,-0.111,-3.91,-1.49e-05,-5.83e-05,2.05e-06,-3.63e-05,8.56e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.33e-06,7.96e-05,7.96e-05,6.28e-05,0.0148,0.0148,0.00796,0.0393,0.0393,0.0353,5.5e-11,5.5e-11,2.54e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 25090000,0.983,-0.0105,-0.012,0.185,0.0442,-0.0572,0.0344,0.182,-0.116,-3.91,-1.49e-05,-5.83e-05,2.01e-06,-3.63e-05,8.6e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.32e-06,7.98e-05,7.98e-05,6.25e-05,0.0159,0.0159,0.00801,0.0432,0.0432,0.0353,5.51e-11,5.51e-11,2.5e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 25190000,0.983,-0.0107,-0.0118,0.185,0.0393,-0.0505,0.0344,0.182,-0.108,-3.91,-1.51e-05,-5.83e-05,1.9e-06,-3.88e-05,8.06e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.31e-06,7.98e-05,7.98e-05,6.21e-05,0.0147,0.0147,0.00794,0.0391,0.0391,0.0351,5.32e-11,5.32e-11,2.47e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
 25290000,0.983,-0.0108,-0.0111,0.185,0.0344,-0.0517,0.0292,0.185,-0.113,-3.91,-1.51e-05,-5.83e-05,1.73e-06,-3.86e-05,8.05e-05,-0.00125,0.204,0.00201,0.435,0,0,0,0,0,2.3e-06,8e-05,8e-05,6.19e-05,0.0158,0.0158,0.00803,0.043,0.043,0.0355,5.33e-11,5.33e-11,2.43e-10,2.85e-06,2.85e-06,5e-08,0,0,0,0,0,0,0,0
@@ -272,31 +272,31 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 26990000,0.983,-0.00701,-0.0102,0.186,-0.0575,0.0231,0.00608,0.0884,-0.0542,-3.93,-1.61e-05,-5.85e-05,5.03e-07,-3.15e-05,4.81e-05,-0.00122,0.204,0.00201,0.435,0,0,0,0,0,2.12e-06,8.15e-05,8.15e-05,5.72e-05,0.0138,0.0138,0.00799,0.0383,0.0383,0.0352,4.12e-11,4.12e-11,1.92e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
 27090000,0.983,-0.00686,-0.0105,0.186,-0.0596,0.0302,0.00875,0.0825,-0.0515,-3.93,-1.61e-05,-5.85e-05,4.4e-07,-3.12e-05,4.75e-05,-0.00122,0.204,0.00201,0.435,0,0,0,0,0,2.11e-06,8.17e-05,8.16e-05,5.69e-05,0.0148,0.0148,0.00803,0.042,0.042,0.0353,4.13e-11,4.13e-11,1.89e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
 27190000,0.983,-0.00689,-0.0104,0.186,-0.0659,0.0356,0.0106,0.0716,-0.0455,-3.94,-1.6e-05,-5.85e-05,3.69e-07,-3.03e-05,4.61e-05,-0.00122,0.204,0.00201,0.435,0,0,0,0,0,2.1e-06,8.16e-05,8.16e-05,5.67e-05,0.0137,0.0137,0.00801,0.0382,0.0382,0.0354,4.03e-11,4.02e-11,1.87e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
-27290000,0.983,-0.00707,-0.0114,0.186,-0.0723,0.0415,0.124,0.0648,-0.0416,-3.94,-1.6e-05,-5.85e-05,3.23e-07,-3.02e-05,4.58e-05,-0.00122,0.204,0.00201,0.435,0,0,0,0,0,2.09e-06,8.18e-05,8.18e-05,5.64e-05,0.0146,0.0146,0.00805,0.0419,0.0419,0.0354,4.04e-11,4.03e-11,1.85e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
-27390000,0.982,-0.00845,-0.0139,0.186,-0.0769,0.0472,0.447,0.0553,-0.0345,-3.92,-1.6e-05,-5.84e-05,2.75e-07,-3.01e-05,4.35e-05,-0.00121,0.204,0.00201,0.435,0,0,0,0,0,2.09e-06,8.18e-05,8.17e-05,5.61e-05,0.0133,0.0133,0.00798,0.0381,0.0381,0.0352,3.94e-11,3.94e-11,1.82e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
+27290000,0.983,-0.00707,-0.0114,0.186,-0.0723,0.0415,0.124,0.0648,-0.0416,-3.94,-1.6e-05,-5.85e-05,3.24e-07,-3.02e-05,4.58e-05,-0.00122,0.204,0.00201,0.435,0,0,0,0,0,2.09e-06,8.18e-05,8.18e-05,5.64e-05,0.0146,0.0146,0.00805,0.0419,0.0419,0.0354,4.04e-11,4.03e-11,1.85e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
+27390000,0.982,-0.00845,-0.0139,0.186,-0.0769,0.0472,0.447,0.0553,-0.0345,-3.92,-1.6e-05,-5.84e-05,2.75e-07,-3.01e-05,4.35e-05,-0.00121,0.204,0.00201,0.435,0,0,0,0,0,2.09e-06,8.18e-05,8.17e-05,5.61e-05,0.0133,0.0133,0.00798,0.0381,0.0381,0.0352,3.94e-11,3.93e-11,1.82e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
 27490000,0.982,-0.0098,-0.0157,0.186,-0.0795,0.0526,0.759,0.0475,-0.0295,-3.86,-1.6e-05,-5.84e-05,9.68e-08,-3.05e-05,4.34e-05,-0.00121,0.204,0.00201,0.435,0,0,0,0,0,2.08e-06,8.2e-05,8.19e-05,5.59e-05,0.0141,0.0141,0.00803,0.0417,0.0417,0.0352,3.95e-11,3.94e-11,1.8e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
-27590000,0.982,-0.00968,-0.0145,0.186,-0.0733,0.055,0.853,0.0392,-0.0254,-3.79,-1.59e-05,-5.84e-05,6.21e-08,-3.15e-05,4.37e-05,-0.0012,0.204,0.00201,0.435,0,0,0,0,0,2.07e-06,8.2e-05,8.2e-05,5.57e-05,0.0131,0.0131,0.008,0.038,0.038,0.0353,3.86e-11,3.86e-11,1.78e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
-27690000,0.983,-0.00845,-0.0116,0.186,-0.0703,0.0517,0.755,0.032,-0.02,-3.72,-1.59e-05,-5.84e-05,4.81e-08,-3.1e-05,4.28e-05,-0.00119,0.204,0.00201,0.435,0,0,0,0,0,2.05e-06,8.22e-05,8.22e-05,5.54e-05,0.0141,0.0141,0.00804,0.0416,0.0416,0.0354,3.87e-11,3.87e-11,1.75e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
+27590000,0.982,-0.00968,-0.0145,0.186,-0.0733,0.055,0.853,0.0392,-0.0254,-3.79,-1.59e-05,-5.84e-05,6.22e-08,-3.15e-05,4.37e-05,-0.0012,0.204,0.00201,0.435,0,0,0,0,0,2.07e-06,8.2e-05,8.2e-05,5.57e-05,0.0131,0.0131,0.008,0.038,0.038,0.0353,3.86e-11,3.86e-11,1.78e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
+27690000,0.983,-0.00845,-0.0116,0.186,-0.0703,0.0517,0.755,0.032,-0.02,-3.72,-1.59e-05,-5.84e-05,4.82e-08,-3.1e-05,4.28e-05,-0.00119,0.204,0.00201,0.435,0,0,0,0,0,2.05e-06,8.22e-05,8.22e-05,5.54e-05,0.0141,0.0141,0.00804,0.0416,0.0416,0.0354,3.87e-11,3.87e-11,1.75e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
 27790000,0.983,-0.00712,-0.0102,0.186,-0.0696,0.0494,0.749,0.0258,-0.0176,-3.65,-1.59e-05,-5.84e-05,4.45e-08,-3.34e-05,4.91e-05,-0.00119,0.204,0.00201,0.435,0,0,0,0,0,2.04e-06,8.23e-05,8.22e-05,5.52e-05,0.0131,0.0131,0.00797,0.0379,0.0379,0.0351,3.78e-11,3.78e-11,1.73e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
-27890000,0.983,-0.00676,-0.0102,0.186,-0.0766,0.0564,0.788,0.0185,-0.0124,-3.57,-1.59e-05,-5.84e-05,6.51e-09,-3.33e-05,4.86e-05,-0.00119,0.204,0.00201,0.435,0,0,0,0,0,2.04e-06,8.25e-05,8.24e-05,5.5e-05,0.014,0.014,0.00806,0.0415,0.0415,0.0355,3.79e-11,3.79e-11,1.71e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
+27890000,0.983,-0.00676,-0.0102,0.186,-0.0766,0.0564,0.788,0.0185,-0.0124,-3.57,-1.59e-05,-5.84e-05,6.54e-09,-3.33e-05,4.86e-05,-0.00119,0.204,0.00201,0.435,0,0,0,0,0,2.04e-06,8.25e-05,8.24e-05,5.5e-05,0.014,0.014,0.00806,0.0415,0.0415,0.0355,3.79e-11,3.79e-11,1.71e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
 27990000,0.983,-0.0072,-0.0106,0.186,-0.0769,0.0578,0.775,0.0132,-0.0106,-3.5,-1.57e-05,-5.83e-05,-3.61e-08,-3.55e-05,5.26e-05,-0.00119,0.204,0.00201,0.435,0,0,0,0,0,2.03e-06,8.25e-05,8.25e-05,5.47e-05,0.013,0.013,0.00799,0.0378,0.0378,0.0352,3.71e-11,3.71e-11,1.69e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
 28090000,0.983,-0.00747,-0.0106,0.186,-0.0805,0.0585,0.782,0.00533,-0.0048,-3.43,-1.57e-05,-5.83e-05,5.74e-08,-3.54e-05,5.25e-05,-0.00119,0.204,0.00201,0.435,0,0,0,0,0,2.02e-06,8.27e-05,8.27e-05,5.45e-05,0.0139,0.0139,0.00803,0.0413,0.0413,0.0353,3.72e-11,3.72e-11,1.67e-10,2.83e-06,2.83e-06,5e-08,0,0,0,0,0,0,0,0
 28190000,0.983,-0.00691,-0.0109,0.186,-0.0811,0.0551,0.788,-0.00133,-0.00437,-3.35,-1.56e-05,-5.83e-05,1.31e-08,-3.65e-05,5.66e-05,-0.00118,0.204,0.00201,0.435,0,0,0,0,0,2.01e-06,8.28e-05,8.27e-05,5.43e-05,0.0129,0.0129,0.008,0.0377,0.0377,0.0354,3.64e-11,3.64e-11,1.65e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
-28290000,0.983,-0.0064,-0.0111,0.186,-0.0864,0.0586,0.787,-0.00968,0.00136,-3.28,-1.56e-05,-5.83e-05,8.9e-08,-3.6e-05,5.56e-05,-0.00118,0.204,0.00201,0.435,0,0,0,0,0,2e-06,8.3e-05,8.29e-05,5.41e-05,0.0138,0.0138,0.00805,0.0412,0.0412,0.0354,3.65e-11,3.65e-11,1.63e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
+28290000,0.983,-0.0064,-0.0111,0.186,-0.0864,0.0586,0.787,-0.00968,0.00136,-3.28,-1.56e-05,-5.83e-05,8.91e-08,-3.6e-05,5.56e-05,-0.00118,0.204,0.00201,0.435,0,0,0,0,0,2e-06,8.3e-05,8.29e-05,5.41e-05,0.0138,0.0138,0.00805,0.0412,0.0412,0.0354,3.65e-11,3.65e-11,1.63e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 28390000,0.983,-0.00641,-0.0118,0.186,-0.087,0.0613,0.787,-0.0144,0.00433,-3.21,-1.55e-05,-5.82e-05,1.33e-07,-3.78e-05,5.72e-05,-0.00118,0.204,0.00201,0.435,0,0,0,0,0,2e-06,8.3e-05,8.29e-05,5.38e-05,0.0129,0.0129,0.00798,0.0376,0.0376,0.0352,3.58e-11,3.58e-11,1.61e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 28490000,0.983,-0.00671,-0.0122,0.186,-0.0887,0.0655,0.788,-0.0231,0.0106,-3.13,-1.55e-05,-5.82e-05,9.27e-08,-3.75e-05,5.63e-05,-0.00117,0.204,0.00201,0.435,0,0,0,0,0,1.99e-06,8.32e-05,8.31e-05,5.37e-05,0.0138,0.0138,0.00806,0.0412,0.0411,0.0355,3.59e-11,3.59e-11,1.59e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 28590000,0.983,-0.00675,-0.0122,0.185,-0.0821,0.0605,0.786,-0.0265,0.00834,-3.06,-1.54e-05,-5.82e-05,1.31e-07,-3.85e-05,5.98e-05,-0.00117,0.204,0.00201,0.435,0,0,0,0,0,1.98e-06,8.32e-05,8.31e-05,5.34e-05,0.0128,0.0128,0.00799,0.0375,0.0375,0.0353,3.52e-11,3.52e-11,1.57e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
-28690000,0.983,-0.00652,-0.0116,0.186,-0.0824,0.0615,0.786,-0.0347,0.0145,-2.99,-1.54e-05,-5.82e-05,6.74e-08,-3.82e-05,5.89e-05,-0.00117,0.204,0.00201,0.435,0,0,0,0,0,1.97e-06,8.34e-05,8.33e-05,5.31e-05,0.0138,0.0138,0.00803,0.0411,0.0411,0.0353,3.53e-11,3.53e-11,1.55e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
+28690000,0.983,-0.00652,-0.0116,0.186,-0.0824,0.0615,0.786,-0.0347,0.0145,-2.99,-1.54e-05,-5.82e-05,6.74e-08,-3.82e-05,5.89e-05,-0.00117,0.204,0.00201,0.435,0,0,0,0,0,1.98e-06,8.34e-05,8.33e-05,5.31e-05,0.0138,0.0138,0.00803,0.0411,0.0411,0.0353,3.53e-11,3.53e-11,1.55e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 28790000,0.983,-0.00585,-0.0114,0.186,-0.079,0.0615,0.784,-0.0372,0.0162,-2.91,-1.53e-05,-5.81e-05,1.37e-07,-3.88e-05,5.88e-05,-0.00117,0.204,0.00201,0.435,0,0,0,0,0,1.97e-06,8.34e-05,8.33e-05,5.29e-05,0.0128,0.0128,0.00797,0.0375,0.0375,0.0351,3.46e-11,3.46e-11,1.53e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 28890000,0.983,-0.00569,-0.0111,0.185,-0.0833,0.0636,0.783,-0.0453,0.0225,-2.84,-1.53e-05,-5.81e-05,2.01e-07,-3.84e-05,5.78e-05,-0.00116,0.204,0.00201,0.435,0,0,0,0,0,1.96e-06,8.36e-05,8.35e-05,5.28e-05,0.0137,0.0137,0.00805,0.041,0.041,0.0355,3.47e-11,3.47e-11,1.51e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 28990000,0.983,-0.00546,-0.0113,0.186,-0.0792,0.0603,0.781,-0.0448,0.0216,-2.77,-1.51e-05,-5.8e-05,1.69e-07,-3.89e-05,5.76e-05,-0.00116,0.204,0.00201,0.435,0,0,0,0,0,1.95e-06,8.35e-05,8.35e-05,5.25e-05,0.0128,0.0128,0.00798,0.0374,0.0374,0.0352,3.4e-11,3.4e-11,1.5e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
-29090000,0.983,-0.00531,-0.0114,0.186,-0.082,0.0626,0.78,-0.0529,0.0277,-2.7,-1.51e-05,-5.8e-05,1.61e-07,-3.87e-05,5.7e-05,-0.00116,0.204,0.00201,0.435,0,0,0,0,0,1.94e-06,8.37e-05,8.37e-05,5.23e-05,0.0137,0.0137,0.00802,0.0409,0.0409,0.0353,3.41e-11,3.41e-11,1.48e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
+29090000,0.983,-0.00531,-0.0114,0.186,-0.082,0.0626,0.78,-0.0529,0.0277,-2.7,-1.51e-05,-5.8e-05,1.61e-07,-3.87e-05,5.7e-05,-0.00116,0.204,0.00201,0.435,0,0,0,0,0,1.95e-06,8.37e-05,8.37e-05,5.23e-05,0.0137,0.0137,0.00802,0.0409,0.0409,0.0353,3.41e-11,3.41e-11,1.48e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29190000,0.983,-0.00526,-0.0116,0.186,-0.0783,0.0611,0.775,-0.0507,0.027,-2.63,-1.5e-05,-5.79e-05,2.24e-07,-3.86e-05,5.57e-05,-0.00115,0.204,0.00201,0.435,0,0,0,0,0,1.94e-06,8.37e-05,8.36e-05,5.21e-05,0.0128,0.0128,0.00799,0.0374,0.0374,0.0354,3.35e-11,3.35e-11,1.46e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29290000,0.983,-0.00553,-0.0116,0.186,-0.0804,0.0672,0.778,-0.0586,0.0334,-2.55,-1.5e-05,-5.79e-05,2.4e-07,-3.84e-05,5.53e-05,-0.00115,0.204,0.00201,0.435,0,0,0,0,0,1.93e-06,8.39e-05,8.38e-05,5.19e-05,0.0137,0.0137,0.00803,0.0409,0.0409,0.0354,3.36e-11,3.36e-11,1.44e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29390000,0.983,-0.00598,-0.0111,0.186,-0.0758,0.0655,0.78,-0.0569,0.0342,-2.48,-1.49e-05,-5.79e-05,2.8e-07,-3.83e-05,5.42e-05,-0.00115,0.204,0.00201,0.435,0,0,0,0,0,1.93e-06,8.38e-05,8.38e-05,5.16e-05,0.0127,0.0127,0.00796,0.0373,0.0373,0.0352,3.3e-11,3.3e-11,1.43e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29490000,0.983,-0.00597,-0.0109,0.186,-0.0787,0.0663,0.78,-0.0646,0.0409,-2.4,-1.49e-05,-5.79e-05,3.89e-07,-3.8e-05,5.36e-05,-0.00115,0.204,0.00201,0.435,0,0,0,0,0,1.92e-06,8.4e-05,8.4e-05,5.15e-05,0.0136,0.0136,0.00805,0.0408,0.0408,0.0355,3.31e-11,3.31e-11,1.41e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29590000,0.983,-0.00588,-0.0109,0.186,-0.0742,0.0641,0.782,-0.062,0.0399,-2.33,-1.47e-05,-5.78e-05,4.92e-07,-3.75e-05,5.19e-05,-0.00115,0.204,0.00201,0.435,0,0,0,0,0,1.91e-06,8.39e-05,8.38e-05,5.13e-05,0.0127,0.0127,0.00798,0.0373,0.0373,0.0353,3.25e-11,3.25e-11,1.4e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
-29690000,0.983,-0.00593,-0.0107,0.185,-0.0785,0.063,0.777,-0.0696,0.0463,-2.25,-1.47e-05,-5.78e-05,5.85e-07,-3.69e-05,5.07e-05,-0.00114,0.204,0.00201,0.435,0,0,0,0,0,1.9e-06,8.41e-05,8.4e-05,5.1e-05,0.0136,0.0136,0.00802,0.0407,0.0407,0.0353,3.26e-11,3.26e-11,1.38e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
+29690000,0.983,-0.00593,-0.0107,0.185,-0.0785,0.063,0.777,-0.0696,0.0463,-2.25,-1.47e-05,-5.78e-05,5.85e-07,-3.69e-05,5.07e-05,-0.00114,0.204,0.00201,0.435,0,0,0,0,0,1.91e-06,8.41e-05,8.4e-05,5.1e-05,0.0136,0.0136,0.00802,0.0407,0.0407,0.0353,3.26e-11,3.26e-11,1.38e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29790000,0.983,-0.00576,-0.0113,0.185,-0.0743,0.0562,0.774,-0.0649,0.0435,-2.18,-1.46e-05,-5.77e-05,6.79e-07,-3.57e-05,4.84e-05,-0.00114,0.204,0.00201,0.435,0,0,0,0,0,1.9e-06,8.39e-05,8.39e-05,5.09e-05,0.0127,0.0127,0.00799,0.0372,0.0372,0.0354,3.2e-11,3.2e-11,1.37e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29890000,0.983,-0.00524,-0.0116,0.185,-0.075,0.0575,0.769,-0.0723,0.0491,-2.11,-1.46e-05,-5.77e-05,7.65e-07,-3.5e-05,4.69e-05,-0.00114,0.204,0.00201,0.435,0,0,0,0,0,1.9e-06,8.41e-05,8.41e-05,5.07e-05,0.0136,0.0136,0.00803,0.0407,0.0407,0.0354,3.21e-11,3.21e-11,1.35e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 29990000,0.983,-0.00541,-0.0117,0.185,-0.0696,0.0524,0.766,-0.0678,0.0444,-2.04,-1.44e-05,-5.76e-05,7.75e-07,-3.36e-05,4.3e-05,-0.00113,0.204,0.00201,0.435,0,0,0,0,0,1.89e-06,8.4e-05,8.39e-05,5.05e-05,0.0127,0.0127,0.00796,0.0372,0.0372,0.0352,3.16e-11,3.16e-11,1.33e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
@@ -305,7 +305,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 30290000,0.983,-0.00552,-0.0118,0.186,-0.0632,0.0503,0.762,-0.0745,0.0528,-1.83,-1.43e-05,-5.75e-05,5.25e-07,-3.11e-05,3.89e-05,-0.00113,0.204,0.00201,0.435,0,0,0,0,0,1.87e-06,8.41e-05,8.41e-05,4.99e-05,0.0135,0.0135,0.00802,0.0406,0.0406,0.0354,3.13e-11,3.13e-11,1.29e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 30390000,0.982,-0.00553,-0.0117,0.186,-0.056,0.0446,0.759,-0.0664,0.0495,-1.77,-1.42e-05,-5.74e-05,6.61e-07,-2.75e-05,3.44e-05,-0.00112,0.204,0.00201,0.435,0,0,0,0,0,1.87e-06,8.39e-05,8.39e-05,4.97e-05,0.0126,0.0126,0.00795,0.0371,0.0371,0.0351,3.07e-11,3.07e-11,1.28e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 30490000,0.983,-0.00553,-0.0118,0.186,-0.0588,0.0447,0.76,-0.0721,0.054,-1.69,-1.42e-05,-5.74e-05,7.44e-07,-2.71e-05,3.37e-05,-0.00112,0.204,0.00201,0.435,0,0,0,0,0,1.86e-06,8.41e-05,8.41e-05,4.96e-05,0.0135,0.0135,0.00803,0.0406,0.0406,0.0355,3.08e-11,3.08e-11,1.27e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
-30590000,0.983,-0.00586,-0.0121,0.186,-0.0544,0.0416,0.76,-0.065,0.0498,-1.62,-1.41e-05,-5.73e-05,8.52e-07,-2.42e-05,2.96e-05,-0.00112,0.204,0.00201,0.435,0,0,0,0,0,1.85e-06,8.38e-05,8.38e-05,4.93e-05,0.0126,0.0126,0.00796,0.0371,0.0371,0.0352,3.03e-11,3.04e-11,1.25e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
+30590000,0.983,-0.00586,-0.0121,0.186,-0.0544,0.0416,0.76,-0.065,0.0498,-1.62,-1.41e-05,-5.73e-05,8.52e-07,-2.42e-05,2.96e-05,-0.00112,0.204,0.00201,0.435,0,0,0,0,0,1.86e-06,8.38e-05,8.38e-05,4.93e-05,0.0126,0.0126,0.00796,0.0371,0.0371,0.0352,3.03e-11,3.04e-11,1.25e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 30690000,0.983,-0.00624,-0.0124,0.186,-0.0524,0.0405,0.758,-0.0704,0.0539,-1.55,-1.41e-05,-5.73e-05,8.6e-07,-2.37e-05,2.86e-05,-0.00111,0.204,0.00201,0.435,0,0,0,0,0,1.85e-06,8.4e-05,8.4e-05,4.91e-05,0.0135,0.0135,0.008,0.0405,0.0405,0.0353,3.04e-11,3.05e-11,1.24e-10,2.82e-06,2.82e-06,5e-08,0,0,0,0,0,0,0,0
 30790000,0.983,-0.00593,-0.012,0.185,-0.0454,0.035,0.757,-0.0631,0.0524,-1.48,-1.4e-05,-5.72e-05,8.66e-07,-2.07e-05,2.7e-05,-0.00111,0.204,0.00201,0.435,0,0,0,0,0,1.84e-06,8.37e-05,8.37e-05,4.9e-05,0.0126,0.0126,0.00797,0.0371,0.0371,0.0354,3e-11,3e-11,1.23e-10,2.82e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 30890000,0.983,-0.00529,-0.0119,0.186,-0.0458,0.0321,0.753,-0.0676,0.0558,-1.42,-1.4e-05,-5.72e-05,7.85e-07,-2.04e-05,2.63e-05,-0.00111,0.204,0.00201,0.435,0,0,0,0,0,1.83e-06,8.39e-05,8.39e-05,4.88e-05,0.0135,0.0135,0.00802,0.0405,0.0405,0.0354,3.01e-11,3.01e-11,1.21e-10,2.82e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
@@ -317,25 +317,25 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 31490000,0.982,-0.00555,-0.0123,0.186,-0.0241,0.00935,0.754,-0.0494,0.0433,-0.993,-1.37e-05,-5.7e-05,8.45e-07,-1.1e-05,1.39e-05,-0.00109,0.204,0.00201,0.435,0,0,0,0,0,1.8e-06,8.35e-05,8.35e-05,4.78e-05,0.0134,0.0134,0.00802,0.0404,0.0404,0.0354,2.9e-11,2.9e-11,1.14e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31590000,0.983,-0.00542,-0.0128,0.186,-0.02,0.00712,0.758,-0.0384,0.0389,-0.922,-1.36e-05,-5.7e-05,9.33e-07,-6.86e-06,1.15e-05,-0.00109,0.204,0.00201,0.435,0,0,0,0,0,1.79e-06,8.32e-05,8.31e-05,4.76e-05,0.0125,0.0125,0.00795,0.0369,0.037,0.0352,2.86e-11,2.86e-11,1.13e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31690000,0.983,-0.00541,-0.0133,0.185,-0.0222,0.00606,0.754,-0.0406,0.0395,-0.854,-1.36e-05,-5.7e-05,1.05e-06,-6.22e-06,1.08e-05,-0.00109,0.204,0.00201,0.435,0,0,0,0,0,1.79e-06,8.33e-05,8.33e-05,4.74e-05,0.0133,0.0133,0.00799,0.0404,0.0404,0.0353,2.87e-11,2.87e-11,1.12e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
-31790000,0.983,-0.00563,-0.0139,0.185,-0.013,0.00349,0.754,-0.029,0.0376,-0.783,-1.36e-05,-5.69e-05,1.12e-06,-9.71e-07,1.07e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.78e-06,8.3e-05,8.29e-05,4.73e-05,0.0125,0.0125,0.00796,0.0369,0.0369,0.0353,2.83e-11,2.83e-11,1.11e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
-31890000,0.983,-0.00536,-0.0136,0.185,-0.00985,0.00122,0.752,-0.0301,0.0378,-0.716,-1.36e-05,-5.69e-05,1.17e-06,-3.5e-07,1.01e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.78e-06,8.31e-05,8.31e-05,4.71e-05,0.0133,0.0133,0.00801,0.0403,0.0403,0.0354,2.84e-11,2.84e-11,1.1e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
+31790000,0.983,-0.00563,-0.0139,0.185,-0.013,0.00349,0.754,-0.029,0.0376,-0.783,-1.36e-05,-5.69e-05,1.12e-06,-9.72e-07,1.07e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.79e-06,8.3e-05,8.29e-05,4.73e-05,0.0125,0.0125,0.00796,0.0369,0.0369,0.0353,2.83e-11,2.83e-11,1.11e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
+31890000,0.983,-0.00536,-0.0136,0.185,-0.00985,0.00122,0.752,-0.0301,0.0378,-0.716,-1.36e-05,-5.69e-05,1.17e-06,-3.52e-07,1.01e-05,-0.00108,0.204,0.00201,0.435,0,0,0,0,0,1.78e-06,8.31e-05,8.31e-05,4.71e-05,0.0133,0.0133,0.00801,0.0403,0.0403,0.0354,2.84e-11,2.84e-11,1.1e-10,2.81e-06,2.81e-06,5e-08,0,0,0,0,0,0,0,0
 31990000,0.983,-0.00563,-0.0132,0.185,-0.00189,0.000554,0.748,-0.0181,0.0347,-0.651,-1.36e-05,-5.68e-05,1.13e-06,4.26e-06,9.25e-06,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.77e-06,8.27e-05,8.27e-05,4.69e-05,0.0124,0.0124,0.00794,0.0369,0.0369,0.0351,2.8e-11,2.8e-11,1.08e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32090000,0.983,-0.00599,-0.0129,0.185,-0.00251,-0.00283,0.75,-0.0183,0.0346,-0.581,-1.36e-05,-5.68e-05,1.14e-06,4.61e-06,9.04e-06,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.77e-06,8.29e-05,8.29e-05,4.68e-05,0.0133,0.0133,0.00802,0.0403,0.0403,0.0355,2.81e-11,2.81e-11,1.08e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32190000,0.983,-0.00617,-0.0132,0.185,0.00289,-0.00605,0.75,-0.00708,0.0332,-0.515,-1.36e-05,-5.67e-05,1.1e-06,8.82e-06,1.06e-05,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.76e-06,8.25e-05,8.25e-05,4.66e-05,0.0124,0.0124,0.00795,0.0369,0.0369,0.0352,2.77e-11,2.77e-11,1.06e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
-32290000,0.983,-0.00608,-0.0134,0.185,0.00441,-0.00873,0.748,-0.00675,0.0324,-0.448,-1.36e-05,-5.67e-05,1.17e-06,9.42e-06,1.03e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.75e-06,8.27e-05,8.27e-05,4.64e-05,0.0132,0.0132,0.00799,0.0403,0.0403,0.0353,2.78e-11,2.78e-11,1.05e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
+32290000,0.983,-0.00608,-0.0134,0.185,0.00441,-0.00873,0.748,-0.00675,0.0324,-0.448,-1.36e-05,-5.67e-05,1.17e-06,9.41e-06,1.03e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.75e-06,8.27e-05,8.27e-05,4.64e-05,0.0132,0.0132,0.00799,0.0403,0.0403,0.0353,2.78e-11,2.78e-11,1.05e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32390000,0.983,-0.0062,-0.0135,0.186,0.0107,-0.01,0.747,0.0046,0.0299,-0.374,-1.35e-05,-5.67e-05,1.13e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.75e-06,8.23e-05,8.23e-05,4.63e-05,0.0124,0.0124,0.00797,0.0369,0.0369,0.0354,2.74e-11,2.74e-11,1.04e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32490000,0.983,-0.00905,-0.0114,0.185,0.035,-0.0726,-0.126,0.00745,0.0237,-0.372,-1.36e-05,-5.67e-05,1.09e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.74e-06,8.24e-05,8.24e-05,4.61e-05,0.0153,0.0153,0.00784,0.0403,0.0403,0.0354,2.75e-11,2.75e-11,1.03e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32590000,0.983,-0.00899,-0.0114,0.185,0.0354,-0.0737,-0.129,0.0196,0.0201,-0.392,-1.36e-05,-5.66e-05,1.18e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.73e-06,8.12e-05,8.12e-05,4.59e-05,0.0157,0.0157,0.00755,0.0369,0.0369,0.0351,2.71e-11,2.71e-11,1.02e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32690000,0.983,-0.00897,-0.0113,0.185,0.0311,-0.0793,-0.13,0.0229,0.0124,-0.408,-1.36e-05,-5.66e-05,1.16e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.73e-06,8.14e-05,8.14e-05,4.57e-05,0.0187,0.0187,0.00736,0.0406,0.0406,0.0351,2.72e-11,2.72e-11,1.01e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32790000,0.983,-0.00866,-0.0113,0.185,0.0297,-0.0768,-0.131,0.0327,0.0105,-0.425,-1.37e-05,-5.65e-05,1.24e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.72e-06,7.87e-05,7.87e-05,4.57e-05,0.0196,0.0196,0.00713,0.0372,0.0372,0.0351,2.68e-11,2.68e-11,1.01e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 32890000,0.983,-0.00862,-0.0114,0.185,0.0293,-0.0827,-0.133,0.0357,0.00253,-0.441,-1.37e-05,-5.65e-05,1.28e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.72e-06,7.88e-05,7.88e-05,4.55e-05,0.0236,0.0236,0.00697,0.041,0.041,0.035,2.69e-11,2.69e-11,9.97e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
-32990000,0.983,-0.00835,-0.0113,0.185,0.0268,-0.0787,-0.132,0.0436,-0.000681,-0.455,-1.38e-05,-5.65e-05,1.36e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.71e-06,7.44e-05,7.44e-05,4.53e-05,0.0243,0.0243,0.00673,0.0376,0.0376,0.0347,2.65e-11,2.65e-11,9.87e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
+32990000,0.983,-0.00835,-0.0113,0.185,0.0268,-0.0787,-0.132,0.0436,-0.00068,-0.455,-1.38e-05,-5.65e-05,1.36e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.71e-06,7.44e-05,7.44e-05,4.53e-05,0.0243,0.0243,0.00673,0.0376,0.0376,0.0347,2.65e-11,2.65e-11,9.87e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 33090000,0.983,-0.00831,-0.0113,0.185,0.0227,-0.0825,-0.129,0.0461,-0.00872,-0.463,-1.38e-05,-5.65e-05,1.34e-06,1.25e-05,1.13e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.71e-06,7.45e-05,7.45e-05,4.52e-05,0.029,0.029,0.00661,0.0418,0.0418,0.0349,2.66e-11,2.66e-11,9.79e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 33190000,0.983,-0.008,-0.0113,0.185,0.019,-0.0781,-0.127,0.0523,-0.0106,-0.47,-1.38e-05,-5.65e-05,1.29e-06,1.23e-05,7.5e-06,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.7e-06,6.85e-05,6.85e-05,4.5e-05,0.0295,0.0296,0.00641,0.0382,0.0382,0.0346,2.62e-11,2.62e-11,9.7e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 33290000,0.983,-0.00805,-0.0113,0.185,0.0157,-0.0794,-0.126,0.054,-0.0185,-0.479,-1.38e-05,-5.65e-05,1.39e-06,1.23e-05,7.51e-06,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.69e-06,6.86e-05,6.86e-05,4.49e-05,0.0357,0.0357,0.00629,0.0428,0.0428,0.0344,2.63e-11,2.63e-11,9.61e-11,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 33390000,0.983,-0.00759,-0.0114,0.185,0.0113,-0.0653,-0.124,0.0573,-0.0135,-0.49,-1.39e-05,-5.64e-05,1.41e-06,6.51e-06,-1.58e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.69e-06,6.16e-05,6.16e-05,4.48e-05,0.0356,0.0356,0.00616,0.0391,0.0391,0.0343,2.6e-11,2.6e-11,9.53e-11,2.79e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0
-33490000,0.983,-0.00757,-0.0113,0.185,0.00683,-0.0661,-0.125,0.0582,-0.0201,-0.505,-1.39e-05,-5.64e-05,1.41e-06,6.54e-06,-1.58e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.68e-06,6.17e-05,6.17e-05,4.46e-05,0.0428,0.0428,0.00608,0.0442,0.0442,0.0342,2.61e-11,2.61e-11,9.44e-11,2.79e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0
-33590000,0.983,-0.0072,-0.0114,0.185,0.00353,-0.0568,-0.122,0.0609,-0.0163,-0.517,-1.4e-05,-5.64e-05,1.47e-06,-1.53e-06,-4.04e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.67e-06,5.43e-05,5.43e-05,4.44e-05,0.0412,0.0412,0.00596,0.0401,0.0402,0.0338,2.59e-11,2.59e-11,9.36e-11,2.74e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0
+33490000,0.983,-0.00757,-0.0113,0.185,0.00683,-0.0661,-0.125,0.0582,-0.0201,-0.505,-1.39e-05,-5.64e-05,1.41e-06,6.53e-06,-1.58e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.68e-06,6.17e-05,6.17e-05,4.46e-05,0.0428,0.0428,0.00608,0.0442,0.0442,0.0342,2.61e-11,2.61e-11,9.44e-11,2.79e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0
+33590000,0.983,-0.0072,-0.0114,0.185,0.00353,-0.0568,-0.122,0.0609,-0.0163,-0.517,-1.4e-05,-5.64e-05,1.47e-06,-1.54e-06,-4.04e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.67e-06,5.43e-05,5.43e-05,4.44e-05,0.0412,0.0412,0.00596,0.0401,0.0402,0.0338,2.59e-11,2.59e-11,9.36e-11,2.74e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0
 33690000,0.983,-0.0072,-0.0113,0.185,-0.00109,-0.0572,-0.124,0.0611,-0.022,-0.529,-1.4e-05,-5.64e-05,1.48e-06,-1.53e-06,-4.04e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.67e-06,5.44e-05,5.44e-05,4.43e-05,0.049,0.049,0.00595,0.0458,0.0458,0.0339,2.6e-11,2.6e-11,9.28e-11,2.74e-06,2.74e-06,5.01e-08,0,0,0,0,0,0,0,0
 33790000,0.983,-0.00696,-0.0114,0.185,-0.00367,-0.0465,-0.119,0.0651,-0.0174,-0.54,-1.4e-05,-5.63e-05,1.42e-06,-1.48e-05,-6.62e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.66e-06,4.76e-05,4.76e-05,4.42e-05,0.0454,0.0454,0.00586,0.0413,0.0413,0.0335,2.58e-11,2.58e-11,9.2e-11,2.67e-06,2.67e-06,5e-08,0,0,0,0,0,0,0,0
 33890000,0.983,-0.00697,-0.0114,0.185,-0.00777,-0.0442,-0.12,0.0645,-0.022,-0.552,-1.4e-05,-5.63e-05,1.48e-06,-1.47e-05,-6.62e-05,-0.00106,0.204,0.00201,0.435,0,0,0,0,0,1.66e-06,4.77e-05,4.77e-05,4.4e-05,0.0532,0.0532,0.00584,0.0475,0.0475,0.0334,2.59e-11,2.59e-11,9.12e-11,2.67e-06,2.67e-06,5e-08,0,0,0,0,0,0,0,0
@@ -348,4 +348,4 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 34590000,0.983,-0.00634,-0.0115,0.185,-0.0115,-0.00451,0.682,0.0722,-0.0082,-0.595,-1.41e-05,-5.63e-05,1.4e-06,-8.54e-05,-0.00013,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.62e-06,3.15e-05,3.15e-05,4.3e-05,0.0445,0.0445,0.00592,0.0451,0.0451,0.0321,2.62e-11,2.62e-11,8.59e-11,2.25e-06,2.25e-06,5e-08,0,0,0,0,0,0,0,0
 34690000,0.983,-0.00633,-0.0112,0.185,-0.0115,-0.0029,1.67,0.071,-0.00857,-0.477,-1.41e-05,-5.63e-05,1.38e-06,-8.54e-05,-0.00013,-0.00107,0.204,0.00201,0.435,0,0,0,0,0,1.62e-06,3.16e-05,3.16e-05,4.3e-05,0.048,0.048,0.00602,0.0518,0.0518,0.0322,2.63e-11,2.63e-11,8.53e-11,2.25e-06,2.25e-06,5e-08,0,0,0,0,0,0,0,0
 34790000,0.983,-0.00631,-0.011,0.185,-0.0108,0.00113,2.64,0.0721,-0.00635,-0.299,-1.41e-05,-5.63e-05,1.36e-06,-7.1e-05,-0.000146,-0.00104,0.204,0.00201,0.435,0,0,0,0,0,1.61e-06,3.02e-05,3.02e-05,4.28e-05,0.0404,0.0404,0.00604,0.0454,0.0454,0.0319,2.63e-11,2.63e-11,8.45e-11,2.13e-06,2.12e-06,5e-08,0,0,0,0,0,0,0,0
-34890000,0.983,-0.00629,-0.0107,0.185,-0.0112,0.00303,3.63,0.071,-0.006,-0.00811,-1.41e-05,-5.63e-05,1.34e-06,-6.73e-05,-0.000148,-0.00104,0.204,0.00201,0.435,0,0,0,0,0,1.6e-06,3.02e-05,3.02e-05,4.27e-05,0.0441,0.0441,0.00614,0.0519,0.0519,0.0318,2.64e-11,2.64e-11,8.38e-11,2.13e-06,2.12e-06,5e-08,0,0,0,0,0,0,0,0
+34890000,0.983,-0.00629,-0.0107,0.185,-0.0112,0.00303,3.63,0.071,-0.006,-0.00811,-1.41e-05,-5.63e-05,1.34e-06,-6.73e-05,-0.000148,-0.00104,0.204,0.00201,0.435,0,0,0,0,0,1.61e-06,3.02e-05,3.02e-05,4.27e-05,0.0441,0.0441,0.00614,0.0519,0.0519,0.0318,2.64e-11,2.64e-11,8.38e-11,2.13e-06,2.12e-06,5e-08,0,0,0,0,0,0,0,0
@@ -67,7 +67,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	_pos_ctrl_landing_status_pub.advertise();
 	_tecs_status_pub.advertise();

-	_slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE);
+	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);

 	/* fetch initial parameter values */
 	parameters_update();
@@ -283,7 +283,7 @@ FixedwingPositionControl::airspeed_poll()

 			airspeed_valid = true;

-			_airspeed_last_valid = airspeed_validated.timestamp;
+			_time_airspeed_last_valid = airspeed_validated.timestamp;
 			_airspeed = airspeed_validated.calibrated_airspeed_m_s;

 			_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
@@ -291,7 +291,7 @@ FixedwingPositionControl::airspeed_poll()

 	} else {
 		// no airspeed updates for one second
-		if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
+		if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
 			airspeed_valid = false;
 		}
 	}
@@ -321,7 +321,7 @@ FixedwingPositionControl::wind_poll()

 	} else {
 		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
-		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < T_WIND_EST_TIMEOUT;
+		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
 	}
 }

@@ -330,21 +330,21 @@ FixedwingPositionControl::manual_control_setpoint_poll()
 {
 	_manual_control_setpoint_sub.update(&_manual_control_setpoint);

-	_manual_control_setpoint_altitude = _manual_control_setpoint.x;
-	_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
+	_manual_control_setpoint_for_height_rate = _manual_control_setpoint.x;
+	_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);

 	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
 		/* Alternate stick allocation (similar concept as for multirotor systems:
 		 * demanding up/down with the throttle stick, and move faster/break with the pitch one.
 		 */
-		_manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
-		_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
+		_manual_control_setpoint_for_height_rate = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
+		_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
 	}

 	// send neutral setpoints if no update for 1 s
 	if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
-		_manual_control_setpoint_altitude = 0.f;
-		_manual_control_setpoint_airspeed = 0.5f;
+		_manual_control_setpoint_for_height_rate = 0.f;
+		_manual_control_setpoint_for_airspeed = 0.5f;
 	}
 }

@@ -393,17 +393,17 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()

 	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
 		// neutral throttle corresponds to trim airspeed
-		if (_manual_control_setpoint_airspeed < 0.5f) {
+		if (_manual_control_setpoint_for_airspeed < 0.5f) {
 			// lower half of throttle is min to trim airspeed
 			altctrl_airspeed = _param_fw_airspd_min.get() +
 					   (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
-					   _manual_control_setpoint_airspeed * 2;
+					   _manual_control_setpoint_for_airspeed * 2;

 		} else {
 			// upper half of throttle is trim to max airspeed
 			altctrl_airspeed = _param_fw_airspd_trim.get() +
 					   (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
-					   (_manual_control_setpoint_airspeed * 2 - 1);
+					   (_manual_control_setpoint_for_airspeed * 2 - 1);
 		}

 	} else if (PX4_ISFINITE(_commanded_airspeed_setpoint)) {
@@ -414,8 +414,8 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
 }

 float
-FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cruise_airspeed,
-		const Vector2f &ground_speed, float dt)
+FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cruise_airspeed,
+		const Vector2f &ground_speed)
 {
 	// overwrite internal setpoint (e.g. set prior through MAV_CMD_DO_CHANGE_SPEED) in case
 	// the current position_setpoint contains a valid airspeed setpoint
@@ -460,15 +460,15 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con
 	airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get());

 	// initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case
-	const bool outside_of_limits = _slew_rate_airspeed.getState() < airspeed_min_adjusted
-				       || _slew_rate_airspeed.getState() > _param_fw_airspd_max.get();
+	const bool outside_of_limits = _airspeed_slew_rate_controller.getState() < airspeed_min_adjusted
+				       || _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();

-	if (!PX4_ISFINITE(_slew_rate_airspeed.getState()) || outside_of_limits) {
-		_slew_rate_airspeed.setForcedValue(airspeed_setpoint);
+	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || outside_of_limits) {
+		_airspeed_slew_rate_controller.setForcedValue(airspeed_setpoint);

-	} else if (dt > FLT_EPSILON) {
+	} else if (control_interval > FLT_EPSILON) {
 		// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
-		airspeed_setpoint = _slew_rate_airspeed.update(airspeed_setpoint, dt);
+		airspeed_setpoint = _airspeed_slew_rate_controller.update(airspeed_setpoint, control_interval);
 	}

 	return airspeed_setpoint;
@@ -707,14 +707,14 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
 	 * an axis. Positive X means to rotate positively around
 	 * the X axis in NED frame, which is pitching down
 	 */
-	if (_manual_control_setpoint_altitude > deadBand) {
+	if (_manual_control_setpoint_for_height_rate > deadBand) {
 		/* pitching down */
-		float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
+		float pitch = -(_manual_control_setpoint_for_height_rate - deadBand) / factor;
 		height_rate_setpoint = pitch * _param_sinkrate_target.get();

-	} else if (_manual_control_setpoint_altitude < - deadBand) {
+	} else if (_manual_control_setpoint_for_height_rate < - deadBand) {
 		/* pitching up */
-		float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
+		float pitch = -(_manual_control_setpoint_for_height_rate + deadBand) / factor;
 		const float climb_rate_target = _param_climbrate_target.get();

 		height_rate_setpoint = pitch * climb_rate_target;
@@ -844,16 +844,6 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 	}
 }

-float
-FixedwingPositionControl::update_position_control_mode_timestep(const hrt_abstime now)
-{
-	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
-
-	return dt;
-}
-
 void
 FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
 {
@@ -885,24 +875,14 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
 }

 void
-FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &curr_pos,
-				       const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-				       const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
+FixedwingPositionControl::control_auto(const float control_interval, const Vector2d &curr_pos,
+				       const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
+				       const position_setpoint_s &pos_sp_next)
 {
-	const float dt = update_position_control_mode_timestep(now);
-
-	update_in_air_states(now);
-
-	_hdg_hold_yaw = _yaw;
-
-	_att_sp.roll_reset_integral = false;
-	_att_sp.pitch_reset_integral = false;
-	_att_sp.yaw_reset_integral = false;
-
 	position_setpoint_s current_sp = pos_sp_curr;
 	move_position_setpoint_for_vtol_transition(current_sp);

-	const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
+	const uint8_t position_sp_type = handle_setpoint_type(current_sp);

 	_position_sp_type = position_sp_type;

@@ -911,20 +891,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		publishOrbitStatus(current_sp);
 	}

-	// update lateral guidance timesteps for slewrates
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
-
 	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();

-	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
-	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
-
 	switch (position_sp_type) {
 	case position_setpoint_s::SETPOINT_TYPE_IDLE:
 		_att_sp.thrust_body[0] = 0.0f;
@@ -936,15 +904,15 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		break;

 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
-		control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;

 	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
-		control_auto_velocity(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
 		break;

 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
-		control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
+		control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
 		break;
 	}

@@ -972,11 +940,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 }

 void
-FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now)
+FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
 {
 	// only control altitude and airspeed ("fixed-bank loiter")

-	tecs_update_pitch_throttle(now, _current_altitude,
+	tecs_update_pitch_throttle(control_interval,
+				   _current_altitude,
 				   _param_fw_airspd_trim.get(),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1003,15 +972,14 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no
 }

 void
-FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
+FixedwingPositionControl::control_auto_descend(const float control_interval)
 {
-	// only control height rate
-
 	// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
 	// but not letting it drift too far away.
 	const float descend_rate = -0.5f;

-	tecs_update_pitch_throttle(now, _current_altitude,
+	tecs_update_pitch_throttle(control_interval,
+				   _current_altitude,
 				   _param_fw_airspd_trim.get(),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1034,8 +1002,10 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
 }

 uint8_t
-FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr)
 {
+	uint8_t position_sp_type = pos_sp_curr.type;
+
 	if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
 		return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
 	}
@@ -1047,8 +1017,6 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons

 	const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);

-	uint8_t position_sp_type = setpoint_type;
-
 	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
 	    || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {

@@ -1091,14 +1059,13 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 }

 void
-FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
+FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
 	Vector2d curr_wp{0, 0};
 	Vector2d prev_wp{0, 0};

-
 	/* current waypoint (the one currently heading for) */
 	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);

@@ -1174,7 +1141,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 		}
 	}

-	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+	float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
+
 	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
 	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
 	Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
@@ -1208,7 +1176,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

-	tecs_update_pitch_throttle(now, position_sp_alt,
+	tecs_update_pitch_throttle(control_interval,
+				   position_sp_alt,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1220,8 +1189,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 }

 void
-FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_auto_velocity(const float control_interval, const Vector2d &curr_pos,
+		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
 {
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
@@ -1255,7 +1224,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 	Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
 	_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));

-	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+	float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);

 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
@@ -1274,7 +1243,8 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

-	tecs_update_pitch_throttle(now, position_sp_alt,
+	tecs_update_pitch_throttle(control_interval,
+				   position_sp_alt,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1288,7 +1258,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 }

 void
-FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
+FixedwingPositionControl::control_auto_loiter(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
 		const position_setpoint_s &pos_sp_next)
 {
@@ -1349,7 +1319,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa

 	if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = (loiter_radius > 0) ? 1 : -1;
+		loiter_direction = signNoZero(loiter_radius);
 	}

 	const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
@@ -1369,7 +1339,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 	}

-	float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
+	float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_sp, ground_speed);

 	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
 	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
@@ -1410,7 +1380,8 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 		_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 	}

-	tecs_update_pitch_throttle(now, alt_sp,
+	tecs_update_pitch_throttle(control_interval,
+				   alt_sp,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1422,31 +1393,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 }

 void
-FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
+		const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
+		const position_setpoint_s &pos_sp_curr)
 {
-	const float dt = update_position_control_mode_timestep(now);
-
-	update_in_air_states(now);
-
-	_hdg_hold_yaw = _yaw;
-
-	_att_sp.roll_reset_integral = false;
-	_att_sp.pitch_reset_integral = false;
-	_att_sp.yaw_reset_integral = false;
-
-	// update lateral guidance timesteps for slewrates
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
-
-	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
-	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
-
 	/* current waypoint (the one currently heading for) */
 	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
 	Vector2d prev_wp{0, 0}; /* previous waypoint */
@@ -1491,9 +1441,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
 				       _current_latitude, _current_longitude, &_mavlink_log_pub);

-		float target_airspeed = get_auto_airspeed_setpoint(now,
-					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
-					dt);
+		float target_airspeed = get_auto_airspeed_setpoint(control_interval,
+					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed);

 		/*
 		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
@@ -1522,7 +1471,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 		// update tecs
 		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());

-		tecs_update_pitch_throttle(now, pos_sp_curr.alt,
+		tecs_update_pitch_throttle(control_interval,
+					   pos_sp_curr.alt,
 					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(takeoff_pitch_max_deg),
@@ -1560,7 +1510,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 				}

 				/* Detect launch using body X (forward) acceleration */
-				_launchDetector.update(dt, _body_acceleration(0));
+				_launchDetector.update(control_interval, _body_acceleration(0));

 				/* update our copy of the launch detection state */
 				_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1575,8 +1525,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 		if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
 			/* Launch has been detected, hence we have to control the plane. */

-			float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
-						dt);
+			float target_airspeed = get_auto_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(), ground_speed);

 			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));

@@ -1610,7 +1559,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 			/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
 			if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
 				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
+				tecs_update_pitch_throttle(control_interval,
+							   pos_sp_curr.alt,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(takeoff_pitch_max_deg),
@@ -1624,7 +1574,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));

 			} else {
-				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
+				tecs_update_pitch_throttle(control_interval,
+							   pos_sp_curr.alt,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
@@ -1679,30 +1630,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 }

 void
-FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float control_interval,
+		const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
+		const position_setpoint_s &pos_sp_curr)
 {
-	const float dt = update_position_control_mode_timestep(now);
-
-	update_in_air_states(now);
-
-	_hdg_hold_yaw = _yaw;
-
-	_att_sp.roll_reset_integral = false;
-	_att_sp.pitch_reset_integral = false;
-	_att_sp.yaw_reset_integral = false;
-
-	// update lateral guidance timesteps for slewrates
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
-
-	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
-	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-
 	// Enable tighter altitude control for landings
 	_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());

@@ -1800,10 +1731,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 			// all good, have valid terrain altitude
 			float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
 			terrain_alt = (_local_pos.ref_alt - terrain_vpos);
-			_t_alt_prev_valid = terrain_alt;
-			_time_last_t_alt = now;
+			_last_valid_terrain_alt_estimate = terrain_alt;
+			_last_time_terrain_alt_was_valid = now;

-		} else if (_time_last_t_alt == 0) {
+		} else if (_last_time_terrain_alt_was_valid == 0) {
 			// we have started landing phase but don't have valid terrain
 			// wait for some time, maybe we will soon get a valid estimate
 			// until then just use the altitude of the landing waypoint
@@ -1816,16 +1747,16 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 				abort_landing(true);
 			}

-		} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT)
+		} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT)
 			   || _land_noreturn_vertical) {
 			// use previous terrain estimate for some time and hope to recover
 			// if we are already flaring (land_noreturn_vertical) then just
 			//  go with the old estimate
-			terrain_alt = _t_alt_prev_valid;
+			terrain_alt = _last_valid_terrain_alt_estimate;

 		} else {
 			// terrain alt was not valid for long time, abort landing
-			terrain_alt = _t_alt_prev_valid;
+			terrain_alt = _last_valid_terrain_alt_estimate;
 			abort_landing(true);
 		}
 	}
@@ -1868,7 +1799,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		}

 		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt);
+		float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed);

 		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;

@@ -1916,7 +1847,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 		}

-		tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
+		tecs_update_pitch_throttle(control_interval,
+					   terrain_alt + flare_curve_alt_rel,
 					   target_airspeed,
 					   radians(_param_fw_lnd_fl_pmin.get()),
 					   radians(_param_fw_lnd_fl_pmax.get()),
@@ -1985,7 +1917,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		}

 		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt);
+		float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_approach, ground_speed);

 		/* lateral guidance */
 		if (_param_fw_use_npfg.get()) {
@@ -2019,7 +1951,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec

 		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

-		tecs_update_pitch_throttle(now, altitude_desired,
+		tecs_update_pitch_throttle(control_interval,
+					   altitude_desired,
 					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
@@ -2045,11 +1978,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 }

 void
-FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_manual_altitude(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
 	/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
-	_last_time_position_control_called = now;

 	/* Get demanded airspeed */
 	float altctrl_airspeed = get_manual_airspeed_setpoint();
@@ -2075,11 +2007,12 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
 	/* throttle limiting */
 	float throttle_max = _param_fw_thr_max.get();

-	if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
+	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}

-	tecs_update_pitch_throttle(now, altitude_sp_amsl,
+	tecs_update_pitch_throttle(control_interval,
+				   altitude_sp_amsl,
 				   altctrl_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -2112,19 +2045,9 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
 }

 void
-FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_manual_position(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
-	const float dt = update_position_control_mode_timestep(now);
-
-	// update lateral guidance timesteps for slewrates
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
-
 	// if we assume that user is taking off then help by demanding altitude setpoint well above ground
 	// and set limit to pitch angle to prevent steering into ground
 	// this will only affect planes and not VTOL
@@ -2146,7 +2069,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 	/* throttle limiting */
 	float throttle_max = _param_fw_thr_max.get();

-	if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
+	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}

@@ -2218,7 +2141,8 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 		}
 	}

-	tecs_update_pitch_throttle(now, altitude_sp_amsl,
+	tecs_update_pitch_throttle(control_interval,
+				   altitude_sp_amsl,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -2240,9 +2164,9 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 		float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
 		const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());

-		if (dt > 0.f && roll_rate_slew_rad > 0.f) {
-			roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * dt,
-						_att_sp.roll_body + roll_rate_slew_rad * dt);
+		if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
+			roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval,
+						_att_sp.roll_body + roll_rate_slew_rad * control_interval);
 		}

 		_att_sp.roll_body = roll_sp_new;
@@ -2303,6 +2227,10 @@ FixedwingPositionControl::Run()

 	if (_local_pos_sub.update(&_local_pos)) {

+		const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f,
+					       MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
+		_last_time_position_control_called = _local_pos.timestamp;
+
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {
 			// clear update
@@ -2443,42 +2371,63 @@ FixedwingPositionControl::Run()

 		set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);

-		_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
+		update_in_air_states(_local_pos.timestamp);
+
+		// update lateral guidance timesteps for slewrates
+		if (_param_fw_use_npfg.get()) {
+			_npfg.setDt(control_interval);
+
+		} else {
+			_l1_control.set_dt(control_interval);
+		}
+
+		// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
+		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
+		_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
+
+		_att_sp.roll_reset_integral = false;
+		_att_sp.pitch_reset_integral = false;
+		_att_sp.yaw_reset_integral = false;
+
+		// by default we don't want yaw to be contoller directly with rudder
+		_att_sp.fw_control_yaw = false;

 		switch (_control_mode_current) {
 		case FW_POSCTRL_MODE_AUTO: {
-				control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
+				control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
 					     _pos_sp_triplet.next);
 				break;
 			}

 		case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
-				control_auto_fixed_bank_alt_hold(_local_pos.timestamp);
+				control_auto_fixed_bank_alt_hold(control_interval);
 				break;
 			}

 		case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
-				control_auto_descend(_local_pos.timestamp);
+				control_auto_descend(control_interval);
 				break;
 			}

 		case FW_POSCTRL_MODE_AUTO_LANDING: {
-				control_auto_landing(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
+				control_auto_landing(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
+						     _pos_sp_triplet.current);
 				break;
 			}

 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
-				control_auto_takeoff(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
+				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
+						     _pos_sp_triplet.current);
 				break;
 			}

 		case FW_POSCTRL_MODE_MANUAL_POSITION: {
-				control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
+				control_manual_position(control_interval, curr_pos, ground_speed);
 				break;
 			}

 		case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
-				control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed);
+				control_manual_altitude(control_interval, curr_pos, ground_speed);
 				break;
 			}

@@ -2569,7 +2518,7 @@ FixedwingPositionControl::reset_landing_state()
 	_time_started_landing = 0;

 	// reset terrain estimation relevant values
-	_time_last_t_alt = 0;
+	_last_time_terrain_alt_was_valid = 0;

 	_land_noreturn_horizontal = false;
 	_land_noreturn_vertical = false;
@@ -2614,15 +2563,12 @@ FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
 }

 void
-FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
+FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
-	const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
-	_last_tecs_update = now;
-
 	// do not run TECS if we are not in air
 	bool run_tecs = !_landed;

@@ -2640,24 +2586,25 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 			// set this to transition airspeed to init tecs correctly
 			if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
 				// some vtols fly without airspeed sensor
-				_asp_after_transition = _param_airspeed_trans;
+				_airspeed_after_transition = _param_airspeed_trans;

 			} else {
-				_asp_after_transition = _airspeed;
+				_airspeed_after_transition = _airspeed;
 			}

-			_asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
+			_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
+							       _param_fw_airspd_max.get());

 		} else if (_was_in_transition) {
 			// after transition we ramp up desired airspeed from the speed we had coming out of the transition
-			_asp_after_transition += dt * 2.0f; // increase 2m/s
+			_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s

-			if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
-				airspeed_sp = max(_asp_after_transition, _airspeed);
+			if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
+				airspeed_sp = max(_airspeed_after_transition, _airspeed);

 			} else {
 				_was_in_transition = false;
-				_asp_after_transition = 0.0f;
+				_airspeed_after_transition = 0.0f;
 			}
 		}
 	}
@@ -2706,14 +2653,21 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 	}

 	_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
-				    _current_altitude, alt_sp,
-				    airspeed_sp, _airspeed, _eas2tas,
+				    _current_altitude,
+				    alt_sp,
+				    airspeed_sp,
+				    _airspeed,
+				    _eas2tas,
 				    climbout_mode,
 				    climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
-				    throttle_min, throttle_max, throttle_cruise,
+				    throttle_min,
+				    throttle_max,
+				    throttle_cruise,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    pitch_max_rad - radians(_param_fw_psp_off.get()),
-				    _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);
+				    _param_climbrate_target.get(),
+				    _param_sinkrate_target.get(),
+				    hgt_rate_sp);

 	tecs_status_publish();
 }
@@ -2751,7 +2705,7 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_

 	if (fabsf(loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = (loiter_radius > 0) ? 1 : -1;
+		loiter_direction = signNoZero(loiter_radius);
 	}

 	orbit_status.radius = static_cast<float>(loiter_direction) * loiter_radius;
@@ -103,25 +103,38 @@ using namespace time_literals;
 using matrix::Vector2d;
 using matrix::Vector2f;

-static constexpr float HDG_HOLD_DIST_NEXT =
-	3000.0f; // initial distance of waypoint in front of plane in heading hold mode
-static constexpr float HDG_HOLD_REACHED_DIST =
-	1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
-static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
-static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;	// max yawrate at which plane locks yaw for heading hold mode
-static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
-	0.01f; // max manual roll/yaw input from user which does not change the locked heading
+// [m] initial distance of waypoint in front of plane in heading hold mode
+static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;

-static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid
+// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
+static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f;

-static constexpr float THROTTLE_THRESH =
-	0.05f;	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
-static constexpr float ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed setpoint changes [m/s/S]
-static constexpr hrt_abstime T_WIND_EST_TIMEOUT =
-	10_s; // time after which the wind estimate is disabled if no longer updating
+// [m] distance by which previous waypoint is set behind the plane
+static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f;

-static constexpr float MIN_AUTO_TIMESTEP = 0.01f;  // minimum time step between auto control updates [s]
-static constexpr float MAX_AUTO_TIMESTEP = 0.05f;  // maximum time step between auto control updates [s]
+// [rad/s] max yawrate at which plane locks yaw for heading hold mode
+static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
+
+// [.] max manual roll/yaw normalized input from user which does not change the locked heading
+static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
+
+// [us] time after which we abort landing if terrain estimate is not valid
+static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
+
+// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
+static constexpr float THROTTLE_THRESH = 0.05f;
+
+// [m/s/s] slew rate limit for airspeed setpoint changes
+static constexpr float ASPD_SP_SLEW_RATE = 1.f;
+
+// [us] time after which the wind estimate is disabled if no longer updating
+static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
+
+// [s] minimum time step between auto control updates
+static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
+
+// [s] maximum time step between auto control updates
+static constexpr float MAX_AUTO_TIMESTEP = 0.05f;

 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -164,44 +177,45 @@ private:
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

-	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
-	uORB::Publication<vehicle_local_position_setpoint_s> 	_local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	///< vehicle local position setpoint publication
-	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
-	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
-	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
-	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
-	uORB::PublicationMulti<orbit_status_s>			_orbit_status_pub{ORB_ID(orbit_status)};
+	uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
+	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};
+	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
+	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
+	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};

-	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
-	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items
-	vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint
-	vehicle_control_mode_s		_control_mode {};		///< control mode
-	vehicle_local_position_s	_local_pos {};			///< vehicle local position
-	vehicle_status_s		_vehicle_status {};		///< vehicle status
+	manual_control_setpoint_s _manual_control_setpoint {}; // r/c channel data
+	position_setpoint_triplet_s _pos_sp_triplet {}; // triplet of mission items
+	vehicle_attitude_setpoint_s _att_sp {}; // vehicle attitude setpoint
+	vehicle_control_mode_s _control_mode {};
+	vehicle_local_position_s _local_pos {};	// vehicle local position
+	vehicle_status_s _vehicle_status {}; // vehicle status

 	double _current_latitude{0};
 	double _current_longitude{0};
 	float _current_altitude{0.f};

-	perf_counter_t	_loop_perf;				///< loop performance counter
+	perf_counter_t _loop_perf; // loop performance counter

 	MapProjection _global_local_proj_ref{};
-	float	_global_local_alt0{NAN};
+	float _global_local_alt0{NAN};

-	float	_takeoff_ground_alt{0.0f};			///< ground altitude at which plane was launched
-	float	_hdg_hold_yaw{0.0f};				///< hold heading for velocity mode
-	bool	_hdg_hold_enabled{false};			///< heading hold enabled
-	bool	_yaw_lock_engaged{false};			///< yaw is locked for heading hold
+	// [m] ground altitude where the plane was launched
+	float _takeoff_ground_alt{0.0f};

-	float	_min_current_sp_distance_xy{FLT_MAX};
+	// [rad] yaw setpoint for manual position mode heading hold
+	float _hdg_hold_yaw{0.0f};

-	position_setpoint_s _hdg_hold_prev_wp {};		///< position where heading hold started
-	position_setpoint_s _hdg_hold_curr_wp {};		///< position to which heading hold flies
+	bool _hdg_hold_enabled{false}; // heading hold enabled
+	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold

-	/**
-	 * @brief Last absolute time position control has been called [us]
-	 *
-	 */
+	float _min_current_sp_distance_xy{FLT_MAX};
+
+	position_setpoint_s _hdg_hold_prev_wp {}; // position where heading hold started
+	position_setpoint_s _hdg_hold_curr_wp {}; // position to which heading hold flies
+
+	// [us] Last absolute time position control has been called
 	hrt_abstime _last_time_position_control_called{0};

 	bool _landed{true};
@@ -216,38 +230,60 @@ private:

 	Landingslope _landingslope;

-	hrt_abstime _time_started_landing{0};			///< time at which landing started
+	// [us] time at which landing started
+	hrt_abstime _time_started_landing{0};

-	float _t_alt_prev_valid{0};				///< last terrain estimate which was valid
-	hrt_abstime _time_last_t_alt{0};			///< time at which we had last valid terrain alt
+	// [m] last terrain estimate which was valid
+	float _last_valid_terrain_alt_estimate{0.0f};

-	float _flare_height{0.0f};				///< estimated height to ground at which flare started
-	float _flare_pitch_sp{0.0f};			///< Current forced (i.e. not determined using TECS) flare pitch setpoint
+	// [us] time at which we had last valid terrain alt
+	hrt_abstime _last_time_terrain_alt_was_valid{0};
+
+	// [m] estimated height to ground at which flare started
+	float _flare_height{0.0f};
+
+	// [m] current forced (i.e. not determined using TECS) flare pitch setpoint
+	float _flare_pitch_sp{0.0f};
+
+	// [m] estimated height to ground at which flare started
 	float _flare_curve_alt_rel_last{0.0f};
-	float _target_bearing{0.0f};				///< estimated height to ground at which flare started

-	bool _was_in_air{false};				///< indicated wether the plane was in the air in the previous interation*/
-	hrt_abstime _time_went_in_air{0};			///< time at which the plane went in the air
+	float _target_bearing{0.0f}; // [rad]

-	/* Takeoff launch detection and runway */
+	// indicates whether the plane was in the air in the previous interation
+	bool _was_in_air{false};
+
+	// [us] time at which the plane went in the air
+	hrt_abstime _time_went_in_air{0};
+
+	// Takeoff launch detection and runway
 	LaunchDetector _launchDetector;
 	LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
 	hrt_abstime _launch_detection_notify{0};

 	RunwayTakeoff _runway_takeoff;

-	bool _last_manual{false};				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+	// true if the last iteration was in manual mode (used to determine when a reset is needed)
+	bool _last_manual{false};

 	/* throttle and airspeed states */
-	bool _airspeed_valid{false};				///< flag if a valid airspeed estimate exists
-	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.
+
+	bool _airspeed_valid{false};
+
+	// [us] last time airspeed was received. used to detect timeouts.
+	hrt_abstime _time_airspeed_last_valid{0};
+
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};

 	/* wind estimates */
-	Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
-	bool _wind_valid{false}; ///< flag if a valid wind estimate exists
-	hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
+
+	// [m/s] wind velocity vector
+	Vector2f _wind_vel{0.0f, 0.0f};
+
+	bool _wind_valid{false};
+
+	hrt_abstime _time_wind_last_received{0}; // [us]

 	float _pitch{0.0f};
 	float _yaw{0.0f};
@@ -256,32 +292,48 @@ private:
 	matrix::Vector3f _body_acceleration{};
 	matrix::Vector3f _body_velocity{};

-	bool _reinitialize_tecs{true};				///< indicates if the TECS states should be reinitialized (used for VTOL)
+	bool _reinitialize_tecs{true};
 	bool _is_tecs_running{false};
-	hrt_abstime _last_tecs_update{0};

-	float _asp_after_transition{0.0f};
+	hrt_abstime _time_last_tecs_update{0}; // [us]
+
+	float _airspeed_after_transition{0.0f};
 	bool _was_in_transition{false};

-	bool _vtol_tailsitter{false};
+	bool _is_vtol_tailsitter{false};

 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};

 	// estimator reset counters
-	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
-	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state

-	float _manual_control_setpoint_altitude{0.0f};
-	float _manual_control_setpoint_airspeed{0.0f};
-	float _commanded_airspeed_setpoint{NAN};		///< airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
+	// captures the number of times the estimator has reset the horizontal position
+	uint8_t _pos_reset_counter{0};

-	hrt_abstime _time_in_fixed_bank_loiter{0};
+	// captures the number of times the estimator has reset the altitude state
+	uint8_t _alt_reset_counter{0};

-	ECL_L1_Pos_Controller	_l1_control;
+	// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
+	float _manual_control_setpoint_for_height_rate{0.0f};
+
+	// [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands
+	float _manual_control_setpoint_for_airspeed{0.0f};
+
+	// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
+	float _commanded_airspeed_setpoint{NAN};
+
+	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
+
+	// L1 guidance - lateral-directional position control
+	ECL_L1_Pos_Controller _l1_control;
+
+	// nonlinear path following guidance - lateral-directional position control
 	NPFG _npfg;
-	TECS			_tecs;
+
+	// total energy control system - airspeed / altitude control
+	TECS _tecs;

 	uint8_t _position_sp_type{0};
+
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
 		FW_POSCTRL_MODE_AUTO_ALTITUDE,
@@ -291,10 +343,11 @@ private:
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
-	} _control_mode_current{FW_POSCTRL_MODE_OTHER};		///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
+	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed

 	param_t _param_handle_airspeed_trans{PARAM_INVALID};
-	float _param_airspeed_trans{NAN};
+
+	float _param_airspeed_trans{NAN}; // [m/s]

 	enum StickConfig {
 		STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
@@ -302,54 +355,61 @@ private:
 	};

 	// Update our local parameter cache.
-	int		parameters_update();
+	int parameters_update();

 	// Update subscriptions
-	void		airspeed_poll();
-	void		control_update();
-	void 		manual_control_setpoint_poll();
-	void		vehicle_attitude_poll();
-	void		vehicle_command_poll();
-	void		vehicle_control_mode_poll();
-	void		vehicle_status_poll();
-	void        wind_poll();
+	void airspeed_poll();
+	void control_update();
+	void manual_control_setpoint_poll();
+	void vehicle_attitude_poll();
+	void vehicle_command_poll();
+	void vehicle_control_mode_poll();
+	void vehicle_status_poll();
+	void wind_poll();

-	void		status_publish();
-	void		landing_status_publish();
-	void		tecs_status_publish();
-	void 		publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
+	void status_publish();
+	void landing_status_publish();
+	void tecs_status_publish();
+	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);

-	void		abort_landing(bool abort);
+	void abort_landing(bool abort);

 	/**
-	 * Get a new waypoint based on heading and distance from current position
+	 * @brief Get a new waypoint based on heading and distance from current position
 	 *
 	 * @param heading the heading to fly to
 	 * @param distance the distance of the generated waypoint
 	 * @param waypoint_prev the waypoint at the current position
 	 * @param waypoint_next the waypoint in the heading direction
 	 */
-	void		get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
-			position_setpoint_s &waypoint_next, bool flag_init);
+	void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
+					   position_setpoint_s &waypoint_next, bool flag_init);

 	/**
-	 * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
+	 * @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
+	 *
+	 * @param takeoff_alt Altitude AMSL at launch or when runway takeoff is detected [m]
 	 */
-	float		get_terrain_altitude_takeoff(float takeoff_alt);
+	float get_terrain_altitude_takeoff(float takeoff_alt);

+	/**
+	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
+	 *
+	 * @return Manual height rate setpoint [m/s]
+	 */
 	float getManualHeightRateSetpoint();

 	/**
-	 * Check if we are in a takeoff situation
+	 * @brief Check if we are in a takeoff situation
 	 */
-	bool 		in_takeoff_situation();
+	bool in_takeoff_situation();

 	/**
-	 * Update desired altitude base on user pitch stick input
+	 * @brief Update desired altitude base on user pitch stick input
 	 *
 	 * @param dt Time step
 	 */
-	void		update_desired_altitude(float dt);
+	void update_desired_altitude(float dt);

 	/**
 	 * @brief Updates timing information for landed and in-air states.
@@ -358,80 +418,203 @@ private:
 	 */
 	void update_in_air_states(const hrt_abstime now);

-	/**
-	 * @brief Updates the time since the last position control call.
-	 *
-	 * @param now Current system time [us]
-	 * @return Time since last position control call [s]
-	 */
-	float update_position_control_mode_timestep(const hrt_abstime now);
-
 	/**
 	 * @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in  a VTOL
 	 * transition.
 	 *
-	 * @param[in,out] current_sp current position setpoint
+	 * @param[in,out] current_sp Current position setpoint
 	 */
 	void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);

-	uint8_t		handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
-	void		control_auto(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
-				     const position_setpoint_s &pos_sp_prev,
-				     const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
+	/**
+	 * @brief Changes the position setpoint type to achieve the desired behavior in some instances.
+	 *
+	 * @param pos_sp_curr Current position setpoint
+	 * @return Adjusted position setpoint type
+	 */
+	uint8_t	handle_setpoint_type(const position_setpoint_s &pos_sp_curr);

-	void		control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
-	void		control_auto_descend(const hrt_abstime &now);
-
-	void		control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-					      const Vector2f &ground_speed,
-					      const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
-	void		control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-					    const Vector2f &ground_speed,
-					    const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
-	void		control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-					      const Vector2f &ground_speed,
-					      const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	/* automatic control methods */

 	/**
-	 * @brief Vehicle control while in takeoff
+	 * @brief Automatic position control for waypoints, orbits, and velocity control
 	 *
-	 * @param now Current system time [us]
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 * @param pos_sp_next next position setpoint
+	 */
+	void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
+			  const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
+
+	/**
+	 * @brief Controls altitude and airspeed for a fixed-bank loiter.
+	 *
+	 * Used as a failsafe mode after a lateral position estimate failure.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 */
+	void control_auto_fixed_bank_alt_hold(const float control_interval);
+
+	/**
+	 * @brief Control airspeed with a fixed descent rate and roll angle.
+	 *
+	 * Used as a failsafe mode after a lateral position estimate failure.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 */
+	void control_auto_descend(const float control_interval);
+
+	/**
+	 * @brief Vehicle control for position waypoints.
+	 *
+	 * @param control_interval Time since last position control call [s]
 	 * @param curr_pos Current 2D local position vector of vehicle [m]
 	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
 	 * @param pos_sp_prev previous position setpoint
 	 * @param pos_sp_curr current position setpoint
 	 */
-	void		control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
-					     const Vector2f &ground_speed,
-					     const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
-	void		control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
-					     const Vector2f &ground_speed,
-					     const position_setpoint_s &pos_sp_prev,
-					     const position_setpoint_s &pos_sp_curr);
-	void		control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
-	void		control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
+	void control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
+				   const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);

-	float		get_tecs_pitch();
-	float		get_tecs_thrust();
+	/**
+	 * @brief Vehicle control for loiter waypoints.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 * @param pos_sp_next next position setpoint
+	 */
+	void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
+				 const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);

-	float		get_manual_airspeed_setpoint();
-	float		get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed,
-			float dt);
+	/**
+	 * @brief Controls a desired airspeed, bearing, and height rate.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_velocity(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
+				   const position_setpoint_s &pos_sp_curr);

-	void		reset_takeoff_state(bool force = false);
-	void		reset_landing_state();
-	bool		using_npfg_with_wind_estimate() const;
-	Vector2f 	get_nav_speed_2d(const Vector2f &ground_speed);
-	void		set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
+	/**
+	 * @brief Controls automatic takeoff.
+	 *
+	 * @param now Current system time [us]
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
+				  const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+
+	/**
+	 * @brief Controls automatic landing.
+	 *
+	 * @param now Current system time [us]
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param control_interval Time since the last position control update [s]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
+				  const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+
+	/* manual control methods */
+
+	/**
+	 * @brief Controls altitude and airspeed, user commands roll setpoint.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 */
+	void control_manual_altitude(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
+
+	/**
+	 * @brief Controls user commanded altitude, airspeed, and bearing.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 */
+	void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
+
+	float get_tecs_pitch();
+	float get_tecs_thrust();
+
+	float get_manual_airspeed_setpoint();
+
+	/**
+	 * @brief Returns an calibrated airspeed setpoint for auto modes.
+	 *
+	 * Adjusts the setpoint for wind, accelerated stall, and slew rates.
+	 *
+	 * @param control_interval Time since the last position control update [s]
+	 * @param pos_sp_cru_airspeed The commanded cruise airspeed from the position setpoint [s]
+	 * @param ground_speed Vehicle ground velocity vector [m/s]
+	 * @return Calibrated airspeed setpoint [m/s]
+	 */
+	float get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cru_airspeed,
+					 const Vector2f &ground_speed);
+
+	void reset_takeoff_state(bool force = false);
+	void reset_landing_state();
+
+	bool using_npfg_with_wind_estimate() const;
+
+	/**
+	 * @brief Returns the velocity vector to be input in the lateral-directional guidance laws.
+	 *
+	 * Replaces the ground velocity vector with an air velocity vector depending on the wind condition and whether
+	 * NPFG or L1 are being used for horizontal position control.
+	 *
+	 * @param ground_speed Vehicle ground velocity vector [m/s]
+	 * @return Velocity vector to control with lateral-directional guidance [m/s]
+	 */
+	Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
+
+	/**
+	 * @brief Decides which control mode to execute.
+	 *
+	 * May also change the position setpoint type depending on the desired behavior.
+	 *
+	 * @param now Current system time [us]
+	 * @param pos_sp_curr_valid True if the current position setpoint is valid
+	 */
+	void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);

 	void publishOrbitStatus(const position_setpoint_s pos_sp);

-	SlewRate<float> _slew_rate_airspeed;
+	SlewRate<float> _airspeed_slew_rate_controller;

-	/*
-	 * Call TECS : a wrapper function to call the TECS implementation
+	/**
+	 * @brief A wrapper function to call the TECS implementation
+	 *
+	 * @param control_interval Time since the last position control update [s]
+	 * @param alt_sp Altitude setpoint, AMSL [m]
+	 * @param airspeed_sp Calibrated airspeed setpoint [m/s]
+	 * @param pitch_min_rad Nominal pitch angle command minimum [rad]
+	 * @param pitch_max_rad Nominal pitch angle command maximum [rad]
+	 * @param throttle_min Minimum throttle command [0,1]
+	 * @param throttle_max Maximum throttle command [0,1]
+	 * @param throttle_cruise Throttle required for level flight at cruising airspeed [0,1]
+	 * @param climbout_mode True if TECS should engage climbout mode
+	 * @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad]
+	 * @param disable_underspeed_detection True if underspeed detection should be disabled
+	 * @param hgt_rate_sp Height rate setpoint [m/s]
 	 */
-	void tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
+	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
 					float pitch_min_rad, float pitch_max_rad,
 					float throttle_min, float throttle_max, float throttle_cruise,
 					bool climbout_mode, float climbout_pitch_min_rad,
@@ -81,12 +81,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);
 	add_topic("parameter_update");
-
-	// Pasta cook request from waiter to the chef
-	add_topic("pasta_cook");
-	// Pasta order from customer to the waiter
-	add_topic("pasta_order");
-
 	add_topic("position_controller_status", 500);
 	add_topic("position_setpoint_triplet", 200);
 	add_optional_topic("px4io_status");
@@ -38,7 +38,6 @@
 * arms and to the lower left disarms the vehicle.
 *
 * @boolean
- * @reboot_required true
 * @group Manual Control
 */
 PARAM_DEFINE_INT32(MAN_ARM_GESTURE, 1);
@@ -184,16 +184,15 @@ MulticopterRateControl::Run()
 					math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};

 				_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
-				_thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
+				_thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
+				_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;

 				// publish rate setpoint
-				vehicle_rates_setpoint.roll           = _rates_setpoint(0);
-				vehicle_rates_setpoint.pitch          = _rates_setpoint(1);
-				vehicle_rates_setpoint.yaw            = _rates_setpoint(2);
-				vehicle_rates_setpoint.thrust_body[0] = 0.0f;
-				vehicle_rates_setpoint.thrust_body[1] = 0.0f;
-				vehicle_rates_setpoint.thrust_body[2] = -_thrust_setpoint;
-				vehicle_rates_setpoint.timestamp      = hrt_absolute_time();
+				vehicle_rates_setpoint.roll = _rates_setpoint(0);
+				vehicle_rates_setpoint.pitch = _rates_setpoint(1);
+				vehicle_rates_setpoint.yaw = _rates_setpoint(2);
+				_thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body);
+				vehicle_rates_setpoint.timestamp = hrt_absolute_time();

 				_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
 			}
@@ -203,7 +202,7 @@ MulticopterRateControl::Run()
 				_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll)  ? vehicle_rates_setpoint.roll  : rates(0);
 				_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
 				_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw)   ? vehicle_rates_setpoint.yaw   : rates(2);
-				_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
+				_thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body);
 			}
 		}

@@ -251,13 +250,14 @@ MulticopterRateControl::Run()
 			actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint) ? _thrust_setpoint : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint(
+						2) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
 			actuators.timestamp_sample = angular_velocity.timestamp_sample;

 			if (!_vehicle_status.is_vtol) {
 				publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample);
-				publishThrustSetpoint(_thrust_setpoint, angular_velocity.timestamp_sample);
+				publishThrustSetpoint(angular_velocity.timestamp_sample);
 			}

 			// scale effort by battery status if enabled
@@ -307,14 +307,12 @@ void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, co
 	_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
 }

-void MulticopterRateControl::publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample)
+void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
 {
 	vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
 	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
 	vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
-	vehicle_thrust_setpoint.xyz[0] = 0.0f;
-	vehicle_thrust_setpoint.xyz[1] = 0.0f;
-	vehicle_thrust_setpoint.xyz[2] = PX4_ISFINITE(thrust_setpoint) ? -thrust_setpoint : 0.0f; // Z is Down
+	_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);

 	_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
 }
@@ -94,8 +94,7 @@ private:
 	void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);

 	void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime &timestamp_sample);
-
-	void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample);
+	void publishThrustSetpoint(const hrt_abstime &timestamp_sample);

 	RateControl _rate_control; ///< class for rate control calculations

@@ -138,7 +137,7 @@ private:
 	matrix::Vector3f _rates_setpoint{};

 	float _battery_status_scale{0.0f};
-	float _thrust_setpoint{0.0f};
+	matrix::Vector3f _thrust_setpoint{};

 	float _energy_integration_time{0.0f};
 	float _control_energy[4] {};
@@ -608,7 +608,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->yaw_valid = PX4_ISFINITE(item.yaw);
 	sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
 			    _navigator->get_loiter_radius();
-	sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
+	sp->loiter_direction = math::signNoZero(item.loiter_radius);

 	if (item.acceptance_radius > 0.0f && PX4_ISFINITE(item.acceptance_radius)) {
 		// if the mission item has a specified acceptance radius, overwrite the default one from parameters
@@ -234,7 +234,10 @@ void Navigator::run()
 				// TODO: move DO_GO_AROUND handling to navigator
 				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);

-			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION
+				   && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+				// only update the reposition setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter,
+				// which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too)

 				bool reposition_valid = true;

@@ -39,7 +39,7 @@ add_library(px4iofirmware
 	mixer.cpp
 	px4io.cpp
 	registers.c
-	safety.cpp
+	safety_button.cpp
 	serial.cpp
 )

@@ -134,13 +134,11 @@ mixer_tick()
 	 * FMU or from the mixer.
 	 *
 	 */
-	should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)				/* IO initialised without error */
-		     && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)			/* and IO is armed */
+	should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)		/* IO initialised without error */
 		     && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		/* and FMU is armed */
 		     ;

-	should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)              /* IO initialised without error */
-				 && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)        /* and IO is armed */
+	should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  /* IO initialised without error */
 				 && ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED)     /* and FMU is prearmed */
 				     || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)       /* or direct PWM is set */
 				    ));
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 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
@@ -114,6 +114,7 @@
 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 11) /* FMU was initialized and OK once */
 #define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 12) /* ST24 input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_SUMD		(1 << 13) /* SUMD input is valid */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT	(1 << 14) /* px4io safety button was pressed for longer than 1 second */

 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_RC_LOST           (1 << 0) /* timed out waiting for RC input */
@@ -184,12 +185,8 @@ enum {							/* DSM bind states */

 #define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
 /* storage space of 12 occupied by CRC */
-#define PX4IO_P_SETUP_FORCE_SAFETY_OFF		12	/* force safety switch into
-                                                           'armed' (PWM enabled) state - this is a non-data write and
-                                                           hence index 12 can safely be used. */
-
-#define PX4IO_P_SETUP_FORCE_SAFETY_ON		14	/* force safety switch into 'disarmed' (PWM disabled state) */
-#define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
+#define PX4IO_P_SETUP_SAFETY_BUTTON_ACK	14	/**< ACK from FMU when it gets safety button pressed status */
+#define PX4IO_P_SETUP_SAFETY_OFF		15	/**< FMU inform PX4IO about safety_off for LED indication*/
 #define PX4IO_P_SETUP_SBUS_RATE			16	/**< frame rate of SBUS1 output in Hz */
 #define PX4IO_P_SETUP_THERMAL			17	/**< thermal management */
 #define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION	18	/**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
@@ -212,7 +209,7 @@ enum {							/* DSM bind states */
 #define PX4IO_PAGE_TEST				127
 #define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */

-/* PWM disarmed values that are active, even when SAFETY_SAFE */
+/* PWM disarmed values that are active */
 #define PX4IO_PAGE_DISARMED_PWM			109		/* 0..CONFIG_ACTUATOR_COUNT-1 */

 /**
@@ -154,8 +154,7 @@ show_debug_messages(void)
 static void
 update_mem_usage(void)
 {
-	if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
-			   /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 		return;
 	}

@@ -183,8 +182,7 @@ ring_blink(void)
 {
 #if defined(LED_GREEN)

-	if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
-			   /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 		LED_GREEN(true);
 		return;
 	}
@@ -314,8 +312,8 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
 	ENABLE_SBUS_OUT(false);
 #endif

-	/* start the safety switch handler */
-	safety_init();
+	/* start the safety button handler */
+	safety_button_init();

 	/* initialise the control inputs */
 	controls_init();
@@ -131,8 +131,6 @@ extern struct sys_state_s system_state;
 # define ADC_VSERVO			4
 # define ADC_RSSI			5

-#define BUTTON_SAFETY		px4_arch_gpioread(GPIO_BTN_SAFETY)
-
 #define PX4_CRITICAL_SECTION(cmd)	{ irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); }

 void atomic_modify_or(volatile uint16_t *target, uint16_t modification);
@@ -145,9 +143,9 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
 extern void	mixer_tick(void);

 /**
- * Safety switch/LED.
+ * Safety button/LED.
 */
-extern void	safety_init(void);
+extern void	safety_button_init(void);
 extern void	failsafe_led_init(void);

 /**
@@ -413,10 +413,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;

 		case PX4IO_P_SETUP_REBOOT_BL:
-			if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
-				// don't allow reboot while armed
-				break;
-			}

 			// check the magic value
 			if (value != PX4IO_REBOOT_BL_MAGIC) {
@@ -433,22 +429,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;

-		case PX4IO_P_SETUP_FORCE_SAFETY_ON:
-			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
-
-			} else {
-				return -1;
-			}
+		case PX4IO_P_SETUP_SAFETY_BUTTON_ACK:
+			// clear safety button pressed flag so it can be used again
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;

 			break;

-		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
-			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+		case PX4IO_P_SETUP_SAFETY_OFF:
+
+			if (value) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;

 			} else {
-				return -1;
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			}

 			break;
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * @file safety.cpp
+ * @file safety_button.cpp
 * Safety button logic.
 *
 * @author Lorenz Meier <lorenz@px4.io>
@@ -46,7 +46,7 @@

 #include "px4io.h"

-static struct hrt_call arming_call;
+static struct hrt_call safety_button_call;
 static struct hrt_call failsafe_call;

 /*
@@ -59,30 +59,23 @@ static unsigned counter = 0;
 * Define the various LED flash sequences for each system state.
 */
 #define LED_PATTERN_FMU_OK_TO_ARM		0x0003			/**< slow blinking			*/
-#define LED_PATTERN_FMU_REFUSE_TO_ARM		0x5555		/**< fast blinking			*/
+#define LED_PATTERN_FMU_REFUSE_TO_ARM	0x5555			/**< fast blinking			*/
 #define LED_PATTERN_IO_ARMED			0x5050			/**< long off, then double blink 	*/
 #define LED_PATTERN_FMU_ARMED			0x5500			/**< long off, then quad blink 		*/
 #define LED_PATTERN_IO_FMU_ARMED		0xffff			/**< constantly on			*/

 static unsigned blink_counter = 0;

-/*
- * IMPORTANT: The arming state machine critically
- * 	      depends on using the same threshold
- *            for arming and disarming. Since disarming
- *            is quite deadly for the system, a similar
- *            length can be justified.
- */
-#define ARM_COUNTER_THRESHOLD	10
+#define SAFETY_SWITCH_THRESHOLD	10

-static void safety_check_button(void *arg);
+static void safety_button_check(void *arg);
 static void failsafe_blink(void *arg);

 void
-safety_init(void)
+safety_button_init(void)
 {
 	/* arrange for the button handler to be called at 10Hz */
-	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+	hrt_call_every(&safety_button_call, 1000, 100000, safety_button_check, NULL);
 }

 void
@@ -93,26 +86,20 @@ failsafe_led_init(void)
 }

 static void
-safety_check_button(void *arg)
+safety_button_check(void *arg)
 {
-	const bool safety_button_pressed = BUTTON_SAFETY;
+	const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);

-	/* Keep safety button pressed for one second to turn off safety
-	 *
-	 * Note that safety cannot be turned on again by button because a button
-	 * hardware problem could accidentally disable it in flight.
+	/* Keep safety button pressed for one second to trigger safety button event.
+	 * The logic to prevent turning on safety again is in the commander.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
-	    (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {

-		if (counter <= ARM_COUNTER_THRESHOLD) {
-			counter++;
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT)) {

-		}
+		counter++;

-		if (counter == ARM_COUNTER_THRESHOLD) {
-			// switch safety off -> ready to arm state
-			atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
+		if (counter >= SAFETY_SWITCH_THRESHOLD) {
+			atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT);
 		}

 	} else {
@@ -377,6 +377,23 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)

 		_calibration.ParametersUpdate();

+		// IMU_GYRO_RATEMAX
+		if (_param_imu_gyro_ratemax.get() <= 0) {
+			const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
+			_param_imu_gyro_ratemax.reset();
+			PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax,
+				 _param_imu_gyro_ratemax.get());
+		}
+
+		// constrain IMU_GYRO_RATEMAX 50-10,000 Hz
+		const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000);
+
+		if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) {
+			PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax);
+			_param_imu_gyro_ratemax.set(imu_gyro_ratemax);
+			_param_imu_gyro_ratemax.commit_no_notification();
+		}
+
 		// gyro low pass cutoff frequency changed
 		for (auto &lp : _lp_filter_velocity) {
 			if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) {
@@ -146,8 +146,7 @@ bool VehicleIMU::ParametersUpdate(bool force)
 		}

 		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
-		int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(),
-						  (int32_t)50, math::min(_param_imu_gyro_ratemax.get(), (int32_t) 1000));
+		const int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), (int32_t)50, (int32_t)1000);

 		if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
 			PX4_WARN("IMU_INTEG_RATE updated %" PRId32 " -> %" PRIu32, _param_imu_integ_rate.get(), imu_integration_rate_hz);
@@ -191,7 +191,6 @@ private:

 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
-		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
 		(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal
 	)
 };
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2022 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
@@ -45,5 +45,4 @@ px4_add_module(
 		drivers_accelerometer
 		drivers_gyroscope
 		drivers_magnetometer
-		px4_work_queue
 	)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2019-2022 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
@@ -139,12 +139,6 @@ private:
 	matrix::Vector3f _v_S;	// velocity in segment frame

 public:
-
-	AeroSeg()
-	{
-		AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f());
-	}
-
 	/** public constructor
 	 * AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
 	 * float AR = -1.0f, float cf = 0.0f, float prop_radius=-1.0f, float cl_alpha=2.0f*M_PI_F, float alpha_max_deg=0.0f, float alpha_min_deg=0.0f)
@@ -162,7 +156,7 @@ public:
 	 * alpha_max_deg: maximum angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
 	 * alpha_min_deg: maximum negative angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
 	 */
-	AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
+	AeroSeg(float span, float mac, float alpha_0_deg, const matrix::Vector3f &p_B, float dihedral_deg = 0.0f,
 		float AR = -1.0f, float cf = 0.0f, float prop_radius = -1.0f, float cl_alpha = 2.0f * M_PI_F,
 		float alpha_max_deg = 0.0f, float alpha_min_deg = 0.0f)
 	{
@@ -176,7 +170,7 @@ public:
 		_span = span;
 		_mac = mac;
 		_alpha_0 = math::radians(alpha_0_deg);
-		_p_B = matrix::Vector3f(p_B);
+		_p_B = p_B;
 		_ar = (AR <= 0.0f) ? _span / _mac : AR; // setting AR<=0 will compute it from _span and _mac
 		_alpha_eff = 0.0f;
 		_alpha_eff_old = 0.0f;
@@ -208,6 +202,10 @@ public:
 		_kD = 1.0f / (M_PI_F * K0 * _ar);
 	}

+
+	AeroSeg() : AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f())
+	{}
+
 	/** aerodynamic force and moments of a generic flate plate segment
 	 * void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f,
 	 *                  float def = 0.0f, float thrust=0.0f, float dt = -1.0f)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 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
@@ -54,8 +54,16 @@ using namespace matrix;
 using namespace time_literals;

 Sih::Sih() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+	ModuleParams(nullptr)
+{}
+
+Sih::~Sih()
+{
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+}
+
+void Sih::run()
 {
 	_px4_accel.set_temperature(T1_C);
 	_px4_gyro.set_temperature(T1_C);
@@ -77,15 +85,86 @@ Sih::Sih() :
 	if (_sys_ctrl_alloc.get()) {
 		_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
 	}
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	lockstep_loop();
+#else
+	realtime_loop();
+#endif
+	exit_and_cleanup();
 }

-Sih::~Sih()
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+// Get current timestamp in microseconds
+uint64_t micros()
 {
-	perf_free(_loop_perf);
-	perf_free(_loop_interval_perf);
+	struct timeval t;
+	gettimeofday(&t, nullptr);
+	return t.tv_sec * ((uint64_t)1000000) + t.tv_usec;
 }

-bool Sih::init()
+void Sih::lockstep_loop()
+{
+
+	int rate = math::min(_imu_gyro_ratemax.get(), _imu_integration_rate.get());
+
+	// default to 400Hz (2500 us interval)
+	if (rate <= 0) {
+		rate = 400;
+	}
+
+	// 200 - 2000 Hz
+	int sim_interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
+
+	float speed_factor = 1.f;
+	const char *speedup = getenv("PX4_SIM_SPEED_FACTOR");
+
+	if (speedup) {
+		speed_factor = atof(speedup);
+	}
+
+	int rt_interval_us = int(roundf(sim_interval_us / speed_factor));
+
+	PX4_INFO("Simulation loop with %d Hz (%d us sim time interval)", rate, sim_interval_us);
+	PX4_INFO("Simulation with %.1fx speedup. Loop with (%d us wall time interval)", (double)speed_factor, rt_interval_us);
+	uint64_t pre_compute_wall_time_us;
+
+	while (!should_exit()) {
+		pre_compute_wall_time_us = micros();
+		perf_count(_loop_interval_perf);
+
+		_current_simulation_time_us += sim_interval_us;
+		struct timespec ts;
+		abstime_to_ts(&ts, _current_simulation_time_us);
+		px4_clock_settime(CLOCK_MONOTONIC, &ts);
+
+		perf_begin(_loop_perf);
+		sensor_step();
+		perf_end(_loop_perf);
+
+		// Only do lock-step once we received the first actuator output
+		int sleep_time;
+		uint64_t current_wall_time_us;
+
+		if (_last_actuator_output_time <= 0) {
+			PX4_DEBUG("SIH starting up - no lockstep yet");
+			current_wall_time_us = micros();
+			sleep_time = math::max(0, sim_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
+
+		} else {
+			px4_lockstep_wait_for_components();
+			current_wall_time_us = micros();
+			sleep_time = math::max(0, rt_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
+		}
+
+		_achieved_speedup = 0.99f * _achieved_speedup + 0.01f * ((float)sim_interval_us / (float)(
+					    current_wall_time_us - pre_compute_wall_time_us + sleep_time));
+		usleep(sleep_time);
+	}
+}
+#endif
+
+void Sih::realtime_loop()
 {
 	int rate = _imu_gyro_ratemax.get();

@@ -96,20 +175,29 @@ bool Sih::init()

 	// 200 - 2000 Hz
 	int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
-	ScheduleOnInterval(interval_us);

-	return true;
-}
+	px4_sem_init(&_data_semaphore, 0, 0);
+	hrt_call_every(&_timer_call, interval_us, interval_us, timer_callback, &_data_semaphore);

-void Sih::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
+	while (!should_exit()) {
+		px4_sem_wait(&_data_semaphore);     // periodic real time wakeup
+		perf_begin(_loop_perf);
+		sensor_step();
+		perf_end(_loop_perf);
 	}

-	perf_count(_loop_interval_perf);
+	hrt_cancel(&_timer_call);
+	px4_sem_destroy(&_data_semaphore);
+}

+
+void Sih::timer_callback(void *sem)
+{
+	px4_sem_post((px4_sem_t *)sem);
+}
+
+void Sih::sensor_step()
+{
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -170,7 +258,7 @@ void Sih::Run()
 		send_gps();
 	}

-	if (_vehicle == VehicleType::FW && _now - _airspeed_time >= 50_ms) {
+	if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && _now - _airspeed_time >= 50_ms) {
 		_airspeed_time = _now;
 		send_airspeed();
 	}
@@ -284,6 +372,7 @@ void Sih::read_motors()
 	float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);

 	if (_actuator_out_sub.update(&actuators_out)) {
+		_last_actuator_output_time = actuators_out.timestamp;

 		if (_sys_ctrl_alloc.get()) {
 			for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
@@ -598,8 +687,8 @@ float Sih::generate_wgn()   // generate white Gaussian noise sample with std=1

 	if (phase) {
 		do {
-			float U1 = (float)rand() / RAND_MAX;
-			float U2 = (float)rand() / RAND_MAX;
+			float U1 = (float)rand() / (float)RAND_MAX;
+			float U2 = (float)rand() / (float)RAND_MAX;
 			V1 = 2.0f * U1 - 1.0f;
 			V2 = 2.0f * U2 - 1.0f;
 			S = V1 * V1 + V2 * V2;
@@ -623,6 +712,11 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)

 int Sih::print_status()
 {
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	PX4_INFO("Running in lockstep mode");
+	PX4_INFO("Achieved speedup: %.2fX", (double)_achieved_speedup);
+#endif
+
 	if (_vehicle == VehicleType::MC) {
 		PX4_INFO("Running MultiCopter");

@@ -658,27 +752,33 @@ int Sih::print_status()
 	return 0;
 }

+
 int Sih::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("sih",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_MAX,
+				      1250,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+Sih *Sih::instantiate(int argc, char *argv[])
 {
 	Sih *instance = new Sih();

-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
+	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
 	}

-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
+	return instance;
 }

 int Sih::custom_command(int argc, char *argv[])
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2019-2022 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
@@ -58,7 +58,6 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>

 #include <matrix/matrix/math.hpp>   // matrix, vectors, dcm, quaterions
 #include <conversion/rotation.h>    // math::radians,
@@ -67,7 +66,7 @@
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <lib/perf/perf_counter.h>
+#include <perf/perf_counter.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
@@ -81,17 +80,27 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/airspeed.h>

+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+#include <sys/time.h>
+#endif
+
 using namespace time_literals;

-class Sih : public ModuleBase<Sih>, public ModuleParams, public px4::ScheduledWorkItem
+extern "C" __EXPORT int sih_main(int argc, char *argv[]);
+
+class Sih : public ModuleBase<Sih>, public ModuleParams
 {
 public:
 	Sih();
-	~Sih() override;
+
+	virtual ~Sih();

 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);

+	/** @see ModuleBase */
+	static Sih *instantiate(int argc, char *argv[]);
+
 	/** @see ModuleBase */
 	static int custom_command(int argc, char *argv[]);

@@ -101,16 +110,18 @@ public:
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);

+	/** @see ModuleBase::run() */
+	void run() override;
+
 	static float generate_wgn();    // generate white Gaussian noise sample

 	// generate white Gaussian noise sample as a 3D vector with specified std
 	static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);

-	bool init();
+	// timer called periodically to post the semaphore
+	static void timer_callback(void *sem);

 private:
-	void Run() override;
-
 	void parameters_updated();

 	// simulated sensor instances
@@ -171,11 +182,23 @@ private:
 	void publish_sih();
 	void generate_fw_aerodynamics();
 	void generate_ts_aerodynamics();
+	void sensor_step();
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	void lockstep_loop();
+	uint64_t _current_simulation_time_us{0};
+	float _achieved_speedup{0.f};
+#endif
+
+	void realtime_loop();
+	px4_sem_t       _data_semaphore;
+	hrt_call 	_timer_call;

 	perf_counter_t  _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 	perf_counter_t  _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};

 	hrt_abstime _last_run{0};
+	hrt_abstime _last_actuator_output_time{0};
 	hrt_abstime _baro_time{0};
 	hrt_abstime _gps_time{0};
 	hrt_abstime _airspeed_time{0};
@@ -275,7 +298,7 @@ private:
 	// parameters defined in sih_params.c
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _imu_gyro_ratemax,
-
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _imu_integration_rate,
 		(ParamFloat<px4::params::SIH_MASS>) _sih_mass,
 		(ParamFloat<px4::params::SIH_IXX>) _sih_ixx,
 		(ParamFloat<px4::params::SIH_IYY>) _sih_iyy,
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2022 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.
-#
-############################################################################
-px4_add_module(
-	MODULE modules__uorb_explained
-	MAIN uorb_explained
-	STACK_MAIN 1200
-	SRCS
-		uorb_explained.cpp
-	)
@@ -1,5 +0,0 @@
-menuconfig MODULES_UORB_EXPLAINED
-	bool "uorb_explained module"
-	default y
-	---help---
-		uORB Explained Module
@@ -1,240 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 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.
- *
- ****************************************************************************/
-
-/**
- * This module is an example module as part of the PX4 uORB Explained Series Part 4.
- *
- * It simulates publishing and subscribing the pasta_order and pasta_cook uORB topics!
- */
-
-#include "uorb_explained.hpp"
-
-void uORBExplained::run()
-{
-	while (!should_exit()) {
-		hrt_abstime now = hrt_absolute_time();
-
-		// If a customer ordered a new pasta, publish (update) the new order
-		if (customer_order_pasta(now, _pasta_order_pub.get())) {
-			_pasta_order_pub.update();
-		}
-
-		// Handle Waiter relaying the order from the customer to the chef
-		waiter_handle_orders(_pasta_order_sub, _pasta_cook_pub);
-
-		// Handle Chef
-		chef_handle_orders(_pasta_cook_sub);
-
-		// Run the loop at 10 Hz, to not overload the processing power
-		px4_usleep(100_ms);
-	}
-}
-
-bool uORBExplained::customer_order_pasta(hrt_abstime now, pasta_information_s &pasta_order)
-{
-	// [us] Period of customers ordering the pasta
-	static constexpr hrt_abstime PASTA_ORDER_TIME_PERIOD = 1_s;
-
-	static uint8_t CUSTOMER_PASTA_TYPE_PREFERENCE = pasta_information_s::PASTA_TYPE_RIGATONI;
-	static uint8_t CUSTOMER_PASTA_COOKED_TEXTURE_PREFERENCE = pasta_information_s::PASTA_COOKED_AL_DENTE;
-
-	// [deg C] Prefered pasta temperature of the customer
-	static constexpr float CUSTOMER_PASTA_TEMPERATURE_PREFERENCE = 36.5;
-	// [us] Period of the sine curve that will determine the pasta temperature
-	static hrt_abstime PASTA_TEMPERATURE_CURVE_PERIOD = 16_s;
-	// [deg C] Magnitude that will be multiplied to the sine value to calculate temperature
-	static constexpr float PASTA_TEMPERATURE_CURVE_MAGNITUDE = 10.0f;
-
-	static hrt_abstime last_pasta_order_time{0};
-	static uint8_t lasta_pasta_ordered_menu{pasta_information_s::PASTA_MENU_UNDEFINED};
-
-	if ((now - last_pasta_order_time) > PASTA_ORDER_TIME_PERIOD) {
-		pasta_order.timestamp = now;
-
-		// Select MENU: Cycle through the next pasta menu
-		switch (lasta_pasta_ordered_menu) {
-		case pasta_information_s::PASTA_MENU_AGLIO_E_OLIO:
-			pasta_order.menu_name = pasta_information_s::PASTA_MENU_AMATRICIANA;
-			break;
-
-		case pasta_information_s::PASTA_MENU_AMATRICIANA:
-			pasta_order.menu_name = pasta_information_s::PASTA_MENU_CARBONARA;
-			break;
-
-		case pasta_information_s::PASTA_MENU_CARBONARA:
-			pasta_order.menu_name = pasta_information_s::PASTA_MENU_BOLOGNESE;
-			break;
-
-		case pasta_information_s::PASTA_MENU_BOLOGNESE:
-		default:
-			// FALLTHROUGH: If the pasta ordered last time is invalid, order
-			// Aglio e Olio as a default behavior on this loop
-			pasta_order.menu_name = pasta_information_s::PASTA_MENU_AGLIO_E_OLIO;
-		}
-
-		// Set COOKED TEXTURE
-		pasta_order.cooked_texture = CUSTOMER_PASTA_COOKED_TEXTURE_PREFERENCE;
-		// Set TYPE of pasta
-		pasta_order.pasta_type = CUSTOMER_PASTA_TYPE_PREFERENCE;
-
-		// Set TEMPERATURE
-		// It will be a sine curve with a preference value centered, manitude value varying,
-		// period- repeating temperature
-		const float current_phase_rad = ((float)now / PASTA_TEMPERATURE_CURVE_PERIOD) * M_PI_F * 2.0f;
-		const float temperature_adjustment = PASTA_TEMPERATURE_CURVE_MAGNITUDE * sinf(current_phase_rad);
-		pasta_order.pasta_temperature = CUSTOMER_PASTA_TEMPERATURE_PREFERENCE + temperature_adjustment;
-
-		// Update the states
-		last_pasta_order_time = now;
-		lasta_pasta_ordered_menu = pasta_order.menu_name;
-
-		return true;
-	}
-
-	// No order is created
-	return false;
-}
-
-void uORBExplained::waiter_handle_orders(uORB::Subscription &pasta_order_sub,
-		uORB::Publication<pasta_information_s> &pasta_cook_pub)
-{
-	static pasta_information_s customer_order{};
-	// Internal table ID waiter tracks to know to which customer (table) each order belongs to
-	static uint16_t customer_table_id{0};
-
-	// New order from the customer is received
-	if (pasta_order_sub.update(&customer_order)) {
-		// Assign table ID to the customer
-		customer_order.customer_table_id = ++customer_table_id;
-
-		// If the customer isn't sure about the menu, recommend Carbonara!
-		if (customer_order.menu_name == pasta_information_s::PASTA_MENU_UNDEFINED) {
-			customer_order.menu_name = pasta_information_s::PASTA_MENU_CARBONARA;
-		}
-
-		// If customer isn't sure about the cooked texture, recommend the Al Dente!
-		if (customer_order.cooked_texture == pasta_information_s::PASTA_COOKED_UNDEFINED) {
-			customer_order.cooked_texture = pasta_information_s::PASTA_COOKED_AL_DENTE;
-		}
-
-		// If customer isn't sure about the pasta type, recommend Rigatoni!
-		if (customer_order.pasta_type == pasta_information_s::PASTA_TYPE_UNDEFINED) {
-			customer_order.pasta_type = pasta_information_s::PASTA_TYPE_RIGATONI;
-		}
-
-		// Publish so that the chef can receive the order!
-		pasta_cook_pub.publish(customer_order);
-	}
-}
-
-void uORBExplained::chef_handle_orders(uORB::Subscription &pasta_cook_sub)
-{
-	static pasta_information_s waiter_order{};
-
-	// New order from the waiter is received
-	if (pasta_cook_sub.update(&waiter_order)) {
-		PX4_INFO("Chef Cooking new pasta menu %d with temperature %f[deg C]!", waiter_order.menu_name,
-			 (double)waiter_order.pasta_temperature);
-	}
-}
-
-/**
- * "uorb_explained" module start / stop handling function
- */
-int uorb_explained_main(int argc, char *argv[])
-{
-	return uORBExplained::main(argc, argv);
-}
-
-uORBExplained::uORBExplained()
-{
-	// Do nothing in the constructor
-}
-
-
-int uORBExplained::task_spawn(int argc, char *argv[])
-{
-	px4_main_t entry_point = (px4_main_t)&run_trampoline;
-
-	_task_id = px4_task_spawn_cmd("uorb_explained",
-				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_DEFAULT,
-				      1500,
-				      entry_point,
-				      (char *const *)argv);
-
-	if (_task_id < 0) {
-		PX4_INFO("uORB Explained module instantiation Failed!");
-		_task_id = -1;
-		return -errno;
-
-	} else {
-		return PX4_OK;
-
-	}
-}
-
-uORBExplained *uORBExplained::instantiate(int argc, char *argv[])
-{
-	return new uORBExplained();
-}
-
-int uORBExplained::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-
-int uORBExplained::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Publishes and subscribes to pasta_cook and pasta_order uORB messages
-
-### Examples
-CLI usage example:
-$ uorb_explained start
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("uuv_att_control", "controller");
-	PRINT_MODULE_USAGE_COMMAND("start")
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
@@ -1,127 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 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.
- *
- ****************************************************************************/
-
-/**
- *
- * Proof of concept module publishing custom uORB topics
- *
- * This is a material for the 'uORB Explained' espisode of the PX4 Explained Series
- * on the px4.io blog.
- *
- * @author Junwoo Hwang <junwoo091400@gmail.com>
- */
-
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/log.h>
-
-#include <px4_platform_common/module.h>
-
-#include <uORB/uORB.h>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-
-#include <uORB/topics/pasta_information.h>
-
-using namespace time_literals; // To use time literals like "100_ms"
-
-extern "C" __EXPORT int uorb_explained_main(int argc, char *argv[]);
-
-class uORBExplained : public ModuleBase<uORBExplained>
-{
-public:
-	uORBExplained();
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	/** @see ModuleBase */
-	static uORBExplained *instantiate(int argc, char *argv[]);
-
-	/**
-	 * @brief Main run function that runs on a thread
-	 * @see ModuleBase
-	 */
-	void run() override;
-
-private:
-	/**
-	 * If a certain time has passed from the last order, fill in the pasta_order with the
-	 * required pasta specification. Returns true if the order has been filled in.
-	 */
-	bool customer_order_pasta(hrt_abstime now, pasta_information_s &pasta_order);
-
-	/**
-	 * Handles Waiters responsibilities: Checking new order from the customer and relaying it
-	 * to the Chef efficiently
-	 */
-	void waiter_handle_orders(uORB::Subscription &pasta_order_sub, uORB::Publication<pasta_information_s> &pasta_cook_pub);
-
-	/**
-	 * Handle Chef's responsibilities: Checking for a new order from the waiter and cooking the pasta!
-	 */
-	void chef_handle_orders(uORB::Subscription &pasta_cook_sub);
-
-	/**
-	 * Publication for the 'pasta_order'
-	 *
-	 * CUSTOMER -> [pasta_cook] -> WAITER
-	 *
-	 * Imaginary topic where a 'customer' orders the 'waiter' to order a pasta.
-	 */
-	uORB::PublicationData<pasta_information_s> _pasta_order_pub{ORB_ID(pasta_order)};
-
-	/**
-	 * Subscription for 'pasta_order', which is done by waiter to receive order from the customer
-	 */
-	uORB::Subscription _pasta_order_sub{ORB_ID(pasta_order)};
-
-	/**
-	 * Publication for the 'pasta_cook'
-	 *
-	 * WAITER -> [pasta_cook] -> CHEF
-	 *
-	 * Imaginary topic where a 'waiter' orders the 'chef' to cook a certain pasta.
-	 */
-	uORB::Publication<pasta_information_s> _pasta_cook_pub{ORB_ID(pasta_cook)};
-
-	/**
-	 * Subscription for 'pasta_cook', which is done by the chef to receive order from the waiter
-	 */
-	uORB::Subscription _pasta_cook_sub{ORB_ID(pasta_cook)};
-};
@@ -118,9 +118,9 @@ failure gps off
 	}
 }

-int inject_failure(uint8_t unit, uint8_t type, uint8_t instance)
+int inject_failure(const FailureUnit& unit, const FailureType& type, uint8_t instance)
 {
-	PX4_WARN("inject failure unit: %s (%d), type: %s (%d), instance: %d", failure_units[unit].key, unit, failure_types[type].key, type, instance);
+	PX4_WARN("inject failure unit: %s (%d), type: %s (%d), instance: %d", unit.key, unit.value, type.key, type.value, instance);

 	uORB::Subscription command_ack_sub{ORB_ID(vehicle_command_ack)};

@@ -128,8 +128,8 @@ int inject_failure(uint8_t unit, uint8_t type, uint8_t instance)
 	vehicle_command_s command{};

 	command.command = vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE;
-	command.param1 = static_cast<float>(unit);
-	command.param2 = static_cast<float>(type);
+	command.param1 = static_cast<float>(unit.value);
+	command.param2 = static_cast<float>(type.value);
 	command.param3 = static_cast<float>(instance);
 	command.timestamp = hrt_absolute_time();
 	command_pub.publish(command);
@@ -214,7 +214,7 @@ extern "C" __EXPORT int failure_main(int argc, char *argv[])
 				continue;
 			}

-			return inject_failure(failure_unit.value, failure_type.value, instance);
+			return inject_failure(failure_unit, failure_type, instance);
 		}

 		PX4_ERR("Failure type '%s' not found", requested_failure_type);
@@ -139,6 +139,11 @@ public:
 	// Blocking call to get the drone's current position in NED frame
 	std::array<float, 3> get_current_position_ned();

+	void set_param_int(const std::string &param, int32_t value)
+	{
+		CHECK(_param->set_param_int(param, value) == Param::Result::Success);
+	}
+
 protected:
 	mavsdk::Param *getParams() const { return _param.get();}
 	mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
@@ -41,6 +41,7 @@ TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopte
 	AutopilotTesterFailure tester;
 	tester.connect(connection_url);
 	tester.wait_until_ready();
+	tester.set_param_int("SYS_FAILURE_EN", 0);
 	tester.arm();
 	tester.takeoff();
 	tester.wait_until_hovering();
@@ -51,11 +52,3 @@ TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopte
 	tester.wait_until_disarmed(until_disarmed_timeout);
 }

-TEST_CASE("Failure Injection - Reject before arming", "[multicopter]")
-{
-	AutopilotTesterFailure tester;
-	tester.connect(connection_url);
-	tester.wait_until_ready();
-	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, 1,
-			      mavsdk::Failure::Result::Disabled);
-}
Author	SHA1	Message	Date
Matthias Grob	9c616fcf63	HACK to enable actuators in HITL Mixers get loaded on normal output modules the first actuator_outputs instance gets sent to the simulator. No lockdown but the rest of HIL stays.	2022-06-13 10:19:08 +02:00
achim	e6f90bcb81	disable uart´s dma Still no way to get GPS and auto flash of the IO without disabling their uart´s dma	2022-06-11 13:39:01 -04:00
fury1895	283aad01fd	Airspeed params: change default for ASPD_SCALE_APPLY to 2	2022-06-10 14:02:41 +02:00
David Sidrane	f15eb91814	px4_fmu-v6x:HB Mini add Ver 3, Ver 4 init	2022-06-10 04:20:26 -07:00
PX4 BuildBot	e33199c182	Update submodule mavlink to latest Fri Jun 10 00:38:25 UTC 2022 - mavlink in PX4/Firmware (59630bcc7e7e983db2d9138c8254144c77855adc): https://github.com/mavlink/mavlink/commit/0909631552187b998dd6359f998ee78ee8765728 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/05864e218e204f1ebeee5555988150fcddbd873e - Changes: https://github.com/mavlink/mavlink/compare/0909631552187b998dd6359f998ee78ee8765728...05864e218e204f1ebeee5555988150fcddbd873e 05864e21 2022-06-09 Peter Barker - common.xml: add ignition_voltage to EFI_STATUS (#1854) adc9c3f5 2022-06-01 olliw42 - update storm32.xml (nfc) (#1851) 49dbdb66 2022-06-01 Randy Mackay - ardupilotmega: add nav_attitude_time command (#1852)	2022-06-10 01:01:20 -04:00
Thomas Debrunner	46c9d1e288	SIH in SITL with lockstep (#19028 ) * sih: Move sih out of work queue This reverts commit `bb7dd0cf00`. * sih-sim: Enable sih in sitl, together with lockstep * sih-sim: new files for sih: quadx and airplane * sih: Added tailsitter for sih-sitl simulation * sitl_target: Added seperate target loop for sih * jmavsim_run: Allow jmavsim to run in UDP mode * lockstep: Post semaphore on last lockstep component removed * sih-sim: Added display for effectively achieved speed * sih: increase stack size * sih-sim: Improved sleep time computation, fixes bug of running too fast * sitl_target: place omnicopter in alphabethic order Co-authored-by: romain-chiap <romain.chiap@gmail.com> Co-authored-by: Matthias Grob <maetugr@gmail.com>	2022-06-09 09:52:34 +02:00
Thomas Stastny	a7e11464c1	fw pos ctrl: update method documentation	2022-06-09 09:23:02 +02:00
Thomas Stastny	594a6d6e80	fw pos ctrl: some incremental clean up of the class - documentation of units, params, returns, and descriptions for variables and methods - rename ambiguous or erroneous state variables - remove unused or unecessary input arguments to functions - remove ugly header white space	2022-06-09 09:23:02 +02:00
Silvan Fuhrer	9863c24b40	navigator_main: DO_REPOSITION: only trigger reposition setpoint update if vehicle is armed Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-06-08 21:47:06 +02:00
Thomas Stastny	cbf0fe8803	fw pos ctrl: centralize parameter and state resets	2022-06-08 08:44:30 -05:00
Thomas Stastny	ccfbbb553a	fw pos ctrl: only manipulate heading hold yaw in manual position control mode	2022-06-08 08:44:30 -05:00
Thomas Stastny	67d8dd359d	fw pos ctrl: calculate control interval once - Use the same time interval for all position control logic (including TECS) - Sync naming in control methods - Remove some unused input arguments	2022-06-08 08:44:30 -05:00
Matthias Grob	f5e7b1e6a8	Commander: switch battery bitflied to dedicated datatype	2022-06-08 11:32:26 +02:00
Matthias Grob	9b2166de72	manual_control_params: configure arm gesture doesn't require reboot I added wrong documentation here.	2022-06-08 05:12:05 -04:00
Matthias Grob	bd50a52c9c	Commander: fix startup sound interrupted by safety button initialization The safetyButtonHandler() reports that the safety statatus changed on the first loop iteration when safety is disabled which makes sense to inform the system that safety is off but the tune for the user should not be played because it interrupts the startup tune.	2022-06-08 05:12:05 -04:00
Matthias Grob	71103e6114	Safety: keep initialized constant flags when safety disabled	2022-06-08 05:12:05 -04:00
Matthias Grob	44c4b8fa85	Style refactoring related to safety button	2022-06-08 05:12:05 -04:00
Daniel Agar	ce70b6f4ac	sensors/vehicle_angular_velocity: add IMU_GYRO_RATEMAX constraints	2022-06-07 15:45:47 -04:00
Daniel Agar	01f0992f49	sensors/vehicle_imu: don't bother checking IMU_GYRO_RATEMAX	2022-06-07 15:45:47 -04:00
Matthias Grob	5df266cedc	MulticopterRateControl: use constructor to copy thrust setpoint array	2022-06-07 11:45:59 -04:00
bresch	4f1091792f	ekf2 preflight: only check innovation of active height sources	2022-06-07 11:44:56 -04:00
Daniel Agar	cb2738e187	Update submodule GPSDrivers to latest Sat Jun 4 12:38:49 UTC 2022 (#19766 ) - GPSDrivers in PX4/Firmware (6e77a084cd43830c8b13018b7fd5470da9bc4ff5): https://github.com/PX4/PX4-GPSDrivers/commit/181fae1a4b5e33576d786755782adb2f195ecc48 - GPSDrivers current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/016c37cd1f18c716427e2465d8daa6aa1054b0f1 - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/181fae1a4b5e33576d786755782adb2f195ecc48...016c37cd1f18c716427e2465d8daa6aa1054b0f1 016c37c 2022-06-01 Julian Oes - sbf: fix overrun on invalid length	2022-06-04 15:58:56 -04:00
PX4BuildBot	a247b42907	[AUTO COMMIT] update change indication	2022-06-04 15:57:11 -04:00
PX4 BuildBot	e86a74321e	Update world_magnetic_model to latest Sat Jun 4 11:14:10 UTC 2022	2022-06-04 15:57:11 -04:00
Beat Küng	da55256f2f	test_multicopter_failure_injection: remove 'Reject before arming' test Not sure why injection should fail before arming.	2022-06-04 07:40:29 +02:00
Beat Küng	d1142008f6	FailureDetector: check if ESCs have current And increase the timeout to 300ms, as some ESCs only update with 10Hz.	2022-06-04 07:40:29 +02:00
Beat Küng	8bf18e31be	SITL: enable failure command (SYS_FAILURE_EN=1)	2022-06-04 07:40:29 +02:00
Beat Küng	56faaae959	failure: fix invalid memory access failure_units[unit].key was incorrect	2022-06-04 07:40:29 +02:00
Beat Küng	0ab61aee2e	control_allocator: show motor axis for MC (as advanced)	2022-06-04 07:40:29 +02:00
Beat Küng	99a329f937	mc_rate_control: pass through 3D thrust	2022-06-04 07:40:29 +02:00
Beat Küng	82eb71d70b	failure_detector: allow disabling attitude failure (as already documented)	2022-06-04 07:40:29 +02:00
Jaeyoung-Lim	bf68d3433e	Add omnicopter SITL model F	2022-06-04 07:40:29 +02:00
bresch	e2955bdd61	terrain est: clear innovation/var/test ration when aiding stops	2022-06-03 17:31:14 -04:00
Matthias Grob	fad3d46907	Use signNoZero() where possible	2022-06-03 16:08:16 +02:00
abarcis	4fc2640ee3	vehicle_command.msg: unneeded comment closings removed (#19759 ) They cause problems with building px4_msgs in ROS2 Humble Hawksbill and removing them fixes the issue. Co-authored-by: Agata Barcis <agata.barcis@tii.ae>	2022-06-03 07:25:05 +02:00
BA-ED	b9be783b69	dshot: corrected DShot motor spin direction command Some ESCs don't store DShot_cmd_spin_direction_reversed persistently	2022-06-02 13:36:40 +02:00
Thomas Watson	d390e6d46d	boards/mro/pixracerpro: fix voltage/current monitoring This corrects the board definition to use the proper polarity for the brick power valid signal, thus allowing the board to detect the battery and monitor it properly.	2022-06-02 08:11:15 +02:00
Mark Sauder	c19e74784a	attitude_estimator_q: Run() method refactoring (#19526 ) * Refactor attitude_estimator_q_main.cpp Run() method by breaking apart into separate method calls: * update_vehicle_local_position() * update_motion_capture_odometry() * update_visual_odometry() * update_magnetometer() * update_vehicle_attitude() * update_sensors() * Rename init_attitude_q() * Standardize whitespace formatting * Add remaining c++ style initializers.	2022-06-01 13:41:42 -04:00
Daniel Agar	21b1c933dc	ekf2: EKFGSF_yaw delete unnecessary internal state	2022-06-01 13:16:06 -04:00
Daniel Agar	3889b79342	ekf2: yaw estimator add yaw_composite_valid boolean	2022-06-01 13:16:06 -04:00
Igor Mišić	a218f4bfaf	drivers/px4io: support legacy px4io firmware to enter BL	2022-06-01 13:15:13 -04:00
Igor Mišić	25488da944	px4io: replace safety_off state with safety button event (#19558 ) internal PX4IO safety_off state is removed and replaced with a normal safety button event. From this 'commit' commander is taking care of the PX4IO safety.	2022-06-01 13:15:13 -04:00