uORB Explained Blog Post : Add pasta_information uORB message definition

Add details to pasta_information.msg file and add to logged_topics

Add uORBExplained Module (Not complete yet)

Add functioning uorb_explained module

- Implements a functioning module that works based on ModulBae
inheritance
- Edits the rcS script to start the module on start-up
- Implements Customer / Waiter / Chef distinction inside the code by
each function to demonstrate how uORB communication can work

Fix timestamp not logged bug + Make temperature draw sine curve

- More interesting in PlotJuggler :)

ROMFS/px4fmu_common/init.d-posix/airframes/10019_omnicopter

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

ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx

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

ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane

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

ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert

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

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

@@ -35,12 +35,8 @@ 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

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

@@ -30,12 +30,3 @@ 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

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -154,8 +154,6 @@ 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)
@@ -204,11 +202,8 @@ 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
-elif ! replay tryapplyparams
+if ! replay tryapplyparams
 then
   . px4-rc.simulator
 fi
@@ -272,3 +267,5 @@ fi
 
 mavlink boot_complete
 replay trystart
+
+uorb_explained start

ROMFS/px4fmu_common/init.d/rc.interface

@@ -188,7 +188,16 @@ then
 
 	if [ $MIXER_AUX_FILE != none ]
 	then
-		# Enable actuators in HITL
+		# 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
 		if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ]
 		then
 			if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}

Tools/jmavsim_run.sh

@@ -5,13 +5,12 @@ set -e
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 cd "$SCRIPT_DIR/jMAVSim"
 
-port=4560
+tcp_port=4560
 extra_args=
 baudrate=921600
 device=
 ip="127.0.0.1"
-protocol="tcp"
-while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
+while getopts ":b:d:p:qsr:f:i:loat" opt; do
 	case $opt in
 		b)
 			baudrate=$OPTARG
@@ -19,14 +18,11 @@ while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
 		d)
 			device="$OPTARG"
 			;;
-		u)
-			protocol="udp"
-			;;
 		i)
 			ip="$OPTARG"
 			;;
 		p)
-			port=$OPTARG
+			tcp_port=$OPTARG
 			;;
 		q)
 			extra_args="$extra_args -qgc"
@@ -57,11 +53,7 @@ while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
 done
 
 if [ "$device" == "" ]; then
-	if [ "$protocol" == "tcp" ]; then
-		device="-tcp $ip:$port"
-	else
-		device="-udp $port"
-	fi
+	device="-tcp $ip:$tcp_port"
 else
 	device="-serial $device $baudrate"
 fi

Tools/sitl_run.sh

@@ -74,9 +74,6 @@ 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"
@@ -217,12 +214,6 @@ 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

boards/mro/pixracerpro/src/board_config.h

@@ -86,9 +86,9 @@
 #define DIRECT_PWM_OUTPUT_CHANNELS  8
 
 /* Power supply control and monitoring GPIOs */
-#define GPIO_POWER_IN_A                 /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
 
-#define GPIO_VDD_BRICK1_VALID           GPIO_POWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_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_VDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK_VALID   (!px4_arch_gpioread(GPIO_nVDD_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_POWER_IN_A,                  \
+		GPIO_nPOWER_IN_A,                 \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D0), \
 		PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D1), \

boards/mro/x21-777/nuttx-config/nsh/defconfig

@@ -198,9 +198,9 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
-CONFIG_UART4_RXBUFSIZE=1200
-CONFIG_UART4_RXDMA=n
-CONFIG_UART4_TXBUFSIZE=1200
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_RXDMA=y
+CONFIG_UART4_TXBUFSIZE=1500
 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=1200
-CONFIG_USART6_RXDMA=n
-CONFIG_USART6_TXBUFSIZE=1200
-CONFIG_USART6_TXDMA=n
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USART6_TXDMA=y
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -47,7 +47,7 @@ then
 	fi
 fi
 
-if ver hwtypecmp V6X04 V6X14 V6X54
+if ver hwtypecmp V6X04 V6X14
 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 V6X53 V6X54
+if ver hwtypecmp V6X03 V6X13 V6X04 V6X14
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
 	icm42670p -R 10 -s start

boards/px4/sitl/default.px4board

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

msg/CMakeLists.txt

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

msg/pasta_information.msg

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

msg/px4io_status.msg

@@ -19,7 +19,7 @@ bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
-bool status_safety_button_event # px4io safety button was pressed for longer than 1 second
+bool status_safety_off
 
 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
 bool alarm_pwm_error

msg/vehicle_command.msg

@@ -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|

msg/vehicle_status.msg

@@ -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

msg/yaw_estimator_status.msg

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

platforms/posix/cmake/sitl_target.cmake

@@ -9,6 +9,7 @@ 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}
@@ -158,7 +159,6 @@ set(models
 	iris_rplidar
 	iris_vision
 	nxp_cupcar
-	omnicopter
 	plane
 	plane_cam
 	plane_catapult
@@ -324,70 +324,6 @@ 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

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp

@@ -84,7 +84,7 @@ void LockstepComponents::unregister_component(int component)
 
 	int components_used_bitset = _components_used_bitset;
 
-	if (_components_progress_bitset == components_used_bitset) {
+	if (_components_progress_bitset == components_used_bitset && components_used_bitset != 0) {
 		_components_progress_bitset = 0;
 		px4_sem_post(&_components_sem);
 	}

src/drivers/drv_pwm_output.h

@@ -188,6 +188,12 @@ 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)
+
 /*
  *
  *

src/drivers/dshot/DShot.cpp

@@ -752,8 +752,8 @@ int DShot::custom_command(int argc, char *argv[])
 	};
 
 	constexpr VerbCommand commands[] = {
-		{"reverse", DShot_cmd_spin_direction_2, 10},
-		{"normal", DShot_cmd_spin_direction_1, 10},
+		{"reverse", DShot_cmd_spin_direction_reversed, 10},
+		{"normal", DShot_cmd_spin_direction_normal, 10},
 		{"save", DShot_cmd_save_settings, 10},
 		{"3d_on", DShot_cmd_3d_mode_on, 10},
 		{"3d_off", DShot_cmd_3d_mode_off, 10},

src/drivers/pca9685_pwm_out/main.cpp

@@ -492,7 +492,9 @@ 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;

src/drivers/pwm_out/PWMOut.cpp

@@ -717,6 +717,8 @@ 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:

src/drivers/px4io/px4io.cpp

@@ -74,7 +74,6 @@
 #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>
 
@@ -197,8 +196,7 @@ 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_status{ORB_ID(vehicle_status)};		///< vehicle status topic
+	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};		///< vehicle command topic
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
@@ -445,14 +443,15 @@ 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;
 	}
@@ -477,6 +476,14 @@ 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);
@@ -614,6 +621,11 @@ 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 */
@@ -962,12 +974,15 @@ 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_ARM_SYNC)) {
+	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+	    && !(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_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
+				    PX4IO_P_STATUS_FLAGS_SAFETY_OFF | 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)) {
 
@@ -984,26 +999,18 @@ int PX4IO::io_handle_status(uint16_t status)
 	}
 
 	/**
-	 * Get and handle the safety button status
+	 * Get and handle the safety status
 	 */
-	const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
+	const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 
-	if (safety_button_pressed) {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
+	/* 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) {
 		_button_publisher.safetyButtonTriggerEvent();
 	}
 
-	/**
-	 * 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;
-		}
-	}
+	_previous_safety_off = safety_off;
 
 	return ret;
 }
@@ -1026,17 +1033,37 @@ 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"));
 
-	int ret = OK;
-	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
 	px4_usleep(500000);
-	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);
+	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);
 	px4_usleep(72000);
-	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
+	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
 	px4_usleep(50000);
-	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+	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;
+	}
 
 	if (ret != OK) {
 		PX4_INFO("Binding DSM failed");
@@ -1106,7 +1133,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_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
+		status.status_safety_off      = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 
 		// PX4IO_P_STATUS_ALARMS
 		status.alarm_rc_lost       = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
@@ -1617,6 +1644,18 @@ 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);
@@ -1686,10 +1725,15 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 		}
 
-		/* 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);
+		// 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);
 
 		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
 		usleep(1);
@@ -2014,6 +2058,29 @@ 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");
@@ -2095,6 +2162,8 @@ 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");

src/drivers/uavcan/uavcan_main.cpp

@@ -951,6 +951,8 @@ 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:

src/lib/world_magnetic_model/geo_magnetic_tables.hpp

@@ -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.4219,
+// Date: 2022.3835,
 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 */ {  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, },
+	/* 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, },
 };
 
 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.4219,
+// Date: 2022.3835,
 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 */ { -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 */ { -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 */ {  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.4219,
+// Date: 2022.3835,
 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 */ {  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: -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:  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, },
 };

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -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, 2);
+PARAM_DEFINE_INT32(ASPD_SCALE_APPLY, 1);
 
 /**
  * Scale of airspeed sensor 1

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -92,82 +92,67 @@ private:
 
 	void Run() override;
 
-	bool init_attitude_q();
-
-	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 init_attq();
+
 	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::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::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)};
+	uORB::Publication<vehicle_attitude_s>	_att_pub{ORB_ID(vehicle_attitude)};
 
-	uORB::Publication<vehicle_attitude_s> _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};
+	float		_mag_decl{0.0f};
+	float		_bias_max{0.0f};
 
-	Vector3f    _accel{};
-	Vector3f    _gyro{};
-	Vector3f    _gyro_bias{};
-	Vector3f    _rates{};
+	Vector3f	_gyro;
+	Vector3f	_accel;
+	Vector3f	_mag;
 
-	Vector3f    _mag{};
-	Vector3f    _mocap_hdg{};
-	Vector3f    _vision_hdg{};
+	Vector3f	_vision_hdg;
+	Vector3f	_mocap_hdg;
 
-	Vector3f    _pos_acc{};
-	Vector3f    _vel_prev{};
+	Quatf		_q;
+	Vector3f	_rates;
+	Vector3f	_gyro_bias;
 
-	Quatf       _q{};
+	Vector3f	_vel_prev;
+	hrt_abstime	_vel_prev_t{0};
 
-	hrt_abstime _imu_timestamp{};
-	hrt_abstime _imu_prev_timestamp{};
-	hrt_abstime _vel_prev_timestamp{};
+	Vector3f	_pos_acc;
 
-	float       _bias_max{};
-	float       _mag_decl{};
+	hrt_abstime	_last_time{0};
 
-	bool        _data_good{false};
-	bool        _ext_hdg_good{false};
-	bool        _initialized{false};
+	bool		_inited{false};
+	bool		_data_good{false};
+	bool		_ext_hdg_good{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
 	)
 };
 
@@ -175,10 +160,25 @@ 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,7 +188,8 @@ bool AttitudeEstimatorQ::init()
 	return true;
 }
 
-void AttitudeEstimatorQ::Run()
+void
+AttitudeEstimatorQ::Run()
 {
 	if (should_exit()) {
 		_sensors_sub.unregisterCallback();
@@ -196,94 +197,14 @@ void 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 validator with recent sensor data
+
+		update_parameters();
+
+		// Feed 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];
@@ -299,93 +220,148 @@ void AttitudeEstimatorQ::update_sensors()
 				return;
 			}
 		}
-	}
-}
 
-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;
+		// Update magnetometer
+		if (_magnetometer_sub.updated()) {
+			vehicle_magnetometer_s magnetometer;
 
-	if (update(dt)) {
-		vehicle_attitude_s vehicle_attitude{};
-		vehicle_attitude.timestamp_sample = _imu_timestamp;
-		_q.copyTo(vehicle_attitude.q);
+			if (_magnetometer_sub.copy(&magnetometer)) {
+				_mag(0) = magnetometer.magnetometer_ga[0];
+				_mag(1) = magnetometer.magnetometer_ga[1];
+				_mag(2) = magnetometer.magnetometer_ga[2];
 
-		/* 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);
+				if (_mag.length() < 0.01f) {
+					PX4_ERR("degenerate mag!");
+					return;
 				}
-
-				_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_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)
+void
+AttitudeEstimatorQ::update_parameters(bool force)
 {
 	// check for parameter updates
 	if (_parameter_update_sub.updated() || force) {
@@ -412,7 +388,8 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 	}
 }
 
-bool AttitudeEstimatorQ::init_attitude_q()
+bool
+AttitudeEstimatorQ::init_attq()
 {
 	// Rotation matrix can be easily constructed from acceleration and mag field vectors
 	// 'k' is Earth Z axis (Down) unit vector in body frame
@@ -444,24 +421,25 @@ bool AttitudeEstimatorQ::init_attitude_q()
 	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) {
-		_initialized = true;
+		_inited = true;
 
 	} else {
-		_initialized = false;
+		_inited = false;
 	}
 
-	return _initialized;
+	return _inited;
 }
 
-bool AttitudeEstimatorQ::update(float dt)
+bool
+AttitudeEstimatorQ::update(float dt)
 {
-	if (!_initialized) {
+	if (!_inited) {
 
 		if (!_data_good) {
 			return false;
 		}
 
-		return init_attitude_q();
+		return init_attq();
 	}
 
 	Quatf q_last = _q;
@@ -565,10 +543,11 @@ bool 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 (!_initialized || fabsf(new_declination - _mag_decl) < 0.0001f) {
+	if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
 		_mag_decl = new_declination;
 
 	} else {
@@ -579,12 +558,14 @@ void 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();
 
@@ -607,7 +588,8 @@ int 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);

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp

@@ -127,7 +127,8 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		}
 
 		if (hil_enabled) {
-			// Enable actuators in HITL
+			/* enforce lockdown in HIL */
+			armed.lockdown = true;
 			status_flags.system_sensors_initialized = true;
 
 			/* recover from a prearm fail */

src/modules/commander/Commander.cpp

@@ -2298,22 +2298,21 @@ Commander::run()
 		}
 
 		/* safety button */
-		const bool safety_changed = _safety.safetyButtonHandler();
+		bool safety_updated = _safety.safetyButtonHandler();
 		_vehicle_status.safety_button_available = _safety.isButtonAvailable();
 		_vehicle_status.safety_off = _safety.isSafetyOff();
 
-		if (safety_changed) {
+		if (safety_updated) {
+
 			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.isSafetyDisabled()) {
-				if (_safety.isSafetyOff()) {
-					set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
+			if (_safety.isSafetyOff()) {
+				set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
 
-				} else {
-					tune_neutral(true);
-				}
+			} else {
+				tune_neutral(true);
 			}
 
 			_status_changed = true;
@@ -2429,8 +2428,10 @@ Commander::run()
 			bool auto_disarm = _actuator_armed.manual_lockdown;
 
 			// auto disarm if locked down to avoid user confusion
-			// Enable actuators in HITL
-			auto_disarm |= _armed.lockdown;
+			//  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;
+			}
 
 			_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());
 
@@ -3804,7 +3805,7 @@ void Commander::avoidance_check()
 
 void Commander::battery_status_check()
 {
-	size_t battery_required_count = 0;
+	int 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
@@ -3829,7 +3830,7 @@ void Commander::battery_status_check()
 
 		if (_arm_state_machine.isArmed()) {
 
-			if (_last_connected_batteries[index] && !battery.connected) {
+			if ((_last_connected_batteries & (1 << 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);
@@ -3846,7 +3847,13 @@ void Commander::battery_status_check()
 			}
 		}
 
-		_last_connected_batteries.set(index, battery.connected);
+		if (battery.connected) {
+			_last_connected_batteries |= 1 << index;
+
+		} else {
+			_last_connected_batteries &= ~(1 << index);
+		}
+
 		_last_battery_mode[index] = battery.mode;
 
 		if (battery.connected) {
@@ -3951,7 +3958,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)
-		&& (_last_connected_batteries.count() >= battery_required_count)
+		&& (math::countSetBits(_last_connected_batteries) >= 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

src/modules/commander/Commander.hpp

@@ -41,7 +41,6 @@
 #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>
@@ -348,7 +347,7 @@ private:
 
 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime	_battery_failsafe_timestamp{0};
-	px4::Bitset<battery_status_s::MAX_INSTANCES> _last_connected_batteries;
+	uint8_t		_last_connected_batteries{0};
 	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] {};
@@ -403,7 +402,7 @@ private:
 	vehicle_status_s        _vehicle_status{};
 	vehicle_status_flags_s  _vehicle_status_flags{};
 
-	Safety _safety;
+	Safety _safety{};
 
 	WorkerThread _worker_thread;
 

src/modules/commander/Safety.cpp

@@ -42,18 +42,20 @@ 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) {
+	if (_safety_disabled) {
+		_button_available = true;
+		_safety_off = true;
+
+	} else {
+
 		if (!_button_available && _safety_button_sub.advertised()) {
 			_button_available = true;
 		}
@@ -65,8 +67,10 @@ bool Safety::safetyButtonHandler()
 		}
 	}
 
-	const bool safety_changed = _previous_safety_off != _safety_off;
+	bool safety_changed = _previous_safety_off != _safety_off;
+
 	_previous_safety_off = _safety_off;
+
 	return safety_changed;
 }
 

src/modules/commander/Safety.hpp

@@ -43,21 +43,23 @@
 
 class Safety
 {
+
 public:
+
 	Safety();
 	~Safety() = default;
 
 	bool safetyButtonHandler();
 	void activateSafety();
-	bool isButtonAvailable() { return _button_available; }
-	bool isSafetyOff() { return _safety_off; }
-	bool isSafetyDisabled() { return _safety_disabled; }
+	bool isButtonAvailable() { return _button_available;}
+	bool isSafetyOff() { return _safety_off;}
 
 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
 };

src/modules/commander/commander_params.c

@@ -927,7 +927,7 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0);
 /**
  * Enable force safety
  *
- * Force safety when the vehicle disarms
+ * Force safety when the vehicle disarms. Not supported when safety button used over PX4IO board.
  *
  * @boolean
  * @group Commander

src/modules/commander/esc_calibration.cpp

@@ -187,6 +187,13 @@ 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();
@@ -226,6 +233,9 @@ 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");

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -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 > FLT_EPSILON) && (fabsf(roll) > max_roll);
-		const bool pitch_status = (max_pitch > FLT_EPSILON) && (fabsf(pitch) > max_pitch);
+		const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
+		const bool pitch_status = (max_pitch > 0.0f) && (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 > 300_ms;
+			const bool esc_timed_out = telemetry_age > 100_ms;  // TODO: magic number
 
 			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,40 +399,35 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
 			}
 
 			// Check if ESC current is too low
-			if (cur_esc_report.esc_current > FLT_EPSILON) {
-				_motor_failure_escs_have_current = true;
+			float esc_throttle = 0.f;
+
+			if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
+				esc_throttle = fabsf(actuator_motors.control[i_esc]);
 			}
 
-			if (_motor_failure_escs_have_current) {
-				float esc_throttle = 0.f;
+			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 (PX4_ISFINITE(actuator_motors.control[i_esc])) {
-					esc_throttle = fabsf(actuator_motors.control[i_esc]);
+			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;
 				}
 
-				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;
-					}
+			} 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);

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -129,7 +129,6 @@ 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)};

src/modules/commander/failure_detector/failure_detector_params.c

@@ -53,7 +53,7 @@
  *
  * Setting this parameter to 0 disables the check
  *
- * @min 0
+ * @min 60
  * @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 0
+ * @min 60
  * @max 180
  * @unit deg
  * @group Failure Detector

src/modules/control_allocator/module.yaml

@@ -556,18 +556,6 @@ 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'

src/modules/ekf2/EKF/EKFGSF_yaw.cpp

@@ -23,6 +23,7 @@ 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
@@ -59,7 +60,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 && _vel_data_updated) {
+	if (_run_ekf_gsf && _vel_data_updated) {
 		if (!_ekf_gsf_vel_fuse_started) {
 			initialiseEKFGSF();
 			ahrsAlignYaw();
@@ -110,7 +111,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
 			}
 		}
 
-	} else if (_ekf_gsf_vel_fuse_started && !run_EKF) {
+	} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
 		// wait to fly again
 		_ekf_gsf_vel_fuse_started = false;
 	}

src/modules/ekf2/EKF/EKFGSF_yaw.h

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

src/modules/ekf2/EKF/ekf.h

@@ -338,9 +338,6 @@ 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; }
@@ -1065,6 +1062,9 @@ 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

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -194,10 +194,6 @@ 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()
@@ -294,10 +290,6 @@ 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()

src/modules/ekf2/EKF2.cpp

@@ -877,11 +877,6 @@ 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) {
@@ -1389,7 +1384,6 @@ 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();
 

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -102,29 +102,10 @@ bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s
 
 bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
 {
-	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;
+	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 PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
@@ -146,10 +127,6 @@ 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;
@@ -157,10 +134,7 @@ void PreFlightChecker::reset()
 	_filter_vel_n_innov.reset();
 	_filter_vel_e_innov.reset();
 	_filter_vel_d_innov.reset();
-	_filter_baro_hgt_innov.reset();
-	_filter_gps_hgt_innov.reset();
-	_filter_rng_hgt_innov.reset();
-	_filter_ev_hgt_innov.reset();
+	_filter_hgt_innov.reset();
 	_filter_heading_innov.reset();
 	_filter_flow_x_innov.reset();
 	_filter_flow_y_innov.reset();

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -79,11 +79,6 @@ 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; }
@@ -154,25 +149,15 @@ 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)

src/modules/ekf2/test/change_indication/iris_gps.csv

@@ -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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.29e-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.28e-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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.45e-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.44e-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.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
+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
 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.64e-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.63e-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.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
+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
 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.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
+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
 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.89e-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.9e-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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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
 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.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
+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

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -67,7 +67,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	_pos_ctrl_landing_status_pub.advertise();
 	_tecs_status_pub.advertise();
 
-	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
+	_slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE);
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -283,7 +283,7 @@ FixedwingPositionControl::airspeed_poll()
 
 			airspeed_valid = true;
 
-			_time_airspeed_last_valid = airspeed_validated.timestamp;
+			_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(&_time_airspeed_last_valid) > 1_s)) {
+		if (airspeed_valid && (hrt_elapsed_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) < WIND_EST_TIMEOUT;
+		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < T_WIND_EST_TIMEOUT;
 	}
 }
 
@@ -330,21 +330,21 @@ FixedwingPositionControl::manual_control_setpoint_poll()
 {
 	_manual_control_setpoint_sub.update(&_manual_control_setpoint);
 
-	_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);
+	_manual_control_setpoint_altitude = _manual_control_setpoint.x;
+	_manual_control_setpoint_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_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;
+		_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;
 	}
 
 	// send neutral setpoints if no update for 1 s
 	if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
-		_manual_control_setpoint_for_height_rate = 0.f;
-		_manual_control_setpoint_for_airspeed = 0.5f;
+		_manual_control_setpoint_altitude = 0.f;
+		_manual_control_setpoint_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_for_airspeed < 0.5f) {
+		if (_manual_control_setpoint_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_for_airspeed * 2;
+					   _manual_control_setpoint_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_for_airspeed * 2 - 1);
+					   (_manual_control_setpoint_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 float control_interval, const float pos_sp_cruise_airspeed,
-		const Vector2f &ground_speed)
+FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cruise_airspeed,
+		const Vector2f &ground_speed, float dt)
 {
 	// 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 float control_interva
 	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 = _airspeed_slew_rate_controller.getState() < airspeed_min_adjusted
-				       || _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();
+	const bool outside_of_limits = _slew_rate_airspeed.getState() < airspeed_min_adjusted
+				       || _slew_rate_airspeed.getState() > _param_fw_airspd_max.get();
 
-	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || outside_of_limits) {
-		_airspeed_slew_rate_controller.setForcedValue(airspeed_setpoint);
+	if (!PX4_ISFINITE(_slew_rate_airspeed.getState()) || outside_of_limits) {
+		_slew_rate_airspeed.setForcedValue(airspeed_setpoint);
 
-	} else if (control_interval > FLT_EPSILON) {
+	} else if (dt > FLT_EPSILON) {
 		// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
-		airspeed_setpoint = _airspeed_slew_rate_controller.update(airspeed_setpoint, control_interval);
+		airspeed_setpoint = _slew_rate_airspeed.update(airspeed_setpoint, dt);
 	}
 
 	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_for_height_rate > deadBand) {
+	if (_manual_control_setpoint_altitude > deadBand) {
 		/* pitching down */
-		float pitch = -(_manual_control_setpoint_for_height_rate - deadBand) / factor;
+		float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
 		height_rate_setpoint = pitch * _param_sinkrate_target.get();
 
-	} else if (_manual_control_setpoint_for_height_rate < - deadBand) {
+	} else if (_manual_control_setpoint_altitude < - deadBand) {
 		/* pitching up */
-		float pitch = -(_manual_control_setpoint_for_height_rate + deadBand) / factor;
+		float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
 		const float climb_rate_target = _param_climbrate_target.get();
 
 		height_rate_setpoint = pitch * climb_rate_target;
@@ -844,6 +844,16 @@ 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)
 {
@@ -875,14 +885,24 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
 }
 
 void
-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)
+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)
 {
+	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);
+	const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
 
 	_position_sp_type = position_sp_type;
 
@@ -891,8 +911,20 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		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;
@@ -904,15 +936,15 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
-		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
-		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
+		control_auto_velocity(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
-		control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
+		control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
 		break;
 	}
 
@@ -940,12 +972,11 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 }
 
 void
-FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
+FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now)
 {
 	// only control altitude and airspeed ("fixed-bank loiter")
 
-	tecs_update_pitch_throttle(control_interval,
-				   _current_altitude,
+	tecs_update_pitch_throttle(now, _current_altitude,
 				   _param_fw_airspd_trim.get(),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -972,14 +1003,15 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 }
 
 void
-FixedwingPositionControl::control_auto_descend(const float control_interval)
+FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
 {
+	// 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(control_interval,
-				   _current_altitude,
+	tecs_update_pitch_throttle(now, _current_altitude,
 				   _param_fw_airspd_trim.get(),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1002,10 +1034,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 }
 
 uint8_t
-FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::handle_setpoint_type(const uint8_t 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;
 	}
@@ -1017,6 +1047,8 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 
 	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) {
 
@@ -1059,13 +1091,14 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 }
 
 void
-FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
+FixedwingPositionControl::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)
 {
 	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);
 
@@ -1141,8 +1174,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 		}
 	}
 
-	float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
-
+	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
 	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));
@@ -1176,8 +1208,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
-	tecs_update_pitch_throttle(control_interval,
-				   position_sp_alt,
+	tecs_update_pitch_throttle(now, position_sp_alt,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1189,8 +1220,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 }
 
 void
-FixedwingPositionControl::control_auto_velocity(const float control_interval, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
+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)
 {
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
@@ -1224,7 +1255,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	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(control_interval, pos_sp_curr.cruising_speed, ground_speed);
+	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
 
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
@@ -1243,8 +1274,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
-	tecs_update_pitch_throttle(control_interval,
-				   position_sp_alt,
+	tecs_update_pitch_throttle(now, position_sp_alt,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1258,7 +1288,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 }
 
 void
-FixedwingPositionControl::control_auto_loiter(const float control_interval, const Vector2d &curr_pos,
+FixedwingPositionControl::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)
 {
@@ -1319,7 +1349,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = signNoZero(loiter_radius);
+		loiter_direction = (loiter_radius > 0) ? 1 : -1;
 	}
 
 	const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
@@ -1339,7 +1369,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 	}
 
-	float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_sp, ground_speed);
+	float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
 
 	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));
@@ -1380,8 +1410,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 	}
 
-	tecs_update_pitch_throttle(control_interval,
-				   alt_sp,
+	tecs_update_pitch_throttle(now, alt_sp,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -1393,10 +1422,31 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 }
 
 void
-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)
+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)
 {
+	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 */
@@ -1441,8 +1491,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
 				       _current_latitude, _current_longitude, &_mavlink_log_pub);
 
-		float target_airspeed = get_auto_airspeed_setpoint(control_interval,
-					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed);
+		float target_airspeed = get_auto_airspeed_setpoint(now,
+					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
+					dt);
 
 		/*
 		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
@@ -1471,8 +1522,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		// update tecs
 		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
 
-		tecs_update_pitch_throttle(control_interval,
-					   pos_sp_curr.alt,
+		tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(takeoff_pitch_max_deg),
@@ -1510,7 +1560,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 				}
 
 				/* Detect launch using body X (forward) acceleration */
-				_launchDetector.update(control_interval, _body_acceleration(0));
+				_launchDetector.update(dt, _body_acceleration(0));
 
 				/* update our copy of the launch detection state */
 				_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1525,7 +1575,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		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(control_interval, _param_fw_airspd_trim.get(), ground_speed);
+			float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
+						dt);
 
 			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
 
@@ -1559,8 +1610,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			/* 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(control_interval,
-							   pos_sp_curr.alt,
+				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(takeoff_pitch_max_deg),
@@ -1574,8 +1624,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
 
 			} else {
-				tecs_update_pitch_throttle(control_interval,
-							   pos_sp_curr.alt,
+				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
@@ -1630,10 +1679,30 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 }
 
 void
-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)
+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)
 {
+	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());
 
@@ -1731,10 +1800,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 			// all good, have valid terrain altitude
 			float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
 			terrain_alt = (_local_pos.ref_alt - terrain_vpos);
-			_last_valid_terrain_alt_estimate = terrain_alt;
-			_last_time_terrain_alt_was_valid = now;
+			_t_alt_prev_valid = terrain_alt;
+			_time_last_t_alt = now;
 
-		} else if (_last_time_terrain_alt_was_valid == 0) {
+		} else if (_time_last_t_alt == 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
@@ -1747,16 +1816,16 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 				abort_landing(true);
 			}
 
-		} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT)
+		} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_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 = _last_valid_terrain_alt_estimate;
+			terrain_alt = _t_alt_prev_valid;
 
 		} else {
 			// terrain alt was not valid for long time, abort landing
-			terrain_alt = _last_valid_terrain_alt_estimate;
+			terrain_alt = _t_alt_prev_valid;
 			abort_landing(true);
 		}
 	}
@@ -1799,7 +1868,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		}
 
 		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed);
+		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt);
 
 		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
 
@@ -1847,8 +1916,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 		}
 
-		tecs_update_pitch_throttle(control_interval,
-					   terrain_alt + flare_curve_alt_rel,
+		tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
 					   target_airspeed,
 					   radians(_param_fw_lnd_fl_pmin.get()),
 					   radians(_param_fw_lnd_fl_pmax.get()),
@@ -1917,7 +1985,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		}
 
 		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_approach, ground_speed);
+		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt);
 
 		/* lateral guidance */
 		if (_param_fw_use_npfg.get()) {
@@ -1951,8 +2019,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 
 		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
-		tecs_update_pitch_throttle(control_interval,
-					   altitude_desired,
+		tecs_update_pitch_throttle(now, altitude_desired,
 					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
@@ -1978,10 +2045,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 }
 
 void
-FixedwingPositionControl::control_manual_altitude(const float control_interval, const Vector2d &curr_pos,
+FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, 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();
@@ -2007,12 +2075,11 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 	/* throttle limiting */
 	float throttle_max = _param_fw_thr_max.get();
 
-	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
+	if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}
 
-	tecs_update_pitch_throttle(control_interval,
-				   altitude_sp_amsl,
+	tecs_update_pitch_throttle(now, altitude_sp_amsl,
 				   altctrl_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -2045,9 +2112,19 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 }
 
 void
-FixedwingPositionControl::control_manual_position(const float control_interval, const Vector2d &curr_pos,
+FixedwingPositionControl::control_manual_position(const hrt_abstime &now, 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
@@ -2069,7 +2146,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 	/* throttle limiting */
 	float throttle_max = _param_fw_thr_max.get();
 
-	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
+	if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}
 
@@ -2141,8 +2218,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		}
 	}
 
-	tecs_update_pitch_throttle(control_interval,
-				   altitude_sp_amsl,
+	tecs_update_pitch_throttle(now, altitude_sp_amsl,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
@@ -2164,9 +2240,9 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		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 (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);
+		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);
 		}
 
 		_att_sp.roll_body = roll_sp_new;
@@ -2227,10 +2303,6 @@ 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
@@ -2371,63 +2443,42 @@ FixedwingPositionControl::Run()
 
 		set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
 
-		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;
+		_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
 
 		switch (_control_mode_current) {
 		case FW_POSCTRL_MODE_AUTO: {
-				control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
+				control_auto(_local_pos.timestamp, 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(control_interval);
+				control_auto_fixed_bank_alt_hold(_local_pos.timestamp);
 				break;
 			}
 
 		case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
-				control_auto_descend(control_interval);
+				control_auto_descend(_local_pos.timestamp);
 				break;
 			}
 
 		case FW_POSCTRL_MODE_AUTO_LANDING: {
-				control_auto_landing(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
-						     _pos_sp_triplet.current);
+				control_auto_landing(_local_pos.timestamp, 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, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
-						     _pos_sp_triplet.current);
+				control_auto_takeoff(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
 				break;
 			}
 
 		case FW_POSCTRL_MODE_MANUAL_POSITION: {
-				control_manual_position(control_interval, curr_pos, ground_speed);
+				control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
 				break;
 			}
 
 		case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
-				control_manual_altitude(control_interval, curr_pos, ground_speed);
+				control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed);
 				break;
 			}
 
@@ -2518,7 +2569,7 @@ FixedwingPositionControl::reset_landing_state()
 	_time_started_landing = 0;
 
 	// reset terrain estimation relevant values
-	_last_time_terrain_alt_was_valid = 0;
+	_time_last_t_alt = 0;
 
 	_land_noreturn_horizontal = false;
 	_land_noreturn_vertical = false;
@@ -2563,12 +2614,15 @@ FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
 }
 
 void
-FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
+FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, 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;
 
@@ -2586,25 +2640,24 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 			// 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
-				_airspeed_after_transition = _param_airspeed_trans;
+				_asp_after_transition = _param_airspeed_trans;
 
 			} else {
-				_airspeed_after_transition = _airspeed;
+				_asp_after_transition = _airspeed;
 			}
 
-			_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
-							       _param_fw_airspd_max.get());
+			_asp_after_transition = constrain(_asp_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
-			_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s
+			_asp_after_transition += dt * 2.0f; // increase 2m/s
 
-			if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
-				airspeed_sp = max(_airspeed_after_transition, _airspeed);
+			if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
+				airspeed_sp = max(_asp_after_transition, _airspeed);
 
 			} else {
 				_was_in_transition = false;
-				_airspeed_after_transition = 0.0f;
+				_asp_after_transition = 0.0f;
 			}
 		}
 	}
@@ -2653,21 +2706,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	_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();
 }
@@ -2705,7 +2751,7 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
 
 	if (fabsf(loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = signNoZero(loiter_radius);
+		loiter_direction = (loiter_radius > 0) ? 1 : -1;
 	}
 
 	orbit_status.radius = static_cast<float>(loiter_direction) * loiter_radius;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -103,38 +103,25 @@ using namespace time_literals;
 using matrix::Vector2d;
 using matrix::Vector2f;
 
-// [m] initial distance of waypoint in front of plane in heading hold mode
-static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;
+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] 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 hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid
 
-// [m] distance by which previous waypoint is set behind the plane
-static constexpr float HDG_HOLD_SET_BACK_DIST = 100.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
 
-// [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;
+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]
 
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -177,45 +164,44 @@ 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)};
-	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)};
+	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)};
 
-	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
+	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
 
 	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};
 
-	// [m] ground altitude where the plane was launched
-	float _takeoff_ground_alt{0.0f};
+	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
 
-	// [rad] yaw setpoint for manual position mode heading hold
-	float _hdg_hold_yaw{0.0f};
+	float	_min_current_sp_distance_xy{FLT_MAX};
 
-	bool _hdg_hold_enabled{false}; // heading hold enabled
-	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
+	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
 
-	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
+	/**
+	 * @brief Last absolute time position control has been called [us]
+	 *
+	 */
 	hrt_abstime _last_time_position_control_called{0};
 
 	bool _landed{true};
@@ -230,60 +216,38 @@ private:
 
 	Landingslope _landingslope;
 
-	// [us] time at which landing started
-	hrt_abstime _time_started_landing{0};
+	hrt_abstime _time_started_landing{0};			///< time at which landing started
 
-	// [m] last terrain estimate which was valid
-	float _last_valid_terrain_alt_estimate{0.0f};
+	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
 
-	// [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_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
 	float _flare_curve_alt_rel_last{0.0f};
+	float _target_bearing{0.0f};				///< estimated height to ground at which flare started
 
-	float _target_bearing{0.0f}; // [rad]
+	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
 
-	// 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
+	/* Takeoff launch detection and runway */
 	LaunchDetector _launchDetector;
 	LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
 	hrt_abstime _launch_detection_notify{0};
 
 	RunwayTakeoff _runway_takeoff;
 
-	// true if the last iteration was in manual mode (used to determine when a reset is needed)
-	bool _last_manual{false};
+	bool _last_manual{false};				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
 
 	/* throttle and airspeed states */
-
-	bool _airspeed_valid{false};
-
-	// [us] last time airspeed was received. used to detect timeouts.
-	hrt_abstime _time_airspeed_last_valid{0};
-
+	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.
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};
 
 	/* wind estimates */
-
-	// [m/s] wind velocity vector
-	Vector2f _wind_vel{0.0f, 0.0f};
-
-	bool _wind_valid{false};
-
-	hrt_abstime _time_wind_last_received{0}; // [us]
+	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.
 
 	float _pitch{0.0f};
 	float _yaw{0.0f};
@@ -292,48 +256,32 @@ private:
 	matrix::Vector3f _body_acceleration{};
 	matrix::Vector3f _body_velocity{};
 
-	bool _reinitialize_tecs{true};
+	bool _reinitialize_tecs{true};				///< indicates if the TECS states should be reinitialized (used for VTOL)
 	bool _is_tecs_running{false};
+	hrt_abstime _last_tecs_update{0};
 
-	hrt_abstime _time_last_tecs_update{0}; // [us]
-
-	float _airspeed_after_transition{0.0f};
+	float _asp_after_transition{0.0f};
 	bool _was_in_transition{false};
 
-	bool _is_vtol_tailsitter{false};
+	bool _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
 
-	// captures the number of times the estimator has reset the horizontal position
-	uint8_t _pos_reset_counter{0};
+	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 altitude state
-	uint8_t _alt_reset_counter{0};
+	hrt_abstime _time_in_fixed_bank_loiter{0};
 
-	// [.] 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
+	ECL_L1_Pos_Controller	_l1_control;
 	NPFG _npfg;
-
-	// total energy control system - airspeed / altitude control
-	TECS _tecs;
+	TECS			_tecs;
 
 	uint8_t _position_sp_type{0};
-
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
 		FW_POSCTRL_MODE_AUTO_ALTITUDE,
@@ -343,11 +291,10 @@ 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 if the mode has changed
+	} _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.
 
 	param_t _param_handle_airspeed_trans{PARAM_INVALID};
-
-	float _param_airspeed_trans{NAN}; // [m/s]
+	float _param_airspeed_trans{NAN};
 
 	enum StickConfig {
 		STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
@@ -355,61 +302,54 @@ 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);
 
 	/**
-	 * @brief Get a new waypoint based on heading and distance from current position
+	 * 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);
 
 	/**
-	 * @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]
+	 * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
 	 */
-	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();
 
 	/**
-	 * @brief Check if we are in a takeoff situation
+	 * Check if we are in a takeoff situation
 	 */
-	bool in_takeoff_situation();
+	bool 		in_takeoff_situation();
 
 	/**
-	 * @brief Update desired altitude base on user pitch stick input
+	 * 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.
@@ -418,203 +358,80 @@ 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);
 
-	/**
-	 * @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);
+	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);
 
-	/* automatic control methods */
+	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);
 
 	/**
-	 * @brief Automatic position control for waypoints, orbits, and velocity control
-	 *
-	 * @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_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);
-
-	/**
-	 * @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);
-
-	/**
-	 * @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);
-
-	/**
-	 * @brief Controls automatic takeoff.
+	 * @brief Vehicle control while in 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);
+	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);
 
-	/**
-	 * @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);
+	float		get_tecs_pitch();
+	float		get_tecs_thrust();
 
-	/* manual control methods */
+	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 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		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);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
-	SlewRate<float> _airspeed_slew_rate_controller;
+	SlewRate<float> _slew_rate_airspeed;
 
-	/**
-	 * @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]
+	/*
+	 * Call TECS : a wrapper function to call the TECS implementation
 	 */
-	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
+	void tecs_update_pitch_throttle(const hrt_abstime &now, 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,

src/modules/logger/logged_topics.cpp

@@ -81,6 +81,12 @@ 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");

src/modules/manual_control/manual_control_params.c

@@ -38,6 +38,7 @@
  * arms and to the lower left disarms the vehicle.
  *
  * @boolean
+ * @reboot_required true
  * @group Manual Control
  */
 PARAM_DEFINE_INT32(MAN_ARM_GESTURE, 1);

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -184,15 +184,16 @@ 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(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
-				_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
+				_thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
 
 				// publish rate setpoint
-				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.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_pub.publish(vehicle_rates_setpoint);
 			}
@@ -202,7 +203,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 = Vector3f(vehicle_rates_setpoint.thrust_body);
+				_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
 			}
 		}
 
@@ -250,14 +251,13 @@ 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(2)) ? -_thrust_setpoint(
-						2) : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint) ? _thrust_setpoint : 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(angular_velocity.timestamp_sample);
+				publishThrustSetpoint(_thrust_setpoint, angular_velocity.timestamp_sample);
 			}
 
 			// scale effort by battery status if enabled
@@ -307,12 +307,14 @@ void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, co
 	_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
 }
 
-void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
+void MulticopterRateControl::publishThrustSetpoint(const float thrust_setpoint, 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;
-	_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
+	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
 
 	_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
 }

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -94,7 +94,8 @@ 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 hrt_abstime &timestamp_sample);
+
+	void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample);
 
 	RateControl _rate_control; ///< class for rate control calculations
 
@@ -137,7 +138,7 @@ private:
 	matrix::Vector3f _rates_setpoint{};
 
 	float _battery_status_scale{0.0f};
-	matrix::Vector3f _thrust_setpoint{};
+	float _thrust_setpoint{0.0f};
 
 	float _energy_integration_time{0.0f};
 	float _control_energy[4] {};

src/modules/navigator/mission_block.cpp

@@ -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 = math::signNoZero(item.loiter_radius);
+	sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
 
 	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

src/modules/navigator/navigator_main.cpp

@@ -234,10 +234,7 @@ 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
-				   && _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)
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
 
 				bool reposition_valid = true;
 

src/modules/px4iofirmware/CMakeLists.txt

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

src/modules/px4iofirmware/mixer.cpp

@@ -134,11 +134,13 @@ mixer_tick()
 	 * FMU or from the mixer.
 	 *
 	 */
-	should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)		/* IO initialised without error */
+	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 */
 		     && (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 */
+	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 */
 				 && ((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 */
 				    ));

src/modules/px4iofirmware/protocol.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2017 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,7 +114,6 @@
 #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 */
@@ -185,8 +184,12 @@ 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_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_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_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 */
@@ -209,7 +212,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 */
+/* PWM disarmed values that are active, even when SAFETY_SAFE */
 #define PX4IO_PAGE_DISARMED_PWM			109		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**

src/modules/px4iofirmware/px4io.cpp

@@ -154,7 +154,8 @@ show_debug_messages(void)
 static void
 update_mem_usage(void)
 {
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+	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)) {
 		return;
 	}
 
@@ -182,7 +183,8 @@ ring_blink(void)
 {
 #if defined(LED_GREEN)
 
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+	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)) {
 		LED_GREEN(true);
 		return;
 	}
@@ -312,8 +314,8 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
 	ENABLE_SBUS_OUT(false);
 #endif
 
-	/* start the safety button handler */
-	safety_button_init();
+	/* start the safety switch handler */
+	safety_init();
 
 	/* initialise the control inputs */
 	controls_init();

src/modules/px4iofirmware/px4io.h

@@ -131,6 +131,8 @@ 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);
@@ -143,9 +145,9 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
 extern void	mixer_tick(void);
 
 /**
- * Safety button/LED.
+ * Safety switch/LED.
  */
-extern void	safety_button_init(void);
+extern void	safety_init(void);
 extern void	failsafe_led_init(void);
 
 /**

src/modules/px4iofirmware/registers.c

@@ -413,6 +413,10 @@ 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) {
@@ -429,19 +433,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
-		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;
+		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;
+			}
 
 			break;
 
-		case PX4IO_P_SETUP_SAFETY_OFF:
-
-			if (value) {
+		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
+			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 
 			} else {
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+				return -1;
 			}
 
 			break;

src/modules/px4iofirmware/safety.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file safety_button.cpp
+ * @file safety.cpp
  * Safety button logic.
  *
  * @author Lorenz Meier <lorenz@px4.io>
@@ -46,7 +46,7 @@
 
 #include "px4io.h"
 
-static struct hrt_call safety_button_call;
+static struct hrt_call arming_call;
 static struct hrt_call failsafe_call;
 
 /*
@@ -59,23 +59,30 @@ 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;
 
-#define SAFETY_SWITCH_THRESHOLD	10
+/*
+ * 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
 
-static void safety_button_check(void *arg);
+static void safety_check_button(void *arg);
 static void failsafe_blink(void *arg);
 
 void
-safety_button_init(void)
+safety_init(void)
 {
 	/* arrange for the button handler to be called at 10Hz */
-	hrt_call_every(&safety_button_call, 1000, 100000, safety_button_check, NULL);
+	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
 }
 
 void
@@ -86,20 +93,26 @@ failsafe_led_init(void)
 }
 
 static void
-safety_button_check(void *arg)
+safety_check_button(void *arg)
 {
-	const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
+	const bool safety_button_pressed = BUTTON_SAFETY;
 
-	/* Keep safety button pressed for one second to trigger safety button event.
-	 * The logic to prevent turning on safety again is in the commander.
+	/* 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.
 	 */
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
+	    (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT)) {
+		if (counter <= ARM_COUNTER_THRESHOLD) {
+			counter++;
 
-		counter++;
+		}
 
-		if (counter >= SAFETY_SWITCH_THRESHOLD) {
-			atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT);
+		if (counter == ARM_COUNTER_THRESHOLD) {
+			// switch safety off -> ready to arm state
+			atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
 		}
 
 	} else {

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -377,23 +377,6 @@ 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) {

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -146,7 +146,8 @@ bool VehicleIMU::ParametersUpdate(bool force)
 		}
 
 		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
-		const int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), (int32_t)50, (int32_t)1000);
+		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));
 
 		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);

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -191,6 +191,7 @@ 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
 	)
 };

src/modules/sih/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2020 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,4 +45,5 @@ px4_add_module(
 		drivers_accelerometer
 		drivers_gyroscope
 		drivers_magnetometer
+		px4_work_queue
 	)

src/modules/sih/aero.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2019-2020 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,6 +139,12 @@ 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)
@@ -156,7 +162,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, const matrix::Vector3f &p_B, float dihedral_deg = 0.0f,
+	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)
 	{
@@ -170,7 +176,7 @@ public:
 		_span = span;
 		_mac = mac;
 		_alpha_0 = math::radians(alpha_0_deg);
-		_p_B = p_B;
+		_p_B = matrix::Vector3f(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;
@@ -202,10 +208,6 @@ 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)

src/modules/sih/sih.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 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,16 +54,8 @@ using namespace matrix;
 using namespace time_literals;
 
 Sih::Sih() :
-	ModuleParams(nullptr)
-{}
-
-Sih::~Sih()
-{
-	perf_free(_loop_perf);
-	perf_free(_loop_interval_perf);
-}
-
-void Sih::run()
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	_px4_accel.set_temperature(T1_C);
 	_px4_gyro.set_temperature(T1_C);
@@ -85,86 +77,15 @@ void Sih::run()
 	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();
 }
 
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-// Get current timestamp in microseconds
-uint64_t micros()
+Sih::~Sih()
 {
-	struct timeval t;
-	gettimeofday(&t, nullptr);
-	return t.tv_sec * ((uint64_t)1000000) + t.tv_usec;
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
 }
 
-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()
+bool Sih::init()
 {
 	int rate = _imu_gyro_ratemax.get();
 
@@ -175,29 +96,20 @@ void Sih::realtime_loop()
 
 	// 200 - 2000 Hz
 	int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
+	ScheduleOnInterval(interval_us);
 
-	px4_sem_init(&_data_semaphore, 0, 0);
-	hrt_call_every(&_timer_call, interval_us, interval_us, timer_callback, &_data_semaphore);
+	return true;
+}
 
-	while (!should_exit()) {
-		px4_sem_wait(&_data_semaphore);     // periodic real time wakeup
-		perf_begin(_loop_perf);
-		sensor_step();
-		perf_end(_loop_perf);
+void Sih::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
 	}
 
-	hrt_cancel(&_timer_call);
-	px4_sem_destroy(&_data_semaphore);
-}
+	perf_count(_loop_interval_perf);
 
-
-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
@@ -258,7 +170,7 @@ void Sih::sensor_step()
 		send_gps();
 	}
 
-	if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && _now - _airspeed_time >= 50_ms) {
+	if (_vehicle == VehicleType::FW && _now - _airspeed_time >= 50_ms) {
 		_airspeed_time = _now;
 		send_airspeed();
 	}
@@ -372,7 +284,6 @@ 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
@@ -687,8 +598,8 @@ float Sih::generate_wgn()   // generate white Gaussian noise sample with std=1
 
 	if (phase) {
 		do {
-			float U1 = (float)rand() / (float)RAND_MAX;
-			float U2 = (float)rand() / (float)RAND_MAX;
+			float U1 = (float)rand() / RAND_MAX;
+			float U2 = (float)rand() / RAND_MAX;
 			V1 = 2.0f * U1 - 1.0f;
 			V2 = 2.0f * U2 - 1.0f;
 			S = V1 * V1 + V2 * V2;
@@ -712,11 +623,6 @@ 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");
 
@@ -752,33 +658,27 @@ 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 == nullptr) {
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
 		PX4_ERR("alloc failed");
 	}
 
-	return instance;
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
 }
 
 int Sih::custom_command(int argc, char *argv[])

src/modules/sih/sih.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2019-2020 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,6 +58,7 @@
 #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,
@@ -66,7 +67,7 @@
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <perf/perf_counter.h>
+#include <lib/perf/perf_counter.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
@@ -80,27 +81,17 @@
 #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;
 
-extern "C" __EXPORT int sih_main(int argc, char *argv[]);
-
-class Sih : public ModuleBase<Sih>, public ModuleParams
+class Sih : public ModuleBase<Sih>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	Sih();
-
-	virtual ~Sih();
+	~Sih() override;
 
 	/** @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[]);
 
@@ -110,18 +101,16 @@ 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);
 
-	// timer called periodically to post the semaphore
-	static void timer_callback(void *sem);
+	bool init();
 
 private:
+	void Run() override;
+
 	void parameters_updated();
 
 	// simulated sensor instances
@@ -182,23 +171,11 @@ 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};
@@ -298,7 +275,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,

src/modules/sih/sih_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 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

src/modules/uorb_explained/CMakeLists.txt

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

src/modules/uorb_explained/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_UORB_EXPLAINED
+	bool "uorb_explained module"
+	default y
+	---help---
+		uORB Explained Module

src/modules/uorb_explained/uorb_explained.cpp

@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   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;
+}

src/modules/uorb_explained/uorb_explained.hpp

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   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)};
+};

src/systemcmds/failure/failure.cpp

@@ -118,9 +118,9 @@ failure gps off
 	}
 }
 
-int inject_failure(const FailureUnit& unit, const FailureType& type, uint8_t instance)
+int inject_failure(uint8_t unit, uint8_t type, uint8_t instance)
 {
-	PX4_WARN("inject failure unit: %s (%d), type: %s (%d), instance: %d", unit.key, unit.value, type.key, type.value, instance);
+	PX4_WARN("inject failure unit: %s (%d), type: %s (%d), instance: %d", failure_units[unit].key, unit, failure_types[type].key, type, instance);
 
 	uORB::Subscription command_ack_sub{ORB_ID(vehicle_command_ack)};
 
@@ -128,8 +128,8 @@ int inject_failure(const FailureUnit& unit, const FailureType& type, uint8_t ins
 	vehicle_command_s command{};
 
 	command.command = vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE;
-	command.param1 = static_cast<float>(unit.value);
-	command.param2 = static_cast<float>(type.value);
+	command.param1 = static_cast<float>(unit);
+	command.param2 = static_cast<float>(type);
 	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, failure_type, instance);
+			return inject_failure(failure_unit.value, failure_type.value, instance);
 		}
 
 		PX4_ERR("Failure type '%s' not found", requested_failure_type);

test/mavsdk_tests/autopilot_tester.h

@@ -139,11 +139,6 @@ 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();}

test/mavsdk_tests/test_multicopter_failure_injection.cpp

@@ -41,7 +41,6 @@ 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();
@@ -52,3 +51,11 @@ 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);
+}