ekf2: push preflight filtered innovation checks into backend

- move flags to EstimatorStatusFlags and update commander HealthAndArmingChecks usage

.ci/Jenkinsfile-hardware

@@ -773,7 +773,7 @@ void runTests() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params" || true' // expected to fail after erase
 
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"'

.vscode/settings.json

@@ -119,7 +119,9 @@
                 "utility": "cpp",
                 "valarray": "cpp",
                 "variant": "cpp",
-                "vector": "cpp"
+                "vector": "cpp",
+                "Jenkinsfile*": "groovy",
+                "*.sdf": "xml"
         },
         "search.exclude": {
                 "${workspaceFolder}/build": true

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -227,7 +227,7 @@ then
 elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ]
 then
 	echo "Error: no autostart file found ($autostart_file)"
-	#exit 1
+	exit 1
 fi
 
 #user defined params for instances can be in PATH

Tools/msg/px_generate_uorb_topic_files.py

@@ -122,13 +122,21 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
     for field in spec.parsed_fields():
         field_name_and_type.update({field.name: field.type})
 
+    # assert if the timestamp field exists
+    try:
+        assert 'timestamp' in field_name_and_type
+    except AssertionError:
+        print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\tNo 'timestamp' field found in " +
+              spec.short_name + " msg definition!")
+        exit(1)
+
     # assert if the timestamp field is of type uint64
-    # try:
-    #     assert field_name_and_type.get('timestamp') == 'uint64'
-    # except AssertionError:
-    #     print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
-    #           " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
-    #     exit(1)
+    try:
+        assert field_name_and_type.get('timestamp') == 'uint64'
+    except AssertionError:
+        print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
+              " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
+        exit(1)
 
     topics = get_topics(filename)
 

boards/modalai/voxl2-slpi/default.px4board

@@ -5,3 +5,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
+CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y

boards/modalai/voxl2-slpi/src/board_config.h

@@ -57,3 +57,8 @@
  */
 #include <system_config.h>
 #include <px4_platform_common/board_common.h>
+
+/*
+ *  Default port for the ESC
+ */
+#define MODALAI_ESC_DEFAULT_PORT 	"2"

boards/modalai/voxl2/target/voxl-px4.config

@@ -19,3 +19,5 @@ fi
 muorb start
 
 qshell icm42688p start -s
+
+qshell modalai_esc start

boards/nxp/mr-canhubk3/nuttx-config/include/board.h

@@ -141,7 +141,7 @@
 /* LPUART0   P2 UART (with flow control) connector */
 
 #define PIN_LPUART0_CTS   PIN_LPUART0_CTS_1  /* PTA0 */
-#define PIN_LPUART0_RTS   PIN_LPUART0_RTS_1  /* PTA1 */
+#define PIN_LPUART0_RTS   (PIN_PTA1 | GPIO_OUTPUT) /* PIN_LPUART0_RTS_1 PTA1 */
 #define PIN_LPUART0_RX    (PIN_LPUART0_RX_1 | PIN_INPUT_PULLUP)   /* PTA2 */
 #define PIN_LPUART0_TX    PIN_LPUART0_TX_1   /* PTA3 */
 

boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig

@@ -118,7 +118,20 @@ CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
 CONFIG_LPI2C0_DMA=y
 CONFIG_LPI2C1_DMA=y
+CONFIG_LPUART0_IFLOWCONTROL=y
+CONFIG_LPUART0_OFLOWCONTROL=y
+CONFIG_LPUART0_RXBUFSIZE=640
+CONFIG_LPUART0_RXDMA=y
+CONFIG_LPUART0_TXDMA=y
+CONFIG_LPUART13_RXDMA=y
+CONFIG_LPUART13_TXDMA=y
+CONFIG_LPUART14_RXDMA=y
+CONFIG_LPUART14_TXDMA=y
+CONFIG_LPUART1_RXDMA=y
+CONFIG_LPUART1_TXDMA=y
+CONFIG_LPUART2_RXDMA=y
 CONFIG_LPUART2_SERIAL_CONSOLE=y
+CONFIG_LPUART2_TXDMA=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -241,6 +254,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1768
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SIG_SIGALRM_ACTION=y

boards/nxp/ucans32k146/nuttx-config/Kconfig

@@ -6,12 +6,14 @@ config BOARD_HAS_PROBES
 	bool "Board provides GPIO or other Hardware for signaling to timing analyze."
 	default y
 	---help---
-		This board provides GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers.
+		This board provides GPIO 6-wr-SPI_INT_N, 6-wr-SPI_CS_N, 6-wr-SPI_RDY_N as
+		PROBE_1-3 to provide timing signals from selected drivers.
 
 config BOARD_USE_PROBES
-	bool "Enable the use the board provided GPIO FMU-CH1-6 as PROBE_1-6 to provide timing signals from selected drivers"
+	bool "Enable the use of the board provided GPIO Probes"
 	default n
 	depends on BOARD_HAS_PROBES
 
 	---help---
-		Select to use GPIO FMU-CH1-6 to provide timing signals from selected drivers.
+		Select to use GPIO GPIO 6-wr-SPI_INT_N, 6-wr-SPI_CS_N, 6-wr-SPI_RDY_N as PROBE_1-3
+		to provide timing signals from selected drivers.

boards/nxp/ucans32k146/nuttx-config/include/board.h

@@ -136,7 +136,7 @@
  */
 
 #define PIN_LPUART0_CTS   PIN_LPUART0_CTS_2 /* PTC8 */
-#define PIN_LPUART0_RTS   PIN_LPUART0_RTS_2 /* PTC9 */
+#define PIN_LPUART0_RTS   (GPIO_OUTPUT | PIN_PORTC | PIN9 ) // PIN_LPUART0_RTS_2 /* PTC9 */
 #define PIN_LPUART0_RX    PIN_LPUART0_RX_1  /* PTB0 */
 #define PIN_LPUART0_TX    PIN_LPUART0_TX_1  /* PTB1 */
 
@@ -167,4 +167,29 @@
 #define PIN_CAN1_ERRN    (GPIO_PULLDOWN  | PIN_PORTE | PIN6 )
 #define PIN_CAN1_EN      (GPIO_OUTPUT  | PIN_PORTE | PIN2 )
 
+/* Board provides GPIO or other Hardware for signaling to timing analyzer */
+
+#if defined(CONFIG_BOARD_USE_PROBES)
+# include "s32k1xx_pin.h"
+# include "hardware/s32k1xx_pinmux.h"
+# define PROBE_N(n) (1<<((n)-1))
+# define PROBE_1    (PIN_PTE0  | GPIO_OUTPUT)  /* 6-wr-SPI_RDY_N */
+# define PROBE_2    (PIN_PTE9  | GPIO_OUTPUT)  /* 6-wr-SPI_INT_N */
+# define PROBE_3    (PIN_PTB5  | GPIO_OUTPUT)  /* 6-wr-SPI_CS_N */
+
+# define PROBE_INIT(mask) \
+	do { \
+		if ((mask)& PROBE_N(1)) { s32k1xx_pinconfig(PROBE_1); } \
+		if ((mask)& PROBE_N(2)) { s32k1xx_pinconfig(PROBE_2); } \
+		if ((mask)& PROBE_N(3)) { s32k1xx_pinconfig(PROBE_3); } \
+	} while(0)
+
+# define PROBE(n,s)  do {s32k1xx_gpiowrite(PROBE_##n,(s));}while(0)
+# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
+#else
+# define PROBE_INIT(mask)
+# define PROBE(n,s)
+# define PROBE_MARK(n)
+#endif
+
 #endif  /* __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H */

boards/nxp/ucans32k146/nuttx-config/nsh/defconfig

@@ -46,18 +46,22 @@ CONFIG_I2CTOOL_MAXADDR=0x7f
 CONFIG_I2CTOOL_MAXBUS=0
 CONFIG_I2CTOOL_MINADDR=0x00
 CONFIG_INIT_ENTRYPOINT="nsh_main"
-CONFIG_INIT_STACKSIZE=2276
+CONFIG_INIT_STACKSIZE=3500
 CONFIG_INTELHEX_BINARY=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LPI2C0_DMA=y
 CONFIG_LPUART0_BAUD=38400
+CONFIG_LPUART0_IFLOWCONTROL=y
+CONFIG_LPUART0_OFLOWCONTROL=y
 CONFIG_LPUART0_RXBUFSIZE=600
 CONFIG_LPUART0_RXDMA=y
 CONFIG_LPUART0_TXBUFSIZE=1100
 CONFIG_LPUART0_TXDMA=y
 CONFIG_LPUART1_RXBUFSIZE=128
+CONFIG_LPUART1_RXDMA=y
 CONFIG_LPUART1_SERIAL_CONSOLE=y
 CONFIG_LPUART1_TXBUFSIZE=128
+CONFIG_LPUART1_TXDMA=y
 CONFIG_MOTOROLA_SREC=y
 CONFIG_MTD=y
 CONFIG_MTD_PARTITION=y
@@ -96,12 +100,20 @@ CONFIG_S32K1XX_EDMA_NTCD=64
 CONFIG_S32K1XX_EEEPROM=y
 CONFIG_S32K1XX_FLEXCAN0=y
 CONFIG_S32K1XX_FLEXCAN1=y
+CONFIG_S32K1XX_GPIOIRQ=y
 CONFIG_S32K1XX_LPI2C0=y
 CONFIG_S32K1XX_LPI2C_DMA=y
 CONFIG_S32K1XX_LPI2C_DYNTIMEO=y
 CONFIG_S32K1XX_LPSPI0=y
+CONFIG_S32K1XX_LPSPI0_DMA=y
+CONFIG_S32K1XX_LPSPI_DMA=y
 CONFIG_S32K1XX_LPUART0=y
 CONFIG_S32K1XX_LPUART1=y
+CONFIG_S32K1XX_PORTAINTS=y
+CONFIG_S32K1XX_PORTBINTS=y
+CONFIG_S32K1XX_PORTCINTS=y
+CONFIG_S32K1XX_PORTDINTS=y
+CONFIG_S32K1XX_PORTEINTS=y
 CONFIG_S32K1XX_RTC=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_HPWORKPRIORITY=249
@@ -113,6 +125,7 @@ CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SIG_DEFAULT=y
 CONFIG_SPITOOL_MAXBUS=2

boards/nxp/ucans32k146/src/spi.cpp

@@ -63,7 +63,7 @@
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBusExternal(SPI::Bus::SPI0, {
 		// Going to assume PTB5 means PortB, Pin5
-		initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin5})
+		initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortE, GPIO::Pin0}),
 	}),
 };
 

boards/px4/fmu-v5/default.px4board

@@ -59,7 +59,7 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

boards/px4/fmu-v5x/default.px4board

@@ -59,7 +59,7 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

msg/CMakeLists.txt

@@ -74,6 +74,7 @@ set(msg_files
 	EstimatorAidSource2d.msg
 	EstimatorAidSource3d.msg
 	EstimatorBias.msg
+	EstimatorBias3d.msg
 	EstimatorEventFlags.msg
 	EstimatorGpsStatus.msg
 	EstimatorInnovations.msg
@@ -182,8 +183,8 @@ set(msg_files
 	TrajectoryWaypoint.msg
 	TransponderReport.msg
 	TuneControl.msg
-	ParameterRequest.msg
-	ParameterValue.msg
+	UavcanParameterRequest.msg
+	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
 	UwbDistance.msg
@@ -219,14 +220,6 @@ set(msg_files
 	WheelEncoders.msg
 	Wind.msg
 	YawEstimatorStatus.msg
-
-        Parameter.msg
-
-        SrvParameterGetRequest.msg
-        SrvParameterGetResponse.msg
-
-        SrvParameterSetRequest.msg
-        SrvParameterSetResponse.msg
 )
 list(SORT msg_files)
 

msg/EstimatorBias.msg

@@ -9,4 +9,4 @@ float32 innov			# innovation of the last measurement fusion (m)
 float32 innov_var		# innovation variance of the last measurement fusion (m^2)
 float32 innov_test_ratio	# normalized innovation squared test ratio
 
-# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias

msg/EstimatorBias3d.msg

@@ -0,0 +1,14 @@
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
+float32[3] bias                 # estimated barometric altitude bias (m)
+float32[3] bias_var             # estimated barometric altitude bias variance (m^2)
+
+float32[3] innov                # innovation of the last measurement fusion (m)
+float32[3] innov_var            # innovation variance of the last measurement fusion (m^2)
+float32[3] innov_test_ratio     # normalized innovation squared test ratio
+
+# TOPICS estimator_bias3d
+# TOPICS estimator_ev_pos_bias

msg/EstimatorStatus.msg

@@ -113,12 +113,6 @@ uint8 reset_count_quat   # number of quaternion reset events (allow to wrap if c
 
 float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time
 
-bool pre_flt_fail_innov_heading
-bool pre_flt_fail_innov_vel_horiz
-bool pre_flt_fail_innov_vel_vert
-bool pre_flt_fail_innov_height
-bool pre_flt_fail_mag_field_disturbed
-
 uint32 accel_device_id
 uint32 gyro_device_id
 uint32 baro_device_id

msg/EstimatorStatusFlags.msg

@@ -73,3 +73,10 @@ bool reject_sideslip                      #  9 - true if the synthetic sideslip
 bool reject_hagl                          # 10 - true if the height above ground observation has been rejected
 bool reject_optflow_x                     # 11 - true if the X optical flow observation has been rejected
 bool reject_optflow_y                     # 12 - true if the Y optical flow observation has been rejected
+
+# preflight failure checks
+bool pre_flt_fail_innov_heading
+bool pre_flt_fail_innov_vel_horiz
+bool pre_flt_fail_innov_vel_vert
+bool pre_flt_fail_innov_pos_horiz
+bool pre_flt_fail_innov_height

msg/Parameter.msg

@@ -1,12 +0,0 @@
-char[17] name                   # parameter name
-int16 index		# -1 if the param_id field should be used as identifier
-int16 index_used		# -1 if the param_id field should be used as identifier
-
-uint8 TYPE_INVALID = 0
-uint8 TYPE_BOOL    = 1
-uint8 TYPE_INT32   = 2
-uint8 TYPE_FLOAT32 = 3
-uint8 type                # parameter type
-
-int32 int32_value               # current value if param_type is int-like
-float32 float32_value           # current value if param_type is float-like

msg/ParameterRequest.msg

@@ -1,27 +0,0 @@
-# parameter request type
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 MESSAGE_TYPE_INVALID            = 0
-uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 1
-uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 2
-uint8 MESSAGE_TYPE_PARAM_SET          = 3
-uint8 message_type                        # message type
-
-uint8 NODE_ID_ALL = 0		# MAV_COMP_ID_ALL
-uint8 node_id			# node ID
-
-char[17] name                   # parameter name
-int16 param_index		# -1 if the param_id field should be used as identifier
-
-uint8 TYPE_INVALID = 0
-uint8 TYPE_BOOL    = 1
-uint8 TYPE_INT32   = 2
-uint8 TYPE_FLOAT32 = 3
-uint8 type                # parameter type
-
-int32 int32_value               # current value if param_type is int-like
-float32 float32_value           # current value if param_type is float-like
-
-uint8 ORB_QUEUE_LENGTH = 8
-
-# TOPICS parameter_request uavcan_parameter_request

msg/ParameterUpdate.msg

@@ -4,6 +4,11 @@ uint64 timestamp		# time since system start (microseconds)
 
 uint32 instance		# Instance count - constantly incrementing
 
-uint16 count
+uint32 get_count
+uint32 set_count
+uint32 find_count
+uint32 export_count
 
-Parameter changed_param
+uint16 active
+uint16 changed
+uint16 custom_default

msg/ParameterValue.msg

@@ -1,22 +0,0 @@
-# parameter response type
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 node_id			# requester node ID
-
-char[17] param_name		# parameter name
-
-int16 param_index		# parameter index, if known
-
-uint16 param_count		# number of parameters exposed by the node
-
-uint8 type                      # parameter type
-
-int32 int32_value               # current value if param_type is int-like
-float32 float32_value           # current value if param_type is float-like
-
-
-uint64 timestamp_requested
-
-uint8 ORB_QUEUE_LENGTH = 8
-
-# TOPICS parameter_value uavcan_parameter_value

msg/SrvParameterGetRequest.msg

@@ -1,6 +0,0 @@
-uint64 timestamp              # time since system start (microseconds)
-
-char[17] name                 # parameter name
-int16 index                   # -1 if the param_id field should be used as identifier
-
-uint8 ORB_QUEUE_LENGTH = 16

msg/SrvParameterGetResponse.msg

@@ -1,11 +0,0 @@
-uint64 timestamp              # time since system start (microseconds)
-uint64 timestamp_requested
-
-Parameter parameter
-
-uint8 RESULT_GET_SUCCESS             = 0
-uint8 RESULT_GET_FAILED              = 1
-uint8 RESULT_ERROR_INVALID_PARAMETER = 2
-uint8 result
-
-uint8 ORB_QUEUE_LENGTH = 16

msg/SrvParameterSetRequest.msg

@@ -1,5 +0,0 @@
-uint64 timestamp              # time since system start (microseconds)
-
-Parameter parameter
-
-uint8 ORB_QUEUE_LENGTH = 8

msg/SrvParameterSetResponse.msg

@@ -1,11 +0,0 @@
-uint64 timestamp              # time since system start (microseconds)
-uint64 timestamp_requested
-
-Parameter parameter
-
-uint8 RESULT_SET_SUCCESS             = 0
-uint8 RESULT_SET_FAILED              = 1
-uint8 RESULT_ERROR_INVALID_PARAMETER = 2
-uint8 result
-
-uint8 ORB_QUEUE_LENGTH = 8

msg/TecsStatus.msg

@@ -18,21 +18,11 @@ float32 true_airspeed_filtered
 float32 true_airspeed_derivative_sp
 float32 true_airspeed_derivative
 float32 true_airspeed_derivative_raw
-float32 true_airspeed_innovation
 
-float32 total_energy_error
-float32 energy_distribution_error
-float32 total_energy_rate_error
-float32 energy_distribution_rate_error
-
-float32 total_energy
 float32 total_energy_rate
-float32 total_energy_balance
 float32 total_energy_balance_rate
 
-float32 total_energy_sp
 float32 total_energy_rate_sp
-float32 total_energy_balance_sp
 float32 total_energy_balance_rate_sp
 
 float32 throttle_integ

msg/UavcanParameterRequest.msg

@@ -0,0 +1,23 @@
+# UAVCAN-MAVLink parameter bridge request type
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20	# MAVLINK_MSG_ID_PARAM_REQUEST_READ
+uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21	# MAVLINK_MSG_ID_PARAM_REQUEST_LIST
+uint8 MESSAGE_TYPE_PARAM_SET = 23		# MAVLINK_MSG_ID_PARAM_SET
+uint8 message_type				# MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET
+
+uint8 NODE_ID_ALL = 0		# MAV_COMP_ID_ALL
+uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
+
+char[17] param_id		# MAVLink/UAVCAN parameter name
+int16 param_index		# -1 if the param_id field should be used as identifier
+
+uint8 PARAM_TYPE_UINT8 = 1	# MAV_PARAM_TYPE_UINT8
+uint8 PARAM_TYPE_INT64 = 8	# MAV_PARAM_TYPE_INT64
+uint8 PARAM_TYPE_REAL32 = 9	# MAV_PARAM_TYPE_REAL32
+uint8 param_type		# MAVLink parameter type
+
+int64 int_value			# current value if param_type is int-like
+float32 real_value		# current value if param_type is float-like
+
+uint8 ORB_QUEUE_LENGTH = 4

msg/UavcanParameterValue.msg

@@ -0,0 +1,9 @@
+# UAVCAN-MAVLink parameter bridge response type
+uint64 timestamp		# time since system start (microseconds)
+uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
+char[17] param_id		# MAVLink/UAVCAN parameter name
+int16 param_index		# parameter index, if known
+uint16 param_count		# number of parameters exposed by the node
+uint8 param_type		# MAVLink parameter type
+int64 int_value			# current value if param_type is int-like
+float32 real_value		# current value if param_type is float-like

msg/VehicleOdometry.msg

@@ -28,4 +28,4 @@ uint8 reset_counter
 int8 quality
 
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
-# TOPICS estimator_odometry estimator_visual_odometry_aligned
+# TOPICS estimator_odometry

platforms/common/include/px4_platform_common/param.h

@@ -118,6 +118,7 @@ public:
 
 	Param()
 	{
+		param_set_used(handle());
 		update();
 	}
 
@@ -145,6 +146,12 @@ public:
 
 	void set(float val) { _val = val; }
 
+	void reset()
+	{
+		param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update() { return param_get(handle(), &_val) == 0; }
 
 	param_t handle() const { return param_handle(p); }
@@ -163,6 +170,7 @@ public:
 	Param(float &external_val)
 		: _val(external_val)
 	{
+		param_set_used(handle());
 		update();
 	}
 
@@ -190,6 +198,12 @@ public:
 
 	void set(float val) { _val = val; }
 
+	void reset()
+	{
+		param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update() { return param_get(handle(), &_val) == 0; }
 
 	param_t handle() const { return param_handle(p); }
@@ -206,6 +220,7 @@ public:
 
 	Param()
 	{
+		param_set_used(handle());
 		update();
 	}
 
@@ -233,6 +248,12 @@ public:
 
 	void set(int32_t val) { _val = val; }
 
+	void reset()
+	{
+		param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update() { return param_get(handle(), &_val) == 0; }
 
 	param_t handle() const { return param_handle(p); }
@@ -251,6 +272,7 @@ public:
 	Param(int32_t &external_val)
 		: _val(external_val)
 	{
+		param_set_used(handle());
 		update();
 	}
 
@@ -278,6 +300,12 @@ public:
 
 	void set(int32_t val) { _val = val; }
 
+	void reset()
+	{
+		param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update() { return param_get(handle(), &_val) == 0; }
 
 	param_t handle() const { return param_handle(p); }
@@ -294,6 +322,7 @@ public:
 
 	Param()
 	{
+		param_set_used(handle());
 		update();
 	}
 
@@ -329,6 +358,12 @@ public:
 
 	void set(bool val) { _val = val; }
 
+	void reset()
+	{
+		param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update()
 	{
 		int32_t value_int;

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2021 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
@@ -76,8 +76,6 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};
 
 static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};
 
-static constexpr wq_config_t parameter_server{"wq:parameter_server", 4096, -20};
-
 static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21};
 static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22};
 static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23};

platforms/common/uORB/SubscriptionBlocking.hpp

@@ -85,7 +85,6 @@ public:
 
 	virtual ~SubscriptionBlocking()
 	{
-		unregisterCallback();
 		pthread_mutex_destroy(&_mutex);
 		pthread_cond_destroy(&_cv);
 	}

platforms/common/uORB/uORBDeviceNode.cpp

@@ -514,7 +514,7 @@ uORB::DeviceNode::register_callback(uORB::SubscriptionCallback *callback_sub)
 		ATOMIC_ENTER;
 
 		// prevent duplicate registrations
-		for (const auto &existing_callbacks : _callbacks) {
+		for (auto existing_callbacks : _callbacks) {
 			if (callback_sub == existing_callbacks) {
 				ATOMIC_LEAVE;
 				return true;

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit ab650c981e2066808f312c1e4599ccbde522f255
+Subproject commit 3459fed6a75ddcf3ea7835a0f563fd625f8c47ae

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

@@ -127,6 +127,8 @@ int px4_platform_init()
 	hrt_ioctl_init();
 #endif
 
+	param_init();
+
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 	cpuload_initialize_once();
@@ -183,8 +185,6 @@ int px4_platform_init()
 
 	uorb_start();
 
-	param_init();
-
 	px4_log_initialize();
 
 #if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_FLAT)

platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h

@@ -103,7 +103,9 @@ __BEGIN_DECLS
 #define px4_arch_gpioread(pinset)               s32k1xx_gpioread(pinset)
 #define px4_arch_gpiowrite(pinset, value)       s32k1xx_gpiowrite(pinset, value)
 
-/* s32k1xx_gpiosetevent is not implemented and will need to be added */
+/* s32k1xx_gpiosetevent is added at PX4 level */
+
+int s32k1xx_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg);
 
 #define px4_arch_gpiosetevent(pinset,r,f,e,fp,a)  s32k1xx_gpiosetevent(pinset,r,f,e,fp,a)
 

platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/CMakeLists.txt

@@ -36,4 +36,6 @@ px4_add_library(arch_io_pins
 	pwm_servo.c
 	pwm_trigger.c
 	input_capture.c
+	input_capture.c
+	s32k1xx_pinirq.c
 )

platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/s32k1xx_pinirq.c

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <systemlib/px4_macros.h>
+
+#include <arch/board/board.h>
+
+#include <errno.h>
+
+#include <s32k1xx_pin.h>
+
+/****************************************************************************
+ * Name: s32k1xx_gpiosetevent
+ *
+ * Description:
+ *   Sets/clears GPIO based event and interrupt triggers.
+ *
+ * Input Parameters:
+ *  - pinset: gpio pin configuration
+ *  - rising/falling edge: enables
+ *  - event:  generate event when set
+ *  - func:   when non-NULL, generate interrupt
+ *  - arg:    Argument passed to the interrupt callback
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure indicating the
+ *   nature of the failure.
+ *
+ ****************************************************************************/
+#if defined(CONFIG_S32K1XX_GPIOIRQ)
+int s32k1xx_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge,
+			 bool event, xcpt_t func, void *arg)
+{
+	int ret = -ENOSYS;
+
+	s32k1xx_pinconfig(pinset);
+
+	if (func == NULL) {
+		s32k1xx_pinirqdisable(pinset);
+		ret = s32k1xx_pinirqattach(pinset, NULL, NULL);
+
+	} else {
+		ret = s32k1xx_pinirqattach(pinset, func, arg);
+		pinset &= ~_PIN_INT_MASK;
+
+		if (risingedge) {
+			pinset |= PIN_INT_RISING;
+		}
+
+		if (fallingedge) {
+			pinset |= PIN_INT_FALLING;
+		}
+
+		s32k1xx_pinirqenable(pinset);
+	}
+
+	return ret;
+}
+#endif /* CONFIG_S32K1XX_GPIOIRQ */

platforms/posix/src/px4/common/px4_init.cpp

@@ -44,12 +44,12 @@ int px4_platform_init(void)
 {
 	hrt_init();
 
+	param_init();
+
 	px4::WorkQueueManagerStart();
 
 	uorb_start();
 
-	param_init();
-
 	px4_log_initialize();
 
 	return PX4_OK;

platforms/qurt/include/termios.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,49 +31,7 @@
  *
  ****************************************************************************/
 
-/**
- * First order "alpha" IIR digital filter with input saturation
- */
+// This file prevents a missing header file error but since Qurt doesn't support
+// termios the actual code will still need to be changed for Qurt
 
-#include <mathlib/mathlib.h>
-
-class InnovationLpf final
-{
-public:
-	InnovationLpf() = default;
-	~InnovationLpf() = default;
-
-	void reset(float val = 0.f) { _x = val; }
-
-	/**
-	 * Update the filter with a new value and returns the filtered state
-	 * The new value is constained by the limit set in setSpikeLimit
-	 * @param val new input
-	 * @param alpha normalized weight of the new input
-	 * @param spike_limit the amplitude of the saturation at the input of the filter
-	 * @return filtered output
-	 */
-	float update(float val, float alpha, float spike_limit)
-	{
-		float val_constrained = math::constrain(val, -spike_limit, spike_limit);
-		float beta = 1.f - alpha;
-
-		_x = beta * _x + alpha * val_constrained;
-
-		return _x;
-	}
-
-	/**
-	 * Helper function to compute alpha from dt and the inverse of tau
-	 * @param dt sampling time in seconds
-	 * @param tau_inv inverse of the time constant of the filter
-	 * @return alpha, the normalized weight of a new measurement
-	 */
-	static float computeAlphaFromDtAndTauInv(float dt, float tau_inv)
-	{
-		return math::constrain(dt * tau_inv, 0.f, 1.f);
-	}
-
-private:
-	float _x{}; ///< current state of the filter
-};
+typedef unsigned int speed_t;

platforms/ros2/include/px4_platform_common/param.h

@@ -107,6 +107,7 @@ public:
 
 	Param()
 	{
+		//param_set_used(handle());
 		update();
 	}
 
@@ -142,6 +143,12 @@ public:
 
 	void set(float val) { _val = val; }
 
+	void reset()
+	{
+		//param_reset_no_notification(handle()); // TODO
+		update();
+	}
+
 	bool update()
 	{
 		return true; // TODO
@@ -164,6 +171,7 @@ public:
 	Param(float &external_val)
 		: _val(external_val)
 	{
+		//param_set_used(handle()); // TODO
 		update();
 	}
 
@@ -199,6 +207,12 @@ public:
 
 	void set(float val) { _val = val; }
 
+	void reset()
+	{
+		//param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update()
 	{
 		return true;
@@ -219,6 +233,7 @@ public:
 
 	Param()
 	{
+		//param_set_used(handle());
 		update();
 	}
 
@@ -254,6 +269,12 @@ public:
 
 	void set(int32_t val) { _val = val; }
 
+	void reset()
+	{
+		//param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update()
 	{
 		//return param_get(handle(), &_val) == 0;
@@ -276,6 +297,7 @@ public:
 	Param(int32_t &external_val)
 		: _val(external_val)
 	{
+		//param_set_used(handle());
 		update();
 	}
 
@@ -311,6 +333,12 @@ public:
 
 	void set(int32_t val) { _val = val; }
 
+	void reset()
+	{
+		//param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update()
 	{
 		//return param_get(handle(), &_val) == 0;
@@ -331,6 +359,7 @@ public:
 
 	Param()
 	{
+		//param_set_used(handle());
 		update();
 	}
 
@@ -368,6 +397,12 @@ public:
 
 	void set(bool val) { _val = val; }
 
+	void reset()
+	{
+		//param_reset_no_notification(handle());
+		update();
+	}
+
 	bool update()
 	{
 		int32_t value_int;

src/drivers/actuators/modalai_esc/CMakeLists.txt

@@ -47,4 +47,5 @@ px4_add_module(
 		qc_esc_packet.h
 	DEPENDS
 		px4_work_queue
+		mixer_module
 	)

src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp

@@ -53,13 +53,18 @@ int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
 	}
 
 	/* Open UART */
+#ifdef __PX4_QURT
+	_uart_fd = qurt_uart_open(dev, speed);
+#else
 	_uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
+#endif
 
 	if (_uart_fd < 0) {
 		PX4_ERR("Error opening port: %s (%i)", dev, errno);
 		return -1;
 	}
 
+#ifndef __PX4_QURT
 	/* Back up the original UART configuration to restore it after exit */
 	int termios_state;
 
@@ -98,6 +103,8 @@ int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
 		return -1;
 	}
 
+#endif
+
 	_speed =  speed;
 
 	return 0;
@@ -105,6 +112,8 @@ int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
 
 int ModalaiEscSerial::uart_set_baud(speed_t speed)
 {
+#ifndef __PX4_QURT
+
 	if (_uart_fd < 0) {
 		return -1;
 	}
@@ -120,10 +129,15 @@ int ModalaiEscSerial::uart_set_baud(speed_t speed)
 	_speed = speed;
 
 	return 0;
+#endif
+
+	return -1;
 }
 
 int ModalaiEscSerial::uart_close()
 {
+#ifndef __PX4_QURT
+
 	if (_uart_fd < 0) {
 		PX4_ERR("invalid state for closing");
 		return -1;
@@ -137,6 +151,8 @@ int ModalaiEscSerial::uart_close()
 		PX4_ERR("error closing uart");
 	}
 
+#endif
+
 	_uart_fd = -1;
 
 	return 0;
@@ -149,7 +165,11 @@ int ModalaiEscSerial::uart_write(FAR void *buf, size_t len)
 		return -1;
 	}
 
+#ifdef __PX4_QURT
+	return qurt_uart_write(_uart_fd, (const char *) buf, len);
+#else
 	return write(_uart_fd, buf, len);
+#endif
 }
 
 int ModalaiEscSerial::uart_read(FAR void *buf, size_t len)
@@ -159,5 +179,13 @@ int ModalaiEscSerial::uart_read(FAR void *buf, size_t len)
 		return -1;
 	}
 
+#ifdef __PX4_QURT
+#define ASYNC_UART_READ_WAIT_US 2000
+	// The UART read on SLPI is via an asynchronous service so specify a timeout
+	// for the return. The driver will poll periodically until the read comes in
+	// so this may block for a while. However, it will timeout if no read comes in.
+	return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US);
+#else
 	return read(_uart_fd, buf, len);
+#endif
 }

src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp

@@ -38,6 +38,10 @@
 #include <fcntl.h>
 #include <termios.h>
 
+#ifdef __PX4_QURT
+#include <drivers/device/qurt/uart.h>
+#define FAR
+#endif
 
 class ModalaiEscSerial
 {
@@ -55,7 +59,11 @@ public:
 
 private:
 	int			_uart_fd = -1;
+
+#if ! defined(__PX4_QURT)
 	struct termios		_orig_cfg;
 	struct termios		_cfg;
+#endif
+
 	int   _speed = -1;
 };

src/drivers/uavcan/uavcan_main.cpp

@@ -701,8 +701,7 @@ UavcanNode::Run()
 
 	// Check for parameter requests (get/set/list)
 	if (_param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-
-		parameter_request_s request{};
+		uavcan_parameter_request_s request{};
 		_param_request_sub.copy(&request);
 
 		if (_param_counts[request.node_id]) {
@@ -710,14 +709,14 @@ UavcanNode::Run()
 			 * We know how many parameters are exposed by this node, so
 			 * process the request.
 			 */
-			if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) {
+			if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) {
 				uavcan::protocol::param::GetSet::Request req;
 
 				if (request.param_index >= 0) {
 					req.index = request.param_index;
 
 				} else {
-					req.name = (char *)request.name;
+					req.name = (char *)request.param_id;
 				}
 
 				int call_res = _param_getset_client.call(request.node_id, req);
@@ -730,28 +729,24 @@ UavcanNode::Run()
 					_param_index = request.param_index;
 				}
 
-			} else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
+			} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
 				uavcan::protocol::param::GetSet::Request req;
 
 				if (request.param_index >= 0) {
 					req.index = request.param_index;
 
 				} else {
-					req.name = (char *)request.name;
+					req.name = (char *)request.param_id;
 				}
 
-				switch (request.type) {
-				case parameter_request_s::TYPE_BOOL:
-					req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int32_value;
-					break;
+				if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_REAL32) {
+					req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
 
-				case parameter_request_s::TYPE_FLOAT32:
-					req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.float32_value;
-					break;
+				} else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) {
+					req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
 
-				default:
-					req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int32_value;
-					break;
+				} else {
+					req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
 				}
 
 				// Set the dirty bit for this node
@@ -767,7 +762,7 @@ UavcanNode::Run()
 					_param_index = request.param_index;
 				}
 
-			} else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
+			} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
 				// This triggers the _param_list_in_progress case below.
 				_param_index = 0;
 				_param_list_in_progress = true;
@@ -777,8 +772,8 @@ UavcanNode::Run()
 				PX4_DEBUG("starting component-specific param list");
 			}
 
-		} else if (request.node_id == parameter_request_s::NODE_ID_ALL) {
-			if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
+		} else if (request.node_id == uavcan_parameter_request_s::NODE_ID_ALL) {
+			if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
 				/*
 				 * This triggers the _param_list_in_progress case below,
 				 * but additionally iterates over all active nodes.
@@ -1096,24 +1091,24 @@ UavcanNode::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::G
 		if (result.isSuccessful()) {
 			uavcan::protocol::param::GetSet::Response param = result.getResponse();
 
-			parameter_value_s response{};
+			uavcan_parameter_value_s response{};
 			response.node_id = result.getCallID().server_node_id.get();
-			strncpy(response.param_name, param.name.c_str(), sizeof(response.param_name) - 1);
-			response.param_name[16] = '\0';
+			strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
+			response.param_id[16] = '\0';
 			response.param_index = _param_index;
 			response.param_count = _param_counts[response.node_id];
 
 			if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
-				response.type = parameter_request_s::TYPE_INT32;
-				response.int32_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_INT64;
+				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
 
 			} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
-				response.type = parameter_request_s::TYPE_FLOAT32;
-				response.float32_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_REAL32;
+				response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
 
 			} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
-				response.type = parameter_request_s::TYPE_BOOL;
-				response.int32_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_UINT8;
+				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
 			}
 
 			_param_response_pub.publish(response);

src/drivers/uavcan/uavcan_main.hpp

@@ -75,8 +75,8 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/parameter_request.h>
-#include <uORB/topics/parameter_value.h>
+#include <uORB/topics/uavcan_parameter_request.h>
+#include <uORB/topics/uavcan_parameter_value.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
 using namespace time_literals;
@@ -267,7 +267,7 @@ private:
 	uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)};
 
-	uORB::Publication<parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
+	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
 	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	/*

src/drivers/uavcan/uavcan_servers.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2021 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/lib/adsb/parameters.c

@@ -226,3 +226,26 @@ PARAM_DEFINE_INT32(ADSB_GPS_OFF_LAT, 0);
  * @group ADSB
  */
 PARAM_DEFINE_INT32(ADSB_GPS_OFF_LON, 0);
+
+/**
+ * First 4 characters of CALLSIGN
+ *
+ * Sets first 4 characters of a total of 8. Valid characters are A-Z, 0-9, " ". Example "PX4 " -> 1347957792
+ * For CALLSIGN shorter than 8 characters use the null terminator at the end '\0'.
+ *
+ * @reboot_required true
+ * @group ADSB
+ */
+
+PARAM_DEFINE_INT32(ADSB_CALLSIGN_1, 0);
+
+/**
+* Second 4 characters of CALLSIGN
+*
+* Sets second 4 characters of a total of 8. Valid characters are A-Z, 0-9, " " only. Example "TEST" -> 1413829460
+* For CALLSIGN shorter than 8 characters use the null terminator at the end '\0'.
+*
+* @reboot_required true
+* @group ADSB
+*/
+PARAM_DEFINE_INT32(ADSB_CALLSIGN_2, 0);

src/lib/airspeed/airspeed.cpp

@@ -228,7 +228,7 @@ float get_air_density(float static_pressure, float temperature_celsius)
 	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
 }
 
-float calc_CAS_from_TAS(float speed_true, float pressure_ambient, float temperature_celsius)
+float calc_calibrated_from_true_airspeed(float speed_true, float air_density)
 {
-	return speed_true * sqrtf(get_air_density(pressure_ambient, temperature_celsius) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 }

src/lib/airspeed/airspeed.h

@@ -130,16 +130,13 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe
 __EXPORT float get_air_density(float static_pressure, float temperature_celsius);
 
 /**
- * Calculate calibrated airspeed (CAS) from true airspeed (TAS).
- * It is the inverse function to calc_TAS_from_CAS()
+ * @brief Calculates calibrated airspeed from true airspeed and air density
  *
- * @param speed_true current true airspeed
- * @param pressure_ambient pressure at the side of the tube/airplane
- * @param temperature_celsius air temperature in degrees Celsius
- * @return CAS in m/s
+ * @param speed_true true airspeed [m/s]
+ * @param air_density air density [kg/m3]
+ * @return calibrated airspeed [m/s]
  */
-__EXPORT float calc_CAS_from_TAS(float speed_true, float pressure_ambient,
-				 float temperature_celsius);
+__EXPORT float calc_calibrated_from_true_airspeed(float speed_true, float air_density);
 
 __END_DECLS
 

src/lib/drivers/device/CMakeLists.txt

@@ -43,7 +43,7 @@ if (${PX4_PLATFORM} STREQUAL "nuttx")
 elseif((${PX4_PLATFORM} MATCHES "qurt"))
 	# list(APPEND SRCS_PLATFORM qurt/I2C.cpp)
 	list(APPEND SRCS_PLATFORM qurt/SPI.cpp)
-	# list(APPEND SRCS_PLATFORM qurt/uart.c)
+	list(APPEND SRCS_PLATFORM qurt/uart.c)
 elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type
 	# Linux I2Cdev and SPIdev
 	list(APPEND SRCS_PLATFORM

src/lib/events/libevents

@@ -1 +1 @@
-Subproject commit 0c8bc543db2f8c78f59214d5bcf959bdadd96677
+Subproject commit 8d9c5551273a52e22253ea6abf28d9e4b05e0ab7

src/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -52,7 +52,7 @@ class AlphaFilter
 {
 public:
 	AlphaFilter() = default;
-	explicit AlphaFilter(float alpha) : _alpha(alpha) {}
+	AlphaFilter(float alpha) : _alpha(alpha) {}
 
 	~AlphaFilter() = default;
 
@@ -99,7 +99,7 @@ public:
 	 *
 	 * @param sample new initial value
 	 */
-	void reset(const T &sample) { _filter_state = sample; }
+	void reset(const T &sample = {}) { _filter_state = sample; }
 
 	/**
 	 * Add a new raw value to the filter

src/lib/parameters/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2017 - 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2017 - 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
@@ -149,12 +149,6 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp)
 
 set(SRCS)
 
-set(PX4_PARAM_CLIENT_SRV 1)
-if(PX4_PARAM_CLIENT_SRV)
-	add_compile_definitions(PX4_PARAM_CLIENT_SRV)
-	list(APPEND SRCS parameters_srv.cpp)
-endif()
-
 list(APPEND SRCS parameters.cpp)
 
 if(BUILD_TESTING)
@@ -163,23 +157,31 @@ else()
 	list(APPEND SRCS param_translation.cpp)
 endif()
 
+if(${PX4_PLATFORM} STREQUAL "nuttx")
+	add_subdirectory(flashparams)
+
+	# build user-side interface for protected build
+	if(NOT CONFIG_BUILD_FLAT)
+		list(APPEND SRCS parameters_ioctl.cpp)
+		add_library(usr_parameters usr_parameters_if.cpp px4_parameters.hpp)
+		add_dependencies(usr_parameters prebuild_targets)
+		target_compile_definitions(usr_parameters PRIVATE -DMODULE_NAME="usr_parameters")
+	endif()
+endif()
+
 # TODO: find a better way to do this
 if(NOT "${PX4_BOARD}" MATCHES "px4_io")
 	add_library(parameters STATIC EXCLUDE_FROM_ALL
 		${SRCS}
 		px4_parameters.hpp
-		ParameterServer.cpp
-		ParameterServer.hpp
 	)
 
 	target_link_libraries(parameters PRIVATE perf tinybson px4_platform)
 
-	target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters")
+	target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters" -D__KERNEL__)
 	target_compile_options(parameters
 		PRIVATE
 			#-DDEBUG_BUILD
-			#${MAX_CUSTOM_OPT_LEVEL}
-			-O0
 			-Wno-cast-align		# TODO: fix and enable
 			-Wno-sign-compare	# TODO: fix and enable
 	)
@@ -189,7 +191,7 @@ endif()
 add_dependencies(parameters prebuild_targets)
 
 if(${PX4_PLATFORM} STREQUAL "nuttx")
-	#target_link_libraries(parameters PRIVATE flashparams)
+	target_link_libraries(parameters PRIVATE flashparams)
 endif()
 
 px4_add_functional_gtest(SRC ParameterTest.cpp LINKLIBS parameters)

src/lib/parameters/ParameterServer.hpp

@@ -1,516 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <px4_platform_common/atomic_bitset.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/posix.h>
-#include <px4_platform_common/sem.h>
-#include <px4_platform_common/shutdown.h>
-#include <containers/Bitset.hpp>
-#include <drivers/drv_hrt.h>
-#include <lib/perf/perf_counter.h>
-
-#include "tinybson/tinybson.h"
-#include "uthash/utarray.h"
-
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_request.h>
-#include <uORB/topics/parameter_value.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <uORB/topics/srv_parameter_get_request.h>
-#include <uORB/topics/srv_parameter_get_response.h>
-#include <uORB/topics/srv_parameter_set_request.h>
-#include <uORB/topics/srv_parameter_set_response.h>
-
-#include "param.h"
-#include <parameters/px4_parameters.hpp>
-
-using namespace time_literals;
-
-class ParameterServer : public px4::ScheduledWorkItem
-{
-public:
-	ParameterServer();
-	~ParameterServer() override;
-
-	/**
-	 * Look up a parameter by name.
-	 *
-	 * @param name		The canonical name of the parameter being looked up.
-	 * @return		A handle to the parameter, or PARAM_INVALID if the parameter does not exist.
-	 */
-	param_t findParameter(const char *name, bool notification = true);
-
-	/**
-	 * Return the total number of parameters.
-	 *
-	 * @return		The number of parameters.
-	 */
-	unsigned count() const { return param_info_count; }
-
-	/**
-	 * Return the actually used number of parameters.
-	 *
-	 * @return		The number of parameters.
-	 */
-	unsigned countUsed() const { return _params_active.count(); }
-
-	/**
-	 * Wether a parameter is in use in the system.
-	 *
-	 * @return		True if it has been written or read
-	 */
-	bool isParameterUsed(param_t param) const;
-
-	/**
-	 * Look up a parameter by index.
-	 *
-	 * @param index		An index from 0 to n, where n is param_count()-1.
-	 * @return		A handle to the parameter, or PARAM_INVALID if the index is out of range.
-	 */
-	param_t forIndex(unsigned index) const;
-
-	/**
-	 * Look up an used parameter by index.
-	 *
-	 * @param index		The parameter to obtain the index for.
-	 * @return		The index of the parameter in use, or -1 if the parameter does not exist.
-	 */
-	param_t forUsedIndex(unsigned index) const;
-
-	/**
-	 * Look up the index of a parameter.
-	 *
-	 * @param param		The parameter to obtain the index for.
-	 * @return		The index, or -1 if the parameter does not exist.
-	 */
-	int getParameterIndex(param_t param) const;
-
-	/**
-	 * Look up the index of an used parameter.
-	 *
-	 * @param param		The parameter to obtain the index for.
-	 * @return		The index of the parameter in use, or -1 if the parameter does not exist.
-	 */
-	int getParameterUsedIndex(param_t param) const;
-
-	/**
-	 * Obtain the name of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @return		The name assigned to the parameter, or NULL if the handle is invalid.
-	 */
-	const char *getParameterName(param_t param) const;
-
-	/**
-	 * Obtain the volatile state of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @return			true if the parameter is volatile
-	 */
-	bool isParameterVolatile(param_t param) const;
-
-	/**
-	 * Test whether a parameter's value has changed from the default.
-	 *
-	 * @return		If true, the parameter's value has not been changed from the default.
-	 */
-	bool isParameterValueDefault(param_t param);
-
-	/**
-	 * Test whether a parameter's value has been changed but not saved.
-	 *
-	 * @return		If true, the parameter's value has not been saved.
-	 */
-	bool isParameterValueUnsaved(param_t param);
-
-	/**
-	 * Obtain the type of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @return		The type assigned to the parameter.
-	 */
-	param_type_t getParameterType(param_t param) const;
-
-	/**
-	 * Determine the size of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @return		The size of the parameter's value.
-	 */
-	size_t getParameterSize(param_t param) const;
-
-	/**
-	 * Copy the value of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @param val		Where to return the value, assumed to point to suitable storage for the parameter type.
-	 * @return		Zero if the parameter's value could be returned, nonzero otherwise.
-	 */
-	int getParameterValue(param_t param, void *val);
-
-	/**
-	 * Copy the (airframe-specific) default value of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @param default_val	Where to return the value, assumed to point to suitable storage for the parameter type.
-	 * @return		Zero if the parameter's deafult value could be returned, nonzero otherwise.
-	 */
-	int getParameterDefaultValue(param_t param, void *default_val);
-
-	/**
-	 * Copy the system-wide default value of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @param default_val	Where to return the value, assumed to point to suitable storage for the parameter type.
-	 * @return		Zero if the parameter's deafult value could be returned, nonzero otherwise.
-	 */
-	int getParameterSystemDefaultValue(param_t param, void *default_val);
-
-	/**
-	 * Set the value of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @param val		The value to set; assumed to point to a variable of the parameter type.
-	 * @return		Zero if the parameter's value could be set from a scalar, nonzero otherwise.
-	 */
-	int setParameter(param_t param, const void *val, bool mark_saved = true, bool notify_changes = true);
-
-	/**
-	 * Set the default value of a parameter.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @param val		The default value to set; assumed to point to a variable of the parameter type.
-	 * @return		Zero if the parameter's default value could be set from a scalar, nonzero otherwise.
-	 */
-	int setParameterDefaultValue(param_t param, const void *val);
-
-	/**
-	 * Notify the system about parameter changes. Can be used for example after several calls to
-	 * param_set_no_notification() to avoid unnecessary system notifications.
-	 */
-	void notifyChanges();
-
-	/**
-	 * Reset all parameters to their default values.
-	 */
-	void resetAllParameters(bool auto_save = true);
-
-	/**
-	 * Reset all parameters to their default values except for excluded parameters.
-	 *
-	 * @param excludes			Array of param names to exclude from resetting. Use a wildcard
-	 *							at the end to exclude parameters with a certain prefix.
-	 * @param num_excludes		The number of excludes provided.
-	 */
-	void resetExcludes(const char *excludes[], int num_excludes);
-
-	/**
-	 * Reset only specific parameters to their default values.
-	 *
-	 * @param resets Array of param names to reset. Use a wildcard at the end to reset parameters with a certain prefix.
-	 * @param num_resets The number of passed reset conditions in the resets array.
-	 */
-	void resetSpecificParameter(const char *resets[], int num_resets);
-
-	/**
-	 * Export changed parameters to a file.
-	 * Note: this method requires a large amount of stack size!
-	 *
-	 * @param filename	Path to the default parameter file.
-	 * @param filter	Filter parameters to be exported. The method should return true if
-	 * 			the parameter should be exported. No filtering if nullptr is passed.
-	 * @return		Zero on success, nonzero on failure.
-	 */
-	typedef bool(*param_filter_func)(param_t handle);
-	int exportToFile(const char *filename, param_filter_func filter);
-
-	/**
-	 * Import parameters from a file, discarding any unrecognized parameters.
-	 *
-	 * This function merges the imported parameters with the current parameter set.
-	 *
-	 * @param fd		File descriptor to import from (-1 selects the FLASH storage).
-	 * @return		Zero on success, nonzero if an error occurred during import.
-	 *			Note that in the failure case, parameters may be inconsistent.
-	 */
-	int importFromFileDescriptor(int fd);
-
-	/**
-	 * Load parameters from a file.
-	 *
-	 * This function resets all parameters to their default values, then loads new
-	 * values from a file.
-	 *
-	 * @param fd		File descriptor to import from (-1 selects the FLASH storage).
-	 * @return		Zero on success, nonzero if an error occurred during import.
-	 *			Note that in the failure case, parameters may be inconsistent.
-	 */
-	int loadFromFileDescriptor(int fd);
-
-	/**
-	 * Apply a function to each parameter.
-	 *
-	 * Note that the parameter set is not locked during the traversal. It also does
-	 * not hold an internal state, so the callback function can block or sleep between
-	 * parameter callbacks.
-	 *
-	 * @param func		The function to invoke for each parameter.
-	 * @param arg		Argument passed to the function.
-	 * @param only_changed	If true, the function is only called for parameters whose values have
-	 *			been changed from the default.
-	 * @param only_used	If true, the function is only called for parameters which have been
-	 *			used in one of the running applications.
-	 */
-	void forEachParameter(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used);
-
-	/**
-	 * Set the default parameter file name.
-	 * This has no effect if the FLASH-based storage is enabled.
-	 *
-	 * @param filename	Path to the default parameter file.  The file is not required to
-	 *			exist.
-	 * @return		Zero on success.
-	 */
-	int setDefaultFile(const char *filename);
-
-	/**
-	 * Get the default parameter file name.
-	 *
-	 * @return		The path to the current default parameter file; either as
-	 *			a result of a call to param_set_default_file, or the
-	 *			built-in default.
-	 */
-	const char *getDefaultFile() const { return _param_default_file; }
-
-	/**
-	 * Set the backup parameter file name.
-	 *
-	 * @param filename	Path to the backup parameter file. The file is not required to
-	 *			exist.
-	 * @return		Zero on success.
-	 */
-	int setBackupFile(const char *filename);
-
-	/**
-	 * Get the backup parameter file name.
-	 *
-	 * @return		The path to the backup parameter file
-	 */
-	const char *getBackupFile() const { return _param_backup_file; }
-
-	/**
-	 * Automatically save the parameters after a timeout and limited rate.
-	 *
-	 * This needs to be called with the writer lock held (it's not necessary that it's the writer lock, but it
-	 * needs to be the same lock as autosave_worker() and param_control_autosave() use).
-	 */
-	void autoSave(bool now = false);
-
-	/**
-	 * Load parameters from the default parameter file.
-	 *
-	 * @return		Zero on success.
-	 */
-	int loadDefault();
-
-	/**
-	 * Generate the hash of all parameters and their values
-	 *
-	 * @return		CRC32 hash of all param_ids and values
-	 */
-	uint32_t hashCheck();
-
-	/**
-	 * Print the status of the param system
-	 *
-	 */
-	void printStatus();
-
-	/**
-	 * Enable/disable the param autosaving.
-	 * Re-enabling with changed params will not cause an autosave.
-	 * @param enable true: enable autosaving, false: disable autosaving
-	 */
-	void controlAutosave(bool enable);
-
-private:
-
-	static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s);
-
-	/**
-	 * Test whether a param_t is value.
-	 *
-	 * @param param			The parameter handle to test.
-	 * @return			True if the handle is valid.
-	 */
-	static constexpr bool handle_in_range(param_t param) { return (param < param_info_count); }
-
-	void lockReader();   // lock the parameter store for read access
-	void unlockReader(); // unlock the parameter store
-
-	void lockWriter();   // lock the parameter store for write access
-	void unlockWriter(); // unlock the parameter store
-
-	// Storage for modified parameters.
-	struct param_wbuf_s {
-		union param_value_u val {};
-		param_t param{PARAM_INVALID};
-	};
-
-	/**
-	 * Compare two modified parameter structures to determine ordering.
-	 *
-	 * This function is suitable for passing to qsort or bsearch.
-	 */
-	static int compareValues(const void *a, const void *b);
-
-	/**
-	 * Locate the modified parameter structure for a parameter, if it exists.
-	 *
-	 * @param param			The parameter being searched.
-	 * @return			The structure holding the modified value, or
-	 *				nullptr if the parameter has not been modified.
-	*/
-	param_wbuf_s *findChanged(param_t param);
-
-	/**
-	 * Reset a parameter to its default value.
-	 *
-	 * @param param		A handle returned by param_find or passed by param_foreach.
-	 * @return		Zero on success, nonzero on failure
-	 */
-	int resetParameter(param_t param);
-
-	/**
-	 * Obtain a pointer to the storage allocated for a parameter.
-	 *
-	 * @param param			The parameter whose storage is sought.
-	 * @return			A pointer to the parameter value, or nullptr
-	 *				if the parameter does not exist.
-	*/
-	const void *getParameterValuePointer(param_t param);
-
-	// get parameter default value, caller is responsible for locking
-	int getParameterDefaultValueInternal(param_t param, void *default_val);
-
-	/**
-	 * Save parameters to the default file.
-	 * Note: this method requires a large amount of stack size!
-	 *
-	 * This function saves all parameters with non-default values.
-	 *
-	 * @return		Zero on success.
-	 */
-	int saveDefault();
-
-	// internal parameter export, caller is responsible for locking
-	int exportInternal(int fd, param_filter_func filter);
-
-	int bsonImportCallback(bson_decoder_t decoder, bson_node_t node);
-	static int importCallbackTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node);
-	int importFromFileDescriptorInternal(int fd);
-
-	int verifyBsonExportCallback(bson_decoder_t decoder, bson_node_t node);
-	static int verifyBsonExportTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node);
-	int verifyBsonExport(int fd);
-
-	char *_param_default_file{nullptr};
-	char *_param_backup_file{nullptr};
-
-	px4::AtomicBitset<param_info_count> _params_active;  // params found
-	px4::AtomicBitset<param_info_count> _params_changed; // params non-default
-	px4::Bitset<param_info_count> _params_custom_default; // params with runtime default value
-	px4::AtomicBitset<param_info_count> _params_unsaved;
-
-	/** flexible array holding modified parameter values */
-	UT_array *_param_values{nullptr};
-	UT_array *_param_custom_default_values{nullptr};
-
-	const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr};
-
-	unsigned int _param_instance{0};
-
-	// the following implements an RW-lock using 2 semaphores (used as mutexes). It gives
-	// priority to readers, meaning a writer could suffer from starvation, but in our use-case
-	// we only have short periods of reads and writes are rare.
-	px4_sem_t _param_sem; ///< this protects against concurrent access to _param_values
-	int _reader_lock_holders{0};
-	px4_sem_t _reader_lock_holders_lock; ///< this protects against concurrent access to reader_lock_holders
-
-	perf_counter_t _export_perf{perf_alloc(PC_ELAPSED, "param: export")};
-	perf_counter_t _find_count_perf{perf_alloc(PC_COUNT, "param: find")};
-	perf_counter_t _get_count_perf{perf_alloc(PC_COUNT, "param: get")};
-	perf_counter_t _set_perf{perf_alloc(PC_ELAPSED, "param: set")};
-
-	px4_sem_t _param_sem_save; ///< this protects against concurrent param saves (file or flash access).
-	///< we use a separate lock to allow concurrent param reads and saves.
-	///< a param_set could still be blocked by a param save, because it
-	///< needs to take the reader lock
-
-
-	void Run() override;
-
-	uORB::Publication<parameter_update_s> _parameter_update_pub{ORB_ID(parameter_update)};
-
-	// srv: parameter_get
-	uORB::SubscriptionCallbackWorkItem _srv_parameter_get_request_sub{this, ORB_ID(srv_parameter_get_request)};
-	uORB::Publication<srv_parameter_get_response_s> _srv_parameter_get_response_pub{ORB_ID(srv_parameter_get_response)};
-
-	// srv: parameter_set
-	uORB::SubscriptionCallbackWorkItem _srv_parameter_set_request_sub{this, ORB_ID(srv_parameter_set_request)};
-	uORB::Publication<srv_parameter_set_response_s> _srv_parameter_set_response_pub{ORB_ID(srv_parameter_set_response)};
-
-	// TODO: replace
-	uORB::Publication<parameter_value_s> _param_response_pub{ORB_ID(parameter_value)};
-	uORB::SubscriptionCallbackWorkItem _param_request_sub{this, ORB_ID(parameter_request)};
-
-	uORB::SubscriptionData<actuator_armed_s> _armed_sub{ORB_ID(actuator_armed)};
-
-
-	// autosaving variables
-	hrt_abstime _last_autosave_timestamp{0};
-	px4::atomic_bool _autosave_scheduled{false};
-	bool _autosave_disabled{false};
-
-	px4::atomic_bool _notify_scheduled{false};
-
-};

src/lib/parameters/flashparams/flashparams.cpp

@@ -87,12 +87,12 @@ param_export_internal(param_filter_func filter)
 	bson_encoder_init_buf(&encoder, nullptr, 0);
 
 	/* no modified parameters -> we are done */
-	if (_param_values == nullptr) {
+	if (param_values == nullptr) {
 		result = 0;
 		goto out;
 	}
 
-	while ((s = (struct param_wbuf_s *)utarray_next(_param_values, s)) != nullptr) {
+	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) {
 
 		int32_t i;
 		float   f;
@@ -103,12 +103,12 @@ param_export_internal(param_filter_func filter)
 
 		/* append the appropriate BSON type object */
 
-		switch (getParameterType(s->param)) {
+		switch (param_type(s->param)) {
 		case PARAM_TYPE_INT32:
 			i = s->val.i;
 
-			if (bson_encoder_append_int32(&encoder, getParameterName(s->param), i)) {
-				debug("BSON append failed for '%s'", getParameterName(s->param));
+			if (bson_encoder_append_int32(&encoder, param_name(s->param), i)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
 				goto out;
 			}
 
@@ -117,8 +117,8 @@ param_export_internal(param_filter_func filter)
 		case PARAM_TYPE_FLOAT:
 			f = s->val.f;
 
-			if (bson_encoder_append_double(&encoder, getParameterName(s->param), f)) {
-				debug("BSON append failed for '%s'", getParameterName(s->param));
+			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
 				goto out;
 			}
 
@@ -214,7 +214,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node)
 
 	switch (node->type) {
 	case BSON_INT32:
-		if (getParameterType(param) != PARAM_TYPE_INT32) {
+		if (param_type(param) != PARAM_TYPE_INT32) {
 			PX4_WARN("unexpected type for %s", node->name);
 			result = 1; // just skip this entry
 			goto out;
@@ -225,7 +225,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node)
 		break;
 
 	case BSON_DOUBLE:
-		if (getParameterType(param) != PARAM_TYPE_FLOAT) {
+		if (param_type(param) != PARAM_TYPE_FLOAT) {
 			PX4_WARN("unexpected type for %s", node->name);
 			result = 1; // just skip this entry
 			goto out;

src/lib/parameters/param.h

@@ -197,6 +197,14 @@ __EXPORT bool		param_value_unsaved(param_t param);
  */
 __EXPORT param_type_t	param_type(param_t param);
 
+/**
+ * Determine the size of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The size of the parameter's value.
+ */
+__EXPORT size_t		param_size(param_t param);
+
 /**
  * Copy the value of a parameter.
  *
@@ -242,6 +250,14 @@ __EXPORT int		param_set(param_t param, const void *val);
  */
 __EXPORT int		param_set_default_value(param_t param, const void *val);
 
+/**
+ * Mark a parameter as used. Only marked parameters will be sent to a GCS.
+ * A call to param_find() will mark a param as used as well.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ */
+__EXPORT void		param_set_used(param_t param);
+
 /**
  * Set the value of a parameter, but do not notify the system about the change.
  *
@@ -257,6 +273,22 @@ __EXPORT int		param_set_no_notification(param_t param, const void *val);
  */
 __EXPORT void		param_notify_changes(void);
 
+/**
+ * Reset a parameter to its default value.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		Zero on success, nonzero on failure
+ */
+__EXPORT int		param_reset(param_t param);
+
+/**
+ * Reset a parameter to its default value, but do not notify the system about the change.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		Zero on success, nonzero on failure
+ */
+__EXPORT int		param_reset_no_notification(param_t param);
+
 /**
  * Reset all parameters to their default values.
  */
@@ -271,6 +303,8 @@ __EXPORT void		param_reset_all(void);
  */
 __EXPORT void		param_reset_excludes(const char *excludes[], int num_excludes);
 
+typedef bool(*param_filter_func)(param_t handle);
+
 /**
  * Reset only specific parameters to their default values.
  *
@@ -288,7 +322,6 @@ __EXPORT void		param_reset_specific(const char *resets[], int num_resets);
  * 			the parameter should be exported. No filtering if nullptr is passed.
  * @return		Zero on success, nonzero on failure.
  */
-typedef bool(*param_filter_func)(param_t handle);
 __EXPORT int		param_export(const char *filename, param_filter_func filter);
 
 /**

src/lib/parameters/parameters_common.cpp

@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <string.h>
+
+/**
+ * @file parameters_common.cpp
+ *
+ * Global parameter store, functions common to kernel and user sides
+ *
+ */
+
+static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s);
+
+/**
+ * Test whether a param_t is value.
+ *
+ * @param param			The parameter handle to test.
+ * @return			True if the handle is valid.
+ */
+static constexpr bool handle_in_range(param_t param) { return (param < param_info_count); }
+
+unsigned param_count()
+{
+	return param_info_count;
+}
+
+int param_get_index(param_t param)
+{
+	if (handle_in_range(param)) {
+		return (unsigned)param;
+	}
+
+	return -1;
+}
+
+param_t param_for_index(unsigned index)
+{
+	if (index < param_info_count) {
+		return (param_t)index;
+	}
+
+	return PARAM_INVALID;
+}
+
+const char *param_name(param_t param)
+{
+	return handle_in_range(param) ? px4::parameters[param].name : nullptr;
+}
+
+param_type_t param_type(param_t param)
+{
+	return handle_in_range(param) ? px4::parameters_type[param] : PARAM_TYPE_UNKNOWN;
+}
+
+bool param_is_volatile(param_t param)
+{
+	if (handle_in_range(param)) {
+		for (const auto &p : px4::parameters_volatile) {
+			if (static_cast<px4::params>(param) == p) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+size_t param_size(param_t param)
+{
+	if (handle_in_range(param)) {
+		switch (param_type(param)) {
+		case PARAM_TYPE_INT32:
+		case PARAM_TYPE_FLOAT:
+			return 4;
+
+		default:
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+int
+param_get_system_default_value(param_t param, void *default_val)
+{
+	if (!handle_in_range(param)) {
+		return PX4_ERROR;
+	}
+
+	int ret = PX4_OK;
+
+	switch (param_type(param)) {
+	case PARAM_TYPE_INT32:
+		memcpy(default_val, &px4::parameters[param].val.i, param_size(param));
+		break;
+
+	case PARAM_TYPE_FLOAT:
+		memcpy(default_val, &px4::parameters[param].val.f, param_size(param));
+		break;
+
+	default:
+		ret = PX4_ERROR;
+		break;
+	}
+
+	return ret;
+}

src/lib/parameters/parameters_ioctl.cpp

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 Technology Innovation Institute. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file parameters_ioctl.cpp
+ *
+ * Protected build kernel space interface to global parameter store.
+ */
+
+#define PARAM_IMPLEMENTATION
+
+#include <errno.h>
+
+#include "param.h"
+#include "parameters_ioctl.h"
+#include <px4_platform_common/defines.h>
+
+int	param_ioctl(unsigned int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	switch (cmd) {
+	case PARAMIOCNOTIFY: {
+			param_notify_changes();
+		}
+		break;
+
+	case PARAMIOCFIND: {
+			paramiocfind_t *data = (paramiocfind_t *)arg;
+
+			if (data->notification) {
+				data->ret = param_find(data->name);
+
+			} else {
+				data->ret = param_find_no_notification(data->name);
+			}
+		}
+		break;
+
+	case PARAMIOCCOUNTUSED: {
+			paramioccountused_t *data = (paramioccountused_t *)arg;
+			data->ret = param_count_used();
+		}
+		break;
+
+	case PARAMIOCFORUSEDINDEX: {
+			paramiocforusedindex_t *data = (paramiocforusedindex_t *)arg;
+			data->ret =  param_for_used_index(data->index);
+		}
+		break;
+
+	case PARAMIOCGETUSEDINDEX: {
+			paramiocgetusedindex_t *data = (paramiocgetusedindex_t *)arg;
+			data->ret = param_get_used_index(data->param);
+		}
+		break;
+
+	case PARAMIOCUNSAVED: {
+			paramiocunsaved_t *data = (paramiocunsaved_t *)arg;
+			data->ret = param_value_unsaved(data->param);
+		}
+		break;
+
+	case PARAMIOCGET: {
+			paramiocget_t *data = (paramiocget_t *)arg;
+
+			if (data->deflt) {
+				data->ret = param_get_default_value(data->param, data->val);
+
+			} else {
+				data->ret = param_get(data->param, data->val);
+			}
+		}
+		break;
+
+	case PARAMIOCAUTOSAVE: {
+			paramiocautosave_t *data = (paramiocautosave_t *)arg;
+			param_control_autosave(data->enable);
+		}
+		break;
+
+	case PARAMIOCSET: {
+			paramiocset_t *data = (paramiocset_t *)arg;
+
+			if (data->notification) {
+				data->ret = param_set(data->param, data->val);
+
+			} else {
+				data->ret = param_set_no_notification(data->param, data->val);
+			}
+		}
+		break;
+
+	case PARAMIOCUSED: {
+			paramiocused_t *data = (paramiocused_t *)arg;
+			data->ret = param_used(data->param);
+		}
+		break;
+
+	case PARAMIOCSETUSED: {
+			paramiocsetused_t *data = (paramiocsetused_t *)arg;
+			param_set_used(data->param);
+		}
+		break;
+
+	case PARAMIOCSETDEFAULT: {
+			paramiocsetdefault_t *data = (paramiocsetdefault_t *)arg;
+			data->ret = param_set_default_value(data->param, data->val);
+		}
+		break;
+
+	case PARAMIOCRESET: {
+			paramiocreset_t *data = (paramiocreset_t *)arg;
+
+			if (data->notification) {
+				data->ret = param_reset(data->param);
+
+			} else {
+				data->ret = param_reset_no_notification(data->param);
+			}
+		}
+		break;
+
+	case PARAMIOCRESETGROUP: {
+			paramiocresetgroup_t *data = (paramiocresetgroup_t *)arg;
+
+			if (data->type == PARAM_RESET_EXCLUDES) {
+				param_reset_excludes(data->group, data->num_in_group);
+
+			} else if (data->type == PARAM_RESET_SPECIFIC) {
+				param_reset_specific(data->group, data->num_in_group);
+
+			} else {
+				param_reset_all();
+			}
+		}
+		break;
+
+	case PARAMIOCSAVEDEFAULT: {
+			paramiocsavedefault_t *data = (paramiocsavedefault_t *)arg;
+			data->ret = param_save_default();
+		}
+		break;
+
+	case PARAMIOCLOADDEFAULT: {
+			paramiocloaddefault_t *data = (paramiocloaddefault_t *)arg;
+			data->ret = param_load_default();
+		}
+		break;
+
+	case PARAMIOCEXPORT: {
+			paramiocexport_t *data = (paramiocexport_t *)arg;
+			data->ret = param_export(data->filename, nullptr);
+		}
+		break;
+
+	case PARAMIOCHASH: {
+			paramiochash_t *data = (paramiochash_t *)arg;
+			data->ret = param_hash_check();
+		}
+		break;
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	return ret;
+}

src/lib/parameters/parameters_ioctl.h

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 Technology Innovation Institute. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file parameters_ioctl.h
+ *
+ * User space - kernel space interface to global parameter store.
+ */
+
+#pragma once
+
+#define PARAM_IMPLEMENTATION
+#include "param.h"
+#include <px4_platform/board_ctrl.h>
+#include <px4_platform_common/defines.h>
+
+#define _PARAMIOC(_n) (_PX4_IOC(_PARAMIOCBASE, _n))
+
+#define PARAMIOCNOTIFY	_PARAMIOC(1)
+
+#define PARAMIOCFIND	_PARAMIOC(2)
+typedef struct paramiocfind {
+	const char *name;
+	const bool notification;
+	param_t ret;
+} paramiocfind_t;
+
+
+#define PARAMIOCCOUNTUSED	_PARAMIOC(3)
+typedef struct paramioccountused {
+	unsigned ret;
+} paramioccountused_t;
+
+#define PARAMIOCFORUSEDINDEX	_PARAMIOC(4)
+typedef struct paramiocforusedindex {
+	const unsigned index;
+	param_t ret;
+} paramiocforusedindex_t;
+
+#define PARAMIOCGETUSEDINDEX	_PARAMIOC(5)
+typedef struct paramiocgetusedindex {
+	const param_t param;
+	unsigned ret;
+} paramiocgetusedindex_t;
+
+#define PARAMIOCUNSAVED	_PARAMIOC(6)
+typedef struct paramiocunsaved {
+	const param_t param;
+	bool ret;
+} paramiocunsaved_t;
+
+#define PARAMIOCGET	_PARAMIOC(7)
+typedef struct paramiocget {
+	const param_t param;
+	const bool deflt;
+	void *const val;
+	int ret;
+} paramiocget_t;
+
+#define PARAMIOCAUTOSAVE	_PARAMIOC(8)
+typedef struct paramiocautosave {
+	const bool enable;
+} paramiocautosave_t;
+
+#define PARAMIOCSET	_PARAMIOC(9)
+typedef struct paramiocset {
+	const param_t param;
+	const bool notification;
+	const void *val;
+	int ret;
+} paramiocset_t;
+
+#define PARAMIOCUSED	_PARAMIOC(10)
+typedef struct paramiocused {
+	const param_t param;
+	bool ret;
+} paramiocused_t;
+
+#define PARAMIOCSETUSED	_PARAMIOC(11)
+typedef struct paramiocsetused {
+	const param_t param;
+} paramiocsetused_t;
+
+#define PARAMIOCSETDEFAULT	_PARAMIOC(12)
+typedef struct paramiocsetdefault {
+	const param_t param;
+	const void *val;
+	int ret;
+} paramiocsetdefault_t;
+
+#define PARAMIOCRESET	_PARAMIOC(13)
+typedef struct paramiocreset {
+	const param_t param;
+	const bool notification;
+	int ret;
+} paramiocreset_t;
+
+#define PARAMIOCRESETGROUP	_PARAMIOC(14)
+typedef enum {
+	PARAM_RESET_ALL,
+	PARAM_RESET_EXCLUDES,
+	PARAM_RESET_SPECIFIC
+} param_reset_type_t;
+
+typedef struct paramiocresetgroup {
+	param_reset_type_t type;
+	const char **group;
+	const int num_in_group;
+} paramiocresetgroup_t;
+
+#define PARAMIOCSAVEDEFAULT	_PARAMIOC(15)
+typedef struct paramiocsavedefault {
+	int ret;
+} paramiocsavedefault_t;
+
+#define PARAMIOCLOADDEFAULT	_PARAMIOC(16)
+typedef struct paramiocloaddefault {
+	int ret;
+} paramiocloaddefault_t;
+
+
+#define PARAMIOCEXPORT	_PARAMIOC(17)
+typedef struct paramiocexport {
+	const char *filename;
+	int ret;
+} paramiocexport_t;
+
+#define PARAMIOCHASH	_PARAMIOC(18)
+typedef struct paramiochash {
+	uint32_t ret;
+} paramiochash_t;
+
+int param_ioctl(unsigned int cmd, unsigned long arg);

src/lib/parameters/parameters_srv.cpp

@@ -1,189 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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.
- *
- ****************************************************************************/
-
-/**
- * @file parameters.cpp
- *
- * Global parameter store.
- *
- * Note that it might make sense to convert this into a driver.  That would
- * offer some interesting options regarding state for e.g. ORB advertisements
- * and background parameter saving.
- */
-
-#define PARAM_IMPLEMENTATION
-
-#include "param.h"
-
-#if defined(PX4_PARAM_CLIENT_SRV)
-
-#include <uORB/SubscriptionBlocking.hpp>
-#include <uORB/Publication.hpp>
-
-#include <uORB/topics/srv_parameter_get_request.h>
-#include <uORB/topics/srv_parameter_get_response.h>
-
-#include <uORB/topics/srv_parameter_set_request.h>
-#include <uORB/topics/srv_parameter_set_response.h>
-
-using namespace time_literals;
-
-param_t param_find(const char *name)
-{
-	// request
-	uORB::Publication<srv_parameter_get_request_s> request_pub{ORB_ID(srv_parameter_get_request)};
-	uORB::SubscriptionBlocking<srv_parameter_get_response_s> response_sub{ORB_ID(srv_parameter_get_response)};
-	srv_parameter_get_request_s request{};
-	request.index = -1;
-	memcpy(request.name, name, 16);
-	request.timestamp = hrt_absolute_time();
-	request_pub.publish(request);
-
-	// response
-	while ((hrt_elapsed_time(&request.timestamp) < 50_ms) || response_sub.updated()) {
-		const unsigned last_generation = response_sub.get_last_generation();
-		srv_parameter_get_response_s response{};
-
-		if (response_sub.updateBlocking(response, 1'000)) {
-
-			if (response_sub.get_last_generation() != last_generation + 1) {
-				PX4_ERR("param_find: missed srv_parameter_get_response, generation %d -> %d", last_generation,
-					response_sub.get_last_generation());
-			}
-
-			if ((request.timestamp == response.timestamp_requested)
-			    && (strncmp(request.name, response.parameter.name, sizeof(request.name)) == 0)) {
-
-				return response.parameter.index;
-			}
-		}
-	}
-
-	return PARAM_INVALID;
-}
-
-param_t param_find_no_notification(const char *name)
-{
-	return param_find(name);
-}
-
-int param_get(param_t param, void *val)
-{
-	// request
-	uORB::Publication<srv_parameter_get_request_s> request_pub{ORB_ID(srv_parameter_get_request)};
-	uORB::SubscriptionBlocking<srv_parameter_get_response_s> response_sub{ORB_ID(srv_parameter_get_response)};
-	srv_parameter_get_request_s request{};
-	request.index = param;
-	request.timestamp = hrt_absolute_time();
-	request_pub.publish(request);
-
-	// response
-	while ((hrt_elapsed_time(&request.timestamp) < 50_ms) || response_sub.updated()) {
-		const unsigned last_generation = response_sub.get_last_generation();
-		srv_parameter_get_response_s response{};
-
-		if (response_sub.updateBlocking(response, 1'000)) {
-
-			if (response_sub.get_last_generation() != last_generation + 1) {
-				PX4_ERR("param_get: missed srv_parameter_get_response, generation %d -> %d", last_generation,
-					response_sub.get_last_generation());
-			}
-
-			if ((request.timestamp == response.timestamp_requested) && (request.index == response.parameter.index)) {
-				switch (response.parameter.type) {
-				case parameter_s::TYPE_INT32:
-					memcpy(val, &response.parameter.int32_value, sizeof(int32_t));
-					break;
-
-				case parameter_s::TYPE_FLOAT32:
-					memcpy(val, &response.parameter.float32_value, sizeof(float));
-					break;
-				}
-
-				if (response.result == srv_parameter_get_response_s::RESULT_GET_SUCCESS) {
-					return 0;
-				}
-
-				return -1;
-			}
-		}
-	}
-
-	return -1;
-}
-
-int param_set(param_t param, const void *val)
-{
-	// request
-	uORB::Publication<srv_parameter_set_request_s> request_pub{ORB_ID(srv_parameter_set_request)};
-	uORB::SubscriptionBlocking<srv_parameter_set_response_s> response_sub{ORB_ID(srv_parameter_set_response)};
-	srv_parameter_set_request_s request{};
-	request.parameter.index = param;
-	memcpy(&request.parameter.int32_value, val, sizeof(int32_t));
-	memcpy(&request.parameter.float32_value, val, sizeof(float));
-	request.timestamp = hrt_absolute_time();
-	request_pub.publish(request);
-
-	// response
-	while ((hrt_elapsed_time(&request.timestamp) < 50_ms) || response_sub.updated()) {
-		const unsigned last_generation = response_sub.get_last_generation();
-		srv_parameter_set_response_s response{};
-
-		if (response_sub.updateBlocking(response, 1'000)) {
-
-			if (response_sub.get_last_generation() != last_generation + 1) {
-				PX4_ERR("param_set: missed srv_parameter_set_response, generation %d -> %d", last_generation,
-					response_sub.get_last_generation());
-			}
-
-			if ((request.timestamp == response.timestamp_requested) && (request.parameter.index == response.parameter.index)) {
-				if (response.result == srv_parameter_set_response_s::RESULT_SET_SUCCESS) {
-					return 0;
-				}
-
-				PX4_ERR("param_set %d failed", param);
-				return -1;
-			}
-		}
-	}
-
-	PX4_ERR("param_set %d failed", param);
-	return -1;
-}
-
-int param_set_no_notification(param_t param, const void *val)
-{
-	return param_set(param, val);
-}
-
-#endif // PX4_PARAM_CLIENT_SRV

src/lib/parameters/usr_parameters_if.cpp

@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 Technology Innovation Institute. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file usr_parameters_if.cpp
+ *
+ * Protected build user space interface to global parameter store.
+ */
+#define PARAM_IMPLEMENTATION
+#include "param.h"
+#include "parameters_ioctl.h"
+#include <parameters/px4_parameters.hpp>
+#include <sys/boardctl.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/log.h>
+
+#include "parameters_common.cpp"
+
+void
+param_notify_changes()
+{
+	boardctl(PARAMIOCNOTIFY, NULL);
+}
+
+param_t param_find(const char *name)
+{
+	paramiocfind_t data = {name, true, PARAM_INVALID};
+	boardctl(PARAMIOCFIND, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+param_t param_find_no_notification(const char *name)
+{
+	paramiocfind_t data = {name, false, PARAM_INVALID};
+	boardctl(PARAMIOCFIND, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+unsigned param_count_used()
+{
+	paramioccountused_t data = {0};
+	boardctl(PARAMIOCCOUNTUSED, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+param_t param_for_used_index(unsigned index)
+{
+	paramiocforusedindex_t data = {index, 0};
+	boardctl(PARAMIOCFORUSEDINDEX, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int param_get_used_index(param_t param)
+{
+	paramiocgetusedindex_t data = {param, 0};
+	boardctl(PARAMIOCGETUSEDINDEX, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+bool
+param_value_unsaved(param_t param)
+{
+	paramiocunsaved_t data = {param, false};
+	boardctl(PARAMIOCUNSAVED, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int
+param_get(param_t param, void *val)
+{
+	paramiocget_t data = {param, false, val, PX4_ERROR};
+	boardctl(PARAMIOCGET, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int
+param_get_default_value(param_t param, void *default_val)
+{
+	paramiocget_t data = {param, true, default_val, PX4_ERROR};
+	boardctl(PARAMIOCGET, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+void
+param_control_autosave(bool enable)
+{
+	paramiocautosave_t data = {enable};
+	boardctl(PARAMIOCAUTOSAVE, reinterpret_cast<unsigned long>(&data));
+}
+
+int param_set(param_t param, const void *val)
+{
+	paramiocset_t data = {param, true, val, PX4_ERROR};
+	boardctl(PARAMIOCSET, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int param_set_no_notification(param_t param, const void *val)
+{
+	paramiocset_t data = {param, false, val, PX4_ERROR};
+	boardctl(PARAMIOCSET, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+bool param_used(param_t param)
+{
+	paramiocused_t data = {param, false};
+	boardctl(PARAMIOCUSED, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+void param_set_used(param_t param)
+{
+	paramiocsetused_t data = {param};
+	boardctl(PARAMIOCSETUSED, reinterpret_cast<unsigned long>(&data));
+}
+
+int param_set_default_value(param_t param, const void *val)
+{
+	paramiocsetdefault_t data = {param, val, PX4_ERROR};
+	boardctl(PARAMIOCSETDEFAULT, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int param_reset(param_t param)
+{
+	paramiocreset_t data = {param, true, PX4_ERROR};
+	boardctl(PARAMIOCRESET, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int param_reset_no_notification(param_t param)
+{
+	paramiocreset_t data = {param, false, PX4_ERROR};
+	boardctl(PARAMIOCRESET, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+void
+param_reset_all()
+{
+	paramiocresetgroup_t data = {PARAM_RESET_ALL, nullptr, 0};
+	boardctl(PARAMIOCRESETGROUP, reinterpret_cast<unsigned long>(&data));
+}
+
+void
+param_reset_excludes(const char *excludes[], int num_excludes)
+{
+	paramiocresetgroup_t data = {PARAM_RESET_EXCLUDES, excludes, num_excludes};
+	boardctl(PARAMIOCRESETGROUP, reinterpret_cast<unsigned long>(&data));
+}
+
+void
+param_reset_specific(const char *resets[], int num_resets)
+{
+	paramiocresetgroup_t data = {PARAM_RESET_SPECIFIC, resets, num_resets};
+	boardctl(PARAMIOCRESETGROUP, reinterpret_cast<unsigned long>(&data));
+}
+
+int param_save_default()
+{
+	paramiocsavedefault_t data = {PX4_ERROR};
+	boardctl(PARAMIOCSAVEDEFAULT, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int
+param_load_default()
+{
+	paramiocloaddefault_t data = {PX4_ERROR};
+	boardctl(PARAMIOCLOADDEFAULT, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+int
+param_export(const char *filename, param_filter_func filter)
+{
+	paramiocexport_t data = {filename, PX4_ERROR};
+
+	if (filter) { PX4_ERR("ERROR: filter not supported in userside blob"); }
+
+	boardctl(PARAMIOCEXPORT, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}
+
+uint32_t param_hash_check()
+{
+	paramiochash_t data = {0};
+	boardctl(PARAMIOCHASH, reinterpret_cast<unsigned long>(&data));
+	return data.ret;
+}

src/lib/tecs/TECS.hpp

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
@@ -49,8 +50,513 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
+class TECSAirspeedFilter
+{
+public:
+	/**
+	 * @brief State of the equivalent airspeed filter.
+	 *
+	 */
+	struct AirspeedFilterState {
+		float speed;		///< speed of the air in EAS [m/s]
+		float speed_rate;	///< rate of speed of the air [m/s²]
+	};
+
+	/**
+	 * @brief Parameters of the airspeed filter.
+	 *
+	 */
+	struct Param {
+		float equivalent_airspeed_trim; 	///< the trim value of the equivalent airspeed [m/s].
+		float airspeed_measurement_std_dev; 	///< airspeed measurement standard deviation in [m/s].
+		float airspeed_rate_measurement_std_dev;///< airspeed rate measurement standard deviation in [m/s²].
+		float airspeed_rate_noise_std_dev; 	///< standard deviation on the airspeed rate deviation in the model in [m/s²].
+	};
+
+	/**
+	 * @brief Input, which will be filtered.
+	 *
+	 */
+	struct Input {
+		float equivalent_airspeed; 	///< the measured equivalent airspeed in [m/s].
+		float equivalent_airspeed_rate;	///< the measured rate of equivalent airspeed in [m/s²].
+	};
+public:
+	TECSAirspeedFilter() = default;
+	~TECSAirspeedFilter() = default;
+	/**
+	 * @brief Initialize filter
+	 *
+	 * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s].
+	 */
+	void initialize(float equivalent_airspeed);
+
+	/**
+	 * @brief Update filter
+	 *
+	 * @param[in] dt is the timestep in [s].
+	 * @param[in] input are the raw measured values.
+	 * @param[in] param are the filter parameters.
+	 * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available.
+	 */
+	void update(float dt, const Input &input, const Param &param, const bool airspeed_sensor_available);
+
+	/**
+	 * @brief Get the filtered airspeed states.
+	 *
+	 * @return Current state of the airspeed filter.
+	 */
+	AirspeedFilterState getState() const;
+
+private:
+	// States
+	AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f};	///< Complimentary filter state
+};
+
+class TECSReferenceModel
+{
+public:
+	/**
+	 * @brief Altitude reference state.
+	 *
+	 */
+	struct AltitudeReferenceState {
+		float alt; 	///< Reference altitude amsl in [m].
+		float alt_rate;	///< Reference altitude rate in [m/s].
+	};
+
+	/**
+	 * @brief Parameters for the reference model.
+	 *
+	 */
+	struct Param {
+		float target_climbrate;	///< The target climbrate in [m/s].
+		float target_sinkrate;	///< The target sinkrate in [m/s].
+		float jerk_max;		///< Magnitude of the maximum jerk allowed [m/s³].
+		float vert_accel_limit;	///< Magnitude of the maximum vertical acceleration allowed [m/s²].
+		float max_climb_rate;	///< Climb rate produced by max allowed throttle [m/s].
+		float max_sink_rate;	///< Maximum safe sink rate [m/s].
+	};
+
+public:
+	TECSReferenceModel() = default;
+	~TECSReferenceModel() = default;
+
+	/**
+	 * @brief Initialize reference models.
+	 *
+	 * @param[in] state is the current altitude state of the vehicle.
+	 */
+	void initialize(const AltitudeReferenceState &state);
+
+	/**
+	 * @brief Update reference models.
+	 *
+	 * @param[in] dt is the update interval in [s].
+	 * @param[in] setpoint are the desired setpoints.
+	 * @param[in] altitude is the altitude amsl in [m].
+	 * @param[in] param are the reference model parameters.
+	 */
+	void update(float dt, const AltitudeReferenceState &setpoint, float altitude, const Param &param);
+
+	/**
+	 * @brief Get the current altitude reference of altitude reference model.
+	 *
+	 * @return Altitude reference state.
+	 */
+	AltitudeReferenceState getAltitudeReference() const;
+
+	/**
+	 * @brief Get the altitude rate reference of the altitude rate reference model.
+	 *
+	 * @return Current altitude rate reference point.
+	 */
+	float getAltitudeRateReference() const;
+
+private:
+	// State
+	VelocitySmoothing
+	_alt_control_traj_generator;		///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
+	float _alt_rate_ref; 			///< Altitude rate reference in [m/s].
+};
+
+class TECSControl
+{
+public:
+	/**
+	 * @brief The control parameters.
+	 *
+	 */
+	struct Param {
+		// Vehicle specific params
+		float max_sink_rate;			///< Maximum safe sink rate [m/s].
+		float max_climb_rate;			///< Climb rate produced by max allowed throttle [m/s].
+		float vert_accel_limit;			///< Magnitude of the maximum vertical acceleration allowed [m/s²].
+		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
+		float tas_min;				///< True airpeed demand lower limit [m/s].
+		float pitch_max;			///< Maximum pitch angle allowed in [rad].
+		float pitch_min;			///< Minimal pitch angle allowed in [rad].
+		float throttle_trim;			///< Normalized throttle required to fly level at given eas.
+		float throttle_max;			///< Normalized throttle upper limit.
+		float throttle_min;			///< Normalized throttle lower limit.
+
+		// Altitude control param
+		float altitude_error_gain;		///< Altitude error inverse time constant [1/s].
+		float altitude_setpoint_gain_ff;	///< Gain from altitude demand derivative to demanded climb rate.
+
+		// Airspeed control param
+		/// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
+		float tas_error_percentage;
+		float airspeed_error_gain;				///< Airspeed error inverse time constant [1/s].
+
+		// Energy control param
+		float ste_rate_time_const;				///< Filter time constant for specific total energy rate (damping path) [s].
+		float seb_rate_ff;					///< Specific energy balance rate feedforward gain.
+
+		// Pitch control param
+		float pitch_speed_weight;				///< Speed control weighting used by pitch demand calculation.
+		float integrator_gain_pitch;				///< Integrator gain used by the pitch demand calculation.
+		float pitch_damping_gain;				///< Damping gain of the pitch demand calculation [s].
+
+		// Throttle control param
+		float integrator_gain_throttle;				///< Integrator gain used by the throttle demand calculation.
+		float throttle_damping_gain;				///< Damping gain of the throttle demand calculation [s].
+		float throttle_slewrate;				///< Throttle demand slew rate limit [1/s].
+
+		float load_factor_correction;				///< Gain from normal load factor increase to total energy rate demand [m²/s³].
+		float load_factor;					///< Additional normal load factor.
+	};
+
+	/**
+	 * @brief The debug output
+	 *
+	 */
+	struct DebugOutput {
+		float altitude_rate_control;			///< Altitude rate setpoint from altitude control loop [m/s].
+		float true_airspeed_derivative_control;		///< Airspeed rate setpoint from airspeed control loop [m/s²].
+		float total_energy_rate_estimate;		///< Total energy rate estimate [m²/s³].
+		float total_energy_rate_sp;			///< Total energy rate setpoint [m²/s³].
+		float energy_balance_rate_estimate;		///< Energy balance rate estimate [m²/s³].
+		float energy_balance_rate_sp;			///< Energy balance rate setpoint [m²/s³].
+		float pitch_integrator;				///< Pitch control integrator state [-].
+		float throttle_integrator;			///< Throttle control integrator state [-].
+	};
+
+	/**
+	 * @brief Given setpoint to control.
+	 *
+	 */
+	struct Setpoint {
+		TECSReferenceModel::AltitudeReferenceState altitude_reference;	///< Altitude reference from reference model.
+		float altitude_rate_setpoint;					///< Altitude rate setpoint.
+		float tas_setpoint;						///< True airspeed setpoint.
+	};
+
+	/**
+	 * @brief Givent current measurement from the UAS.
+	 *
+	 */
+	struct Input {
+		float altitude;		///< Current altitude of the UAS [m].
+		float altitude_rate;	///< Current altitude rate of the UAS [m/s].
+		float tas;		///< Current true airspeed of the UAS [m/s].
+		float tas_rate;		///< Current true airspeed rate of the UAS [m/s²].
+	};
+
+	/**
+	 * @brief Control flags.
+	 *
+	 */
+	struct Flag {
+		bool airspeed_enabled;			///< Flag if the airspeed sensor is enabled.
+		bool climbout_mode_active;		///< Flag if climbout mode is activated.
+		bool detect_underspeed_enabled;		///< Flag if underspeed detection is enabled.
+	};
+public:
+	TECSControl() = default;
+	~TECSControl() = default;
+	/**
+	 * @brief Initialization of the state.
+	 *
+	 */
+	void initialize(const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag);
+	/**
+	 * @brief Update state and output.
+	 *
+	 * @param[in] dt is the update time intervall in [s].
+	 * @param[in] setpoint is the current setpoint struct.
+	 * @param[in] input is the current input measurements.
+	 * @param[in] param is the current parameter set.
+	 * @param[in] flag is the current activated flags.
+	 */
+	void update(float dt, const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag);
+	/**
+	 * @brief Reset the control loop integrals.
+	 *
+	 */
+	void resetIntegrals();
+	/**
+	 * @brief Get the percent of the undersped.
+	 *
+	 * @return Ratio of detected undersped [0,1].
+	 */
+	float getRatioUndersped() const {return _ratio_undersped;};
+	/**
+	 * @brief Get the throttle setpoint.
+	 *
+	 * @return throttle setpoint.
+	 */
+	float getThrottleSetpoint() const {return _throttle_setpoint;};
+	/**
+	 * @brief Get the pitch setpoint.
+	 *
+	 * @return THe commanded pitch angle in [rad].
+	 */
+	float getPitchSetpoint() const {return _pitch_setpoint;};
+	/**
+	 * @brief Get specific total energy rate.
+	 *
+	 * @return the total specific energy rate in [m²/s³].
+	 */
+	float getSteRate() const {return _ste_rate;};
+	/**
+	 * @brief Get the Debug Output
+	 *
+	 * @return the debug outpus struct.
+	 */
+	const DebugOutput &getDebugOutput() const { return _debug_output; }
+
+private:
+	/**
+	 * @brief Specific total energy rate limit.
+	 *
+	 */
+	struct STERateLimit {
+		float STE_rate_max;	///< Maximum specific total energy rate limit [m²/s³].
+		float STE_rate_min;	///< Minimal specific total energy rate limit [m²/s³].
+	};
+
+	/**
+	 * @brief Control values.
+	 * setpoint and current state estimate value as input to a controller.
+	 *
+	 */
+	struct ControlValues {
+		float setpoint;		///< Control setpoint.
+		float estimate;		///< Control estimate of current state value.
+	};
+
+	/**
+	 * @brief Calculated specific energy rates.
+	 *
+	 */
+	struct SpecificEnergyRates {
+		ControlValues ske_rate;	///< Specific kinetic energy rate [m²/s³].
+		ControlValues spe_rate;	///< Specific potential energy rate [m²/s³].
+	};
+
+	/**
+	 * @brief Controlled altitude and pitch setpoints.
+	 *
+	 */
+	struct AltitudePitchControl {
+		float altitude_rate_setpoint;	///< Controlled altitude rate setpoint [m/s].
+		float tas_rate_setpoint;	///< Controlled true airspeed rate setpoint [m/s²].
+	};
+
+	/**
+	 * @brief Weight factors for specific energy.
+	 *
+	 */
+	struct SpecificEnergyWeighting {
+		float spe_weighting;	///< Specific potential energy weight.
+		float ske_weighting;	///< Specific kinetic energy weight.
+	};
+
+private:
+	/**
+	 * @brief Get control error from etpoint and estimate
+	 *
+	 * @param val is the current control setpoint and estimate.
+	 * @return error value
+	 */
+	static inline constexpr float _getControlError(TECSControl::ControlValues val) {return (val.setpoint - val.estimate);};
+	/**
+	 * @brief Calculate specific total energy rate limits.
+	 *
+	 * @param[in] param are the control parametes.
+	 * @return Specific total energy rate limits in [m²/s³].
+	 */
+	STERateLimit _calculateTotalEnergyRateLimit(const Param &param) const;
+	/**
+	 * @brief calculate airspeed control proportional output.
+	 *
+	 * @param setpoint is the control setpoints.
+	 * @param input	is the current input measurment of the UAS.
+	 * @param param	is the control parameters.
+	 * @param flag	is the control flags.
+	 * @return controlled airspeed rate setpoint in [m/s²].
+	 */
+	float _calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param &param,
+					 const Flag &flag) const;
+	/**
+	 * @brief calculate altitude control proportional output.
+	 *
+	 * @param setpoint is the control setpoints.
+	 * @param input is the current input measurment of the UAS.
+	 * @param param is the control parameters.
+	 * @return controlled altitude rate setpoint in [m/s].
+	 */
+	float _calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param &param) const;
+	/**
+	 * @brief Calculate specific energy rates.
+	 *
+	 * @param control_setpoint is the controlles altitude and airspeed rate setpoints.
+	 * @param input is the current input measurment of the UAS.
+	 * @return Specific energy rates in [m²/s³].
+	 */
+	SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
+	/**
+	 * @brief Detect underspeed.
+	 *
+	 * @param input is the current input measurment of the UAS.
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 */
+	void _detectUnderspeed(const Input &input, const Param &param, const Flag &flag);
+	/**
+	 * @brief Update specific energy balance weights.
+	 *
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 * @return Weights used for the specific energy balance.
+	 */
+	SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param &param, const Flag &flag);
+	/**
+	 * @brief Calculate pitch control.
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param input is the current input measurement of the UAS.
+	 * @param specific_energy_rate is the calculated specific energy.
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 */
+	void _calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rate,
+			       const Param &param,
+			       const Flag &flag);
+
+	/**
+	 * @brief Calculate pitch control specific energy balance rates.
+	 *
+	 * @param weight is the weighting use of the potential and kinetic energy.
+	 * @param specific_energy_rate is the specific energy rates in [m²/s³].
+	 * @return specific energy balance rate values in [m²/s³].
+	 */
+	ControlValues _calcPitchControlSebRate(const SpecificEnergyWeighting &weight,
+					       const SpecificEnergyRates &specific_energy_rate) const;
+
+	/**
+	 * @brief Calculate the pitch control update function.
+	 * Update the states of the pitch control
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param seb_rate is the specific energy balance rate in [m²/s³].
+	 * @param param is the control parameters.
+	 */
+	void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param &param);
+
+	/**
+	 * @brief Calculate the pitch control output function.
+	 *
+	 * @param input is the current input measurement of the UAS.
+	 * @param seb_rate is the specific energy balance rate in [m²/s³].
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 * @return pitch setpoint angle [rad].
+	 */
+	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
+				      const Flag &flag) const;
+
+	/**
+	 * @brief Update controlled throttle setpoint.
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param specific_energy_rate is the calculated specific energy.
+	 * @param flag is the control flags.
+	 */
+	void _calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rate, const Param &param,
+				  const Flag &flag);
+
+	/**
+	 * @brief Calculate throttle control specific total energy
+	 *
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param specific_energy_rate is the specific energy rates in [m²/s³].
+	 * @param param is the control parameters.
+	 * @return specific total energy rate values in [m²/s³]
+	 */
+	ControlValues _calcThrottleControlSteRate(const STERateLimit &limit, const SpecificEnergyRates &specific_energy_rate,
+			const Param &param) const;
+
+	/**
+	 * @brief Calculate the throttle control update function.
+	 * Update the throttle control states.
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param ste_rate is the specific total energy rates in [m²/s³].
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 */
+	void _calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate, const Param &param,
+					const Flag &flag);
+
+	/**
+	 * @brief Calculate the throttle control output function.
+	 *
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param ste_rate is the specific total energy rates in [m²/s³].
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 * @return throttle setpoin in [0,1].
+	 */
+	float _calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate, const Param &param,
+					 const Flag &flag) const;
+
+private:
+	// State
+	AlphaFilter<float> _ste_rate_estimate_filter;		///< Low pass filter for the specific total energy rate.
+	float _pitch_integ_state{0.0f};				///< Pitch integrator state [rad].
+	float _throttle_integ_state{0.0f};			///< Throttle integrator state.
+
+
+	// Output
+	DebugOutput _debug_output;				///< Debug output.
+	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
+	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint [0,1].
+	float _ratio_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
+	float _ste_rate{0.0f};					///< Specific total energy rate [m²/s³].
+};
+
 class TECS
 {
+public:
+	enum ECL_TECS_MODE {
+		ECL_TECS_MODE_NORMAL = 0,
+		ECL_TECS_MODE_UNDERSPEED,
+		ECL_TECS_MODE_BAD_DESCENT,
+		ECL_TECS_MODE_CLIMBOUT
+	};
+
+	struct DebugOutput {
+		TECSControl::DebugOutput control;
+		float true_airspeed_filtered;
+		float true_airspeed_derivative;
+		float altitude_sp;
+		float altitude_rate_alt_ref;
+		float altitude_rate_feedforward;
+		enum ECL_TECS_MODE tecs_mode;
+	};
 public:
 	TECS() = default;
 	~TECS() = default;
@@ -61,146 +567,75 @@ public:
 	TECS(TECS &&) = delete;
 	TECS &operator=(TECS &&) = delete;
 
+	const DebugOutput &getStatus() const { return _debug_status; }
+
 	/**
 	 * Get the current airspeed status
 	 *
 	 * @return true if airspeed is enabled for control
 	 */
-	bool airspeed_sensor_enabled() { return _airspeed_enabled; }
+	bool airspeed_sensor_enabled() { return _control_flag.airspeed_enabled; }
 
 	/**
 	 * Set the airspeed enable state
 	 */
-	void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; }
+	void enable_airspeed(bool enabled) { _control_flag.airspeed_enabled = enabled; }
 
 	/**
-	 * Updates the following vehicle kineamtic state estimates:
-	 * Vertical position, velocity and acceleration.
-	 * Speed derivative
-	 * Must be called prior to udating tecs control loops
-	 * Must be called at 50Hz or greater
+	 * @brief Update the control loop calculations
+	 *
 	 */
-	void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock,
-					    float altitude, float vz);
+	void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
+		    float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
+		    float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
+		    float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
 
 	/**
-	 * Update the control loop calculations
+	 * @brief Initialize the control loop
+	 *
 	 */
-	void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
-				   float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
-				   float throttle_min, float throttle_setpoint_max, float throttle_trim,
-				   float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);
-
-	float get_throttle_setpoint() { return _last_throttle_setpoint; }
-	float get_pitch_setpoint() { return _last_pitch_setpoint; }
-	float get_speed_weight() { return _pitch_speed_weight; }
-	float get_throttle_trim() { return _throttle_trim; }
-
-	void reset_state() { _states_initialized = false; }
+	void initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas);
 
 	void resetIntegrals()
 	{
-		_throttle_integ_state =  0.0f;
-		_pitch_integ_state = 0.0f;
+		_control.resetIntegrals();
 	}
 
-	/**
-	 * @brief Resets the altitude and height rate control trajectory generators to the input altitude
-	 *
-	 * @param altitude Vehicle altitude (AMSL) [m]
-	 */
-	void resetTrajectoryGenerators(const float altitude)
-	{
-		_alt_control_traj_generator.reset(0, 0, altitude);
-		_velocity_control_traj_generator.reset(0.0f, 0.0f, altitude);
-	}
+	void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; };
 
-	enum ECL_TECS_MODE {
-		ECL_TECS_MODE_NORMAL = 0,
-		ECL_TECS_MODE_UNDERSPEED,
-		ECL_TECS_MODE_BAD_DESCENT,
-		ECL_TECS_MODE_CLIMBOUT
-	};
+	// // setters for parameters
+	void set_airspeed_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_measurement_std_dev = std_dev;};
+	void set_airspeed_rate_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_measurement_std_dev = std_dev;};
+	void set_airspeed_filter_process_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_noise_std_dev = std_dev;};
 
-	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
+	void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;};
+	void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; };
 
-	// setters for controller parameters
-	void set_integrator_gain_throttle(float gain) { _integrator_gain_throttle = gain; }
-	void set_integrator_gain_pitch(float gain) { _integrator_gain_pitch = gain; }
+	void set_max_sink_rate(float sink_rate) { _control_param.max_sink_rate = sink_rate; _reference_param.max_sink_rate = sink_rate; };
+	void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; };
 
-	void set_min_sink_rate(float rate) { _min_sink_rate = rate; }
-	void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; }
-	void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; }
-
-	void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
-	void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); }
+	void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
+	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; };
 
 	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
-	void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; }
+	void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }
 
-	void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
-	void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; }
+	void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
+	void set_vertical_accel_limit(float limit) { _reference_param.vert_accel_limit = limit; _control_param.vert_accel_limit = limit; };
 
-	void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; }
-	void set_speed_weight(float weight) { _pitch_speed_weight = weight; }
-	void set_airspeed_error_time_constant(float time_const) { _airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); }
+	void set_speed_weight(float weight) { _control_param.pitch_speed_weight = weight; };
+	void set_airspeed_error_time_constant(float time_const) { _control_param.airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); };
 
-	void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; }
-	void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; }
+	void set_throttle_damp(float throttle_damp) { _control_param.throttle_damping_gain = throttle_damp; };
+	void set_throttle_slewrate(float slewrate) { _control_param.throttle_slewrate = slewrate; };
 
-	void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; }
-	void set_load_factor(float load_factor) { _load_factor = load_factor; }
+	void set_roll_throttle_compensation(float compensation) { _control_param.load_factor_correction = compensation; };
+	void set_load_factor(float load_factor) { _control_param.load_factor = load_factor; };
 
-	void set_ste_rate_time_const(float time_const) { _STE_rate_time_const = time_const; }
-	void set_speed_derivative_time_constant(float time_const) { _speed_derivative_time_const = time_const; }
-
-	void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
-
-	// TECS status
-	uint64_t timestamp() { return _pitch_update_timestamp; }
-	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
-
-	float hgt_setpoint() { return _hgt_setpoint; }
-	float vert_pos_state() { return _vert_pos_state; }
-
-	float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
-	float tas_state() { return _tas_state; }
-	float getTASInnovation() { return _tas_innov; }
-
-	float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
-	float vert_vel_state() { return _vert_vel_state; }
-
-	float get_EAS_setpoint() { return _EAS_setpoint; };
-	float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
-	float speed_derivative() { return _tas_rate_filtered; }
-	float speed_derivative_raw() { return _tas_rate_raw; }
-
-	float STE_error() { return _STE_error; }
-	float STE_rate_error() { return _STE_rate_error; }
-
-	float SEB_error() { return _SEB_error; }
-	float SEB_rate_error() { return _SEB_rate_error; }
-
-	float throttle_integ_state() { return _throttle_integ_state; }
-	float pitch_integ_state() { return _pitch_integ_state; }
-
-	float STE() { return _SPE_estimate + _SKE_estimate; }
-
-	float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
-
-	float STE_rate() { return _SPE_rate + _SKE_rate; }
-
-	float STE_rate_setpoint() { return _STE_rate_setpoint; }
-
-	float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
-
-	float SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }
-
-	float SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
-
-	float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
+	void set_ste_rate_time_const(float time_const) { _control_param.ste_rate_time_const = time_const; };
 
+	void set_seb_rate_ff_gain(float ff_gain) { _control_param.seb_rate_ff = ff_gain; };
 
 	/**
 	 * Handle the altitude reset
@@ -208,192 +643,102 @@ public:
 	 * If the estimation system resets the height in one discrete step this
 	 * will gracefully even out the reset over time.
 	 */
-	void handle_alt_step(float delta_alt, float altitude)
+	void handle_alt_step(float altitude, float altitude_rate)
 	{
-		_hgt_setpoint += delta_alt;
+		TECSReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
+				.alt_rate = altitude_rate};
 
-		// reset height states
-		_vert_pos_state = altitude;
-		_vert_vel_state = 0.0f;
+		// reset altitude reference model.
+		_reference_model.initialize(init_state);
 	}
 
+	float get_pitch_setpoint() {return _control.getPitchSetpoint();}
+	float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
+
+	// // TECS status
+	uint64_t timestamp() { return _update_timestamp; }
+	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
+
 private:
+	TECSControl 		_control;				///< Control submodule.
+	TECSAirspeedFilter 	_airspeed_filter;			///< Airspeed filter submodule.
+	TECSReferenceModel 	_reference_model;			///< Setpoint reference model submodule.
 
-	// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
-	static constexpr float kTASErrorPercentage = 0.15;
+	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};		///< Current activated mode.
 
-	static constexpr float _jerk_max =
-		1000.0f;
+	hrt_abstime _update_timestamp{0};				///< last timestamp of the update function call.
 
-	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};
-
-	// timestamps
-	uint64_t _state_update_timestamp{0};				///< last timestamp of the 50 Hz function call
-	uint64_t _speed_update_timestamp{0};				///< last timestamp of the speed function call
-	uint64_t _pitch_update_timestamp{0};				///< last timestamp of the pitch function call
-
-	// controller parameters
-	float _tas_estimate_freq{0.0f};					///< cross-over frequency of the true airspeed complementary filter (rad/sec)
-	float _max_climb_rate{2.0f};					///< climb rate produced by max allowed throttle (m/sec)
-	float _min_sink_rate{1.0f};					///< sink rate produced by min allowed throttle (m/sec)
-	float _max_sink_rate{2.0f};					///< maximum safe sink rate (m/sec)
-	float _pitch_damping_gain{0.0f};				///< damping gain of the pitch demand calculation (sec)
-	float _throttle_damping_gain{0.0f};				///< damping gain of the throttle demand calculation (sec)
-	float _integrator_gain_throttle{0.0f};				///< integrator gain used by the throttle demand calculation
-	float _integrator_gain_pitch{0.0f};				///< integrator gain used by the pitch demand calculation
-	float _vert_accel_limit{0.0f};					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
-	float _load_factor{1.0f};					///< additional normal load factor
-	float _load_factor_correction{0.0f};				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
-	float _pitch_speed_weight{1.0f};				///< speed control weighting used by pitch demand calculation
-	float _height_error_gain{0.2f};					///< height error inverse time constant [1/s]
-	float _height_setpoint_gain_ff{0.0f};				///< gain from height demand derivative to demanded climb rate
-	float _airspeed_error_gain{0.1f};				///< airspeed error inverse time constant [1/s]
 	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
-	float _equivalent_airspeed_trim{15.0f};			///< equivalent cruise airspeed for airspeed less mode (m/sec)
-	float _throttle_slewrate{0.0f};					///< throttle demand slew rate limit (1/sec)
-	float _STE_rate_time_const{0.1f};				///< filter time constant for specific total energy rate (damping path) (s)
-	float _speed_derivative_time_const{0.01f};			///< speed derivative filter time constant (s)
-	float _SEB_rate_ff{1.0f};
-
-	// complimentary filter states
-	float _vert_vel_state{0.0f};					///< complimentary filter state - height rate (m/sec)
-	float _vert_pos_state{0.0f};					///< complimentary filter state - height (m)
-	float _tas_rate_state{0.0f};					///< complimentary filter state - true airspeed first derivative (m/sec**2)
-	float _tas_state{0.0f};						///< complimentary filter state - true airspeed (m/sec)
-	float _tas_innov{0.0f};						///< complimentary filter true airspeed innovation (m/sec)
-
-	// controller states
-	float _throttle_integ_state{0.0f};				///< throttle integrator state
-	float _pitch_integ_state{0.0f};					///< pitch integrator state (rad)
-	float _last_throttle_setpoint{0.0f};				///< throttle demand rate limiter state (1/sec)
-	float _last_pitch_setpoint{0.0f};				///< pitch demand rate limiter state (rad/sec)
-	float _tas_rate_filtered{0.0f};					///< low pass filtered rate of change of speed along X axis (m/sec**2)
-
-	// speed demand calculations
-	float _EAS{0.0f};						///< equivalent airspeed (m/sec)
-	float _TAS_max{30.0f};						///< true airpeed demand upper limit (m/sec)
-	float _TAS_min{3.0f};						///< true airpeed demand lower limit (m/sec)
-	float _TAS_setpoint{0.0f};					///< current airpeed demand (m/sec)
-	float _TAS_setpoint_last{0.0f};					///< previous true airpeed demand (m/sec)
-	float _EAS_setpoint{0.0f};					///< Equivalent airspeed demand (m/sec)
-	float _TAS_setpoint_adj{0.0f};					///< true airspeed demand tracked by the TECS algorithm (m/sec)
-	float _TAS_rate_setpoint{0.0f};					///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2)
-	float _tas_rate_raw{0.0f};					///< true airspeed rate, calculated as inertial acceleration in body X direction
-
-	// height demand calculations
-	float _hgt_setpoint{0.0f};					///< demanded height tracked by the TECS algorithm (m)
-	float _hgt_rate_setpoint{0.0f};					///< demanded climb rate tracked by the TECS algorithm
-
-	// vehicle physical limits
-	float _pitch_setpoint_unc{0.0f};				///< pitch demand before limiting (rad)
-	float _STE_rate_max{FLT_EPSILON};				///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
-	float _STE_rate_min{-FLT_EPSILON};				///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
-	float _throttle_setpoint_max{0.0f};				///< normalised throttle upper limit
-	float _throttle_setpoint_min{0.0f};				///< normalised throttle lower limit
-	float _throttle_trim{0.0f};					///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight
-	float _pitch_setpoint_max{0.5f};				///< pitch demand upper limit (rad)
-	float _pitch_setpoint_min{-0.5f};				///< pitch demand lower limit (rad)
-
-	// specific energy quantities
-	float _SPE_setpoint{0.0f};					///< specific potential energy demand (m**2/sec**2)
-	float _SKE_setpoint{0.0f};					///< specific kinetic energy demand (m**2/sec**2)
-	float _SPE_rate_setpoint{0.0f};					///< specific potential energy rate demand (m**2/sec**3)
-	float _SKE_rate_setpoint{0.0f};					///< specific kinetic energy rate demand (m**2/sec**3)
-	float _STE_rate_setpoint{0.0f};					///< specific total energy rate demand (m**s/sec**3)
-	float _SPE_estimate{0.0f};					///< specific potential energy estimate (m**2/sec**2)
-	float _SKE_estimate{0.0f};					///< specific kinetic energy estimate (m**2/sec**2)
-	float _SPE_rate{0.0f};						///< specific potential energy rate estimate (m**2/sec**3)
-	float _SKE_rate{0.0f};						///< specific kinetic energy rate estimate (m**2/sec**3)
-
-	// specific energy error quantities
-	float _STE_error{0.0f};						///< specific total energy error (m**2/sec**2)
-	float _STE_rate_error{0.0f};					///< specific total energy rate error (m**2/sec**3)
-	float _SEB_error{0.0f};						///< specific energy balance error (m**2/sec**2)
-	float _SEB_rate_error{0.0f};					///< specific energy balance rate error (m**2/sec**3)
-
-	// speed height weighting
-	float _SPE_weighting{1.0f};
-	float _SKE_weighting{1.0f};
-
-	// time steps (non-fixed)
-	float _dt{DT_DEFAULT};						///< Time since last update of main TECS loop (sec)
-	static constexpr float DT_DEFAULT = 0.02f;			///< default value for _dt (sec)
 
 	// controller mode logic
-	float _percent_undersped{0.0f};					///< a continuous representation of how "undersped" the TAS is [0,1]
-	bool _detect_underspeed_enabled{true};				///< true when underspeed detection is enabled
 	bool _uncommanded_descent_recovery{false};			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
-	bool _climbout_mode_active{false};				///< true when in climbout mode
-	bool _airspeed_enabled{false};					///< true when airspeed use has been enabled
-	bool _states_initialized{false};				///< true when TECS states have been iniitalized
 
-	/**
-	 * Update the airspeed internal state using a second order complementary filter
-	 */
-	void _update_speed_states(float airspeed_setpoint, float equivalent_airspeed, float cas_to_tas);
+	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
+	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
+
+	DebugOutput _debug_status{};
+
+	// Params
+	/// Airspeed filter parameters.
+	TECSAirspeedFilter::Param _airspeed_filter_param{
+		.equivalent_airspeed_trim = 15.0f,
+		.airspeed_measurement_std_dev = 0.2f,
+		.airspeed_rate_measurement_std_dev = 0.05f,
+		.airspeed_rate_noise_std_dev = 0.02f
+	};
+	/// Reference model parameters.
+	TECSReferenceModel::Param _reference_param{
+		.target_climbrate = 2.0f,
+		.target_sinkrate = 2.0f,
+		.jerk_max = 1000.0f,
+		.vert_accel_limit = 0.0f,
+		.max_climb_rate = 2.0f,
+		.max_sink_rate = 2.0f,
+	};
+	/// Control parameters.
+	TECSControl::Param _control_param{
+		.max_sink_rate = 2.0f,
+		.max_climb_rate = 2.0f,
+		.vert_accel_limit = 0.0f,
+		.equivalent_airspeed_trim = 15.0f,
+		.tas_min = 3.0f,
+		.pitch_max = 5.0f,
+		.pitch_min = -5.0f,
+		.throttle_trim = 0.0f,
+		.throttle_max = 1.0f,
+		.throttle_min = 0.1f,
+		.altitude_error_gain = 0.2f,
+		.altitude_setpoint_gain_ff = 0.0f,
+		.tas_error_percentage = 0.15f,
+		.airspeed_error_gain = 0.1f,
+		.ste_rate_time_const = 0.1f,
+		.seb_rate_ff = 1.0f,
+		.pitch_speed_weight = 1.0f,
+		.integrator_gain_pitch = 0.0f,
+		.pitch_damping_gain = 0.0f,
+		.integrator_gain_throttle = 0.0f,
+		.throttle_damping_gain = 0.0f,
+		.throttle_slewrate = 0.0f,
+		.load_factor_correction = 0.0f,
+		.load_factor = 1.0f,
+	};
+
+	TECSControl::Flag _control_flag{
+		.airspeed_enabled = false,
+		.climbout_mode_active = false,
+		.detect_underspeed_enabled = false,
+	};
 
 	/**
 	 * Update the desired airspeed
 	 */
-	void _update_speed_setpoint();
-
-	/**
-	 * Calculate desired height rate from altitude demand
-	 */
-	void runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
-			float alt_amsl);
-
-	/**
-	 * Detect how undersped the aircraft is
-	 */
-	void _detect_underspeed();
-
-	/**
-	 * Update specific energy
-	 */
-	void _update_energy_estimates();
-
-	/**
-	 * Update throttle setpoint
-	 */
-	void _update_throttle_setpoint();
+	float _update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas);
 
 	/**
 	 * Detect an uncommanded descent
 	 */
-	void _detect_uncommanded_descent();
-
-	/**
-	 * Update the pitch setpoint
-	 */
-	void _update_pitch_setpoint();
-
-	void _updateTrajectoryGenerationConstraints();
-
-	void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
-					  float target_sinkrate, float altitude_amsl);
-
-	/**
-	 * Initialize the controller
-	 */
-	void _initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
-				float eas_to_tas);
-
-	/**
-	 * Calculate specific total energy rate limits
-	 */
-	void _update_STE_rate_lim();
-
-	void _update_speed_height_weights();
-
-	AlphaFilter<float> _STE_rate_error_filter;
-
-	AlphaFilter<float> _TAS_rate_filter;
-
-	VelocitySmoothing
-	_alt_control_traj_generator;	// generates height rate and altitude setpoint trajectory when altitude is commanded
-	ManualVelocitySmoothingZ
-	_velocity_control_traj_generator;	// generates height rate and altitude setpoint trajectory when height rate is commanded
-
+	void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas,
+					 float tas_setpoint);
 };
+

src/lib/tinybson/tinybson.cpp

@@ -115,11 +115,10 @@ read_double(bson_decoder_t decoder, double *d)
 }
 
 int
-bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *priv)
+bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback)
 {
 	decoder->fd = fd;
 	decoder->callback = callback;
-	decoder->priv = priv;
 	decoder->nesting = 1;
 	decoder->node.type = BSON_UNDEFINED;
 
@@ -135,7 +134,7 @@ bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback cal
 }
 
 int
-bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *priv)
+bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback)
 {
 	/* argument sanity */
 	if ((buf == nullptr) || (callback == nullptr)) {
@@ -156,7 +155,6 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_
 
 	decoder->bufpos = 0;
 	decoder->callback = callback;
-	decoder->priv = priv;
 	decoder->nesting = 1;
 	decoder->pending = 0;
 	decoder->node.type = BSON_UNDEFINED;
@@ -317,7 +315,7 @@ bson_decoder_next(bson_decoder_t decoder)
 	}
 
 	/* call the callback and pass its results back */
-	return decoder->callback(decoder, decoder->priv, &decoder->node);
+	return decoder->callback(decoder, &decoder->node);
 }
 
 int

src/lib/tinybson/tinybson.h

@@ -101,7 +101,7 @@ typedef struct bson_decoder_s *bson_decoder_t;
  *
  * The node callback function's return value is returned by bson_decoder_next.
  */
-typedef int	(* bson_decoder_callback)(bson_decoder_t decoder, void *priv, bson_node_t node);
+typedef int	(* bson_decoder_callback)(bson_decoder_t decoder, bson_node_t node);
 
 struct bson_decoder_s {
 	/* file reader state */
@@ -114,7 +114,6 @@ struct bson_decoder_s {
 
 	bool			dead{false};
 	bson_decoder_callback	callback;
-	void			*priv{nullptr};
 	unsigned		nesting{0};
 	struct bson_node_s	node {};
 	int32_t			pending{0};
@@ -136,10 +135,9 @@ struct bson_decoder_s {
  * @param decoder		Decoder state structure to be initialised.
  * @param fd			File to read BSON data from.
  * @param callback		Callback to be invoked by bson_decoder_next
- * @param priv                 Callback private data, stored in node.
  * @return			Zero on success.
  */
-__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *priv);
+__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback);
 
 /**
  * Initialise the decoder to read from a buffer in memory.
@@ -150,11 +148,9 @@ __EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder
  *				passed as zero if the buffer size should be extracted from the
  *				BSON header only.
  * @param callback		Callback to be invoked by bson_decoder_next
- * @param priv		        Callback private data, stored in node.
  * @return			Zero on success.
  */
-__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback,
-				   void *priv);
+__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback);
 
 /**
  * Process the next node from the stream and invoke the callback.

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -397,32 +397,32 @@ AirspeedModule::Run()
 
 			}
 
-			// save estimated airspeed scale after disarm
+			// save estimated airspeed scale after disarm if airspeed is valid and scale has changed
 			if (!armed && _armed_prev) {
-				if (_param_aspd_scale_apply.get() > 0) {
-					if (fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > 0.01f) {
-						// apply the new scale if changed more than 0.01
-						mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.2f --> %.2f", i + 1,
-								 (double)_param_airspeed_scale[i],
-								 (double)_airspeed_validator[i].get_CAS_scale_validated());
+				if (_param_aspd_scale_apply.get() > 0 && _airspeed_validator[i].get_airspeed_valid()
+				    && fabsf(_airspeed_validator[i].get_CAS_scale_validated() - _param_airspeed_scale[i]) > FLT_EPSILON) {
 
-						switch (i) {
-						case 0:
-							_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated());
-							_param_airspeed_scale_1.commit_no_notification();
-							break;
+					mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor Nr. %d ASPD_SCALE updated: %.4f --> %.4f", i + 1,
+							 (double)_param_airspeed_scale[i],
+							 (double)_airspeed_validator[i].get_CAS_scale_validated());
 
-						case 1:
-							_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated());
-							_param_airspeed_scale_2.commit_no_notification();
-							break;
+					switch (i) {
+					case 0:
+						_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated());
+						_param_airspeed_scale_1.commit_no_notification();
+						break;
 
-						case 2:
-							_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated());
-							_param_airspeed_scale_3.commit_no_notification();
-							break;
-						}
+					case 1:
+						_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated());
+						_param_airspeed_scale_2.commit_no_notification();
+						break;
+
+					case 2:
+						_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated());
+						_param_airspeed_scale_3.commit_no_notification();
+						break;
 					}
+
 				}
 
 				_airspeed_validator[i].set_scale_init(_param_airspeed_scale[i]);
@@ -553,8 +553,7 @@ void AirspeedModule::update_ground_minus_wind_airspeed()
 		const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
 		const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
 		_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
-		_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
-					 _vehicle_air_data.baro_temp_celcius);
+		_ground_minus_wind_CAS = calc_calibrated_from_true_airspeed(_ground_minus_wind_TAS, _vehicle_air_data.rho);
 
 	} else {
 		_ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN;

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -88,14 +88,14 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 	}
 
 	if (!missing_data) {
-		estimator_status_s estimator_status;
+		estimator_status_flags_s estimator_status_flags;
 
-		if (_estimator_status_sub.copy(&estimator_status)) {
-			pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
-			pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
+		if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
+			pre_flt_fail_innov_heading = estimator_status_flags.pre_flt_fail_innov_heading;
+			pre_flt_fail_innov_vel_horiz = estimator_status_flags.pre_flt_fail_innov_vel_horiz;
 
-			checkEstimatorStatus(context, reporter, estimator_status, required_groups);
-			checkEstimatorStatusFlags(context, reporter, estimator_status, lpos);
+			checkEstimatorStatus(context, reporter, lpos, required_groups);
+			checkEstimatorStatusFlags(context, reporter, estimator_status_flags, required_groups);
 
 		} else {
 			missing_data = true;
@@ -129,327 +129,261 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 }
 
 void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
-		const estimator_status_s &estimator_status, NavModes required_groups)
+		const vehicle_local_position_s &lpos, NavModes required_groups)
 {
-	if (!context.isArmed() && estimator_status.pre_flt_fail_innov_heading) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_heading_not_stable"),
-					    events::Log::Error, "Heading estimate not stable");
+	estimator_status_s estimator_status;
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
+	if (_estimator_status_sub.copy(&estimator_status)) {
+
+		// check vertical position innovation test ratio
+		if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_hgt_est_err"),
+					events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
+			}
 		}
 
-	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_vel_horiz) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_hor_vel_not_stable"),
-					    events::Log::Error, "Horizontal velocity unstable");
+		// check velocity innovation test ratio
+		if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_vel_est_err"),
+					events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity unstable");
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
+			}
 		}
 
-	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_vel_vert) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_vert_vel_not_stable"),
-					    events::Log::Error, "Vertical velocity unstable");
+		// check horizontal position innovation test ratio
+		if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_pos_est_err"),
+					events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable");
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
+			}
 		}
 
-	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_hgt_not_stable"),
-					    events::Log::Error, "Height estimate not stable");
+		// check magnetometer innovation test ratio
+		if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) {
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * Test ratio: {1:.3}, limit: {2:.3}.
+			 *
+			 * This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
+					events::ID("check_estimator_yaw_est_err"),
+					events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable");
-		}
-	}
-
-
-	if ((_param_com_arm_mag_str.get() >= 1)
-	    && (!context.isArmed() && estimator_status.pre_flt_fail_mag_field_disturbed)) {
-
-		NavModes required_groups_mag = required_groups;
-
-		if (_param_com_arm_mag_str.get() != 1) {
-			required_groups_mag = NavModes::None; // optional
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
+			}
 		}
 
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_mag_interference"),
-					    events::Log::Warning, "Strong magnetic interference");
+		// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
+		if (!context.isArmed() && _param_sys_has_gps.get()) {
+			const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+			const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference");
-		}
-	}
-
-	// check vertical position innovation test ratio
-	if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_hgt_est_err"),
-				events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
-		}
-	}
-
-	// check velocity innovation test ratio
-	if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_vel_est_err"),
-				events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
-		}
-	}
-
-	// check horizontal position innovation test ratio
-	if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_pos_est_err"),
-				events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
-		}
-	}
-
-	// check magnetometer innovation test ratio
-	if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_yaw_est_err"),
-				events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
-		}
-	}
-
-	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
-	if (!context.isArmed() && _param_sys_has_gps.get()) {
-		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
-		const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
-
-		if (ekf_gps_fusion) {
-			reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
-		}
-
-		if (ekf_gps_check_fail) {
-			NavModes required_groups_gps = required_groups;
-			events::Log log_level = events::Log::Error;
-
-			if (_param_com_arm_wo_gps.get()) {
-				required_groups_gps = NavModes::None; // optional
-				log_level = events::Log::Warning;
+			if (ekf_gps_fusion) {
+				reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
 			}
 
-			// Only report the first failure to avoid spamming
-			const char *message = nullptr;
+			if (ekf_gps_check_fail) {
+				NavModes required_groups_gps = required_groups;
+				events::Log log_level = events::Log::Error;
 
-			if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
-				message = "Preflight%s: GPS fix too low";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_fix_too_low"),
-							    log_level, "GPS fix too low");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
-				message = "Preflight%s: not enough GPS Satellites";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_num_sats_too_low"),
-							    log_level, "Not enough GPS Satellites");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
-				message = "Preflight%s: GPS PDOP too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_pdop_too_high"),
-							    log_level, "GPS PDOP too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
-				message = "Preflight%s: GPS Horizontal Pos Error too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_pos_err_too_high"),
-							    log_level, "GPS Horizontal Position Error too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
-				message = "Preflight%s: GPS Vertical Pos Error too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_pos_err_too_high"),
-							    log_level, "GPS Vertical Position Error too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
-				message = "Preflight%s: GPS Speed Accuracy too low";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_speed_acc_too_low"),
-							    log_level, "GPS Speed Accuracy too low");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
-				message = "Preflight%s: GPS Horizontal Pos Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
-							    log_level, "GPS Horizontal Position Drift too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
-				message = "Preflight%s: GPS Vertical Pos Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
-							    log_level, "GPS Vertical Position Drift too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
-				message = "Preflight%s: GPS Hor Speed Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
-							    log_level, "GPS Horizontal Speed Drift too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
-				message = "Preflight%s: GPS Vert Speed Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
-							    log_level, "GPS Vertical Speed Drift too high");
-
-			} else {
-				if (!ekf_gps_fusion) {
-					// Likely cause unknown
-					message = "Preflight%s: Estimator not using GPS";
-					/* EVENT
-					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-								    events::ID("check_estimator_gps_not_fusing"),
-								    log_level, "Estimator not using GPS");
-
-				} else {
-					// if we land here there was a new flag added and the code not updated. Show a generic message.
-					message = "Preflight%s: Poor GPS Quality";
-					/* EVENT
-					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-								    events::ID("check_estimator_gps_generic"),
-								    log_level, "Poor GPS Quality");
+				if (_param_com_arm_wo_gps.get()) {
+					required_groups_gps = NavModes::None; // optional
+					log_level = events::Log::Warning;
 				}
-			}
 
-			if (message && reporter.mavlink_log_pub()) {
-				if (!_param_com_arm_wo_gps.get()) {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+				// Only report the first failure to avoid spamming
+				const char *message = nullptr;
+
+				if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
+					message = "Preflight%s: GPS fix too low";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_fix_too_low"),
+								    log_level, "GPS fix too low");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
+					message = "Preflight%s: not enough GPS Satellites";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_num_sats_too_low"),
+								    log_level, "Not enough GPS Satellites");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
+					message = "Preflight%s: GPS PDOP too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_pdop_too_high"),
+								    log_level, "GPS PDOP too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
+					message = "Preflight%s: GPS Horizontal Pos Error too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_pos_err_too_high"),
+								    log_level, "GPS Horizontal Position Error too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
+					message = "Preflight%s: GPS Vertical Pos Error too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_pos_err_too_high"),
+								    log_level, "GPS Vertical Position Error too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
+					message = "Preflight%s: GPS Speed Accuracy too low";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_speed_acc_too_low"),
+								    log_level, "GPS Speed Accuracy too low");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
+					message = "Preflight%s: GPS Horizontal Pos Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
+								    log_level, "GPS Horizontal Position Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
+					message = "Preflight%s: GPS Vertical Pos Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
+								    log_level, "GPS Vertical Position Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
+					message = "Preflight%s: GPS Hor Speed Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
+								    log_level, "GPS Horizontal Speed Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
+					message = "Preflight%s: GPS Vert Speed Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
+								    log_level, "GPS Vertical Speed Drift too high");
 
 				} else {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+					if (!ekf_gps_fusion) {
+						// Likely cause unknown
+						message = "Preflight%s: Estimator not using GPS";
+						/* EVENT
+						 */
+						reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+									    events::ID("check_estimator_gps_not_fusing"),
+									    log_level, "Estimator not using GPS");
+
+					} else {
+						// if we land here there was a new flag added and the code not updated. Show a generic message.
+						message = "Preflight%s: Poor GPS Quality";
+						/* EVENT
+						 */
+						reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+									    events::ID("check_estimator_gps_generic"),
+									    log_level, "Poor GPS Quality");
+					}
+				}
+
+				if (message && reporter.mavlink_log_pub()) {
+					if (!_param_com_arm_wo_gps.get()) {
+						mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+
+					} else {
+						mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+					}
 				}
 			}
 		}
 	}
-
 }
 
 void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups)
@@ -530,111 +464,117 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter,
 }
 
 void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &reporter,
-		const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos)
+		const estimator_status_flags_s &estimator_status_flags, NavModes required_groups)
 {
-	estimator_status_flags_s estimator_status_flags;
+	if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_heading) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_heading_not_stable"),
+					    events::Log::Error, "Heading estimate not stable");
 
-	if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
-
-		bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
-				      || estimator_status_flags.cs_inertial_dead_reckoning;
-
-		if (!dead_reckoning) {
-			// position requirements (update if not dead reckoning)
-			bool gps             = estimator_status_flags.cs_gps;
-			bool optical_flow    = estimator_status_flags.cs_opt_flow;
-			bool vision_position = estimator_status_flags.cs_ev_pos;
-
-			_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
 		}
 
-		// Check for a magnetomer fault and notify the user
-		if (estimator_status_flags.cs_mag_fault) {
-			/* EVENT
-			 * @description
-			 * Land and calibrate the compass.
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
-						    events::ID("check_estimator_mag_fault"),
-						    events::Log::Critical, "Stopping compass use");
+	} else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_vel_horiz) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hor_vel_not_stable"),
+					    events::Log::Error, "Horizontal velocity unstable");
 
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t");
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity unstable");
 		}
 
-		if (estimator_status_flags.cs_gps_yaw_fault) {
-			/* EVENT
-			 * @description
-			 * Land now
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
-						    events::ID("check_estimator_gnss_fault"),
-						    events::Log::Critical, "GNSS heading not reliable");
+	} else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_vel_vert) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_vert_vel_not_stable"),
+					    events::Log::Error, "Vertical velocity unstable");
 
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t");
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable");
+		}
+
+	} else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_height) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hgt_not_stable"),
+					    events::Log::Error, "Height estimate not stable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable");
 		}
 	}
 
-	const hrt_abstime now = hrt_absolute_time();
 
-	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
-	* for a short time interval after takeoff.
-	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
-	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
-	* if this does not fix the issue we need to stop using a position controlled
-	* mode to prevent flyaway crashes.
-	*/
+	if ((_param_com_arm_mag_str.get() >= 1)
+	    && (!context.isArmed() && estimator_status_flags.cs_mag_field_disturbed)) {
 
-	if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		NavModes required_groups_mag = required_groups;
 
-		if (!context.isArmed()) {
-			_nav_test_failed = false;
-			_nav_test_passed = false;
+		if (_param_com_arm_mag_str.get() != 1) {
+			required_groups_mag = NavModes::None; // optional
+		}
 
-		} else {
-			if (!_nav_test_passed) {
-				// Both test ratios need to pass/fail together to change the nav test status
-				const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
-							     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_mag_interference"),
+					    events::Log::Warning, "Strong magnetic interference");
 
-				const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference");
+		}
+	}
 
-				if (innovation_pass) {
-					_time_last_innov_pass = now;
 
-					// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-					const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s);
-					const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
+	bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
+			      || estimator_status_flags.cs_inertial_dead_reckoning;
 
-					// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
-					if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) {
-						_nav_test_passed = true;
-						_nav_test_failed = false;
-					}
+	if (!dead_reckoning) {
+		// position requirements (update if not dead reckoning)
+		bool gps             = estimator_status_flags.cs_gps;
+		bool optical_flow    = estimator_status_flags.cs_opt_flow;
+		bool vision_position = estimator_status_flags.cs_ev_pos;
 
-				} else if (innovation_fail) {
-					_time_last_innov_fail = now;
+		_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
+	}
 
-					if (now > _time_last_innov_pass + 2_s) {
-						// if the innovation test has failed continuously, declare the nav as failed
-						_nav_test_failed = true;
-						/* EVENT
-						 * @description
-						 * Land and recalibrate the sensors.
-						 */
-						reporter.healthFailure(NavModes::All, health_component_t::local_position_estimate,
-								       events::ID("check_estimator_nav_failure"),
-								       events::Log::Emergency, "Navigation failure");
+	// Check for a magnetomer fault and notify the user
+	if (estimator_status_flags.cs_mag_fault) {
+		/* EVENT
+		 * @description
+		 * Land and calibrate the compass.
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_mag_fault"),
+					    events::Log::Critical, "Stopping compass use");
 
-						if (reporter.mavlink_log_pub()) {
-							mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t");
-						}
-					}
-				}
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t");
+		}
+	}
+
+	if (estimator_status_flags.cs_gps_yaw_fault) {
+		/* EVENT
+		 * @description
+		 * Land now
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_gnss_fault"),
+					    events::Log::Critical, "GNSS heading not reliable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t");
 		}
 	}
 }
@@ -693,8 +633,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		lpos_eph_threshold_relaxed = INFINITY;
 	}
 
-	bool xy_valid = lpos.xy_valid && !_nav_test_failed;
-	bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
+	bool xy_valid = lpos.xy_valid;
+	bool v_xy_valid = lpos.v_xy_valid;
 
 	if (!context.isArmed()) {
 		if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -59,11 +59,11 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
-	void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
+	void checkEstimatorStatus(const Context &context, Report &reporter, const vehicle_local_position_s &lpos,
 				  NavModes required_groups);
 	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
-	void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
-				       const vehicle_local_position_s &lpos);
+	void checkEstimatorStatusFlags(const Context &context, Report &reporter,
+				       const estimator_status_flags_s &estimator_status_flags, NavModes required_groups);
 
 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void gpsNoLongerValid(const Context &context, Report &reporter) const;
@@ -93,12 +93,6 @@ private:
 	hrt_abstime	_last_lpos_relaxed_fail_time_us{0};	///< Last time that the relaxed local position validity recovery check failed (usec)
 	hrt_abstime	_last_lvel_fail_time_us{0};	///< Last time that the local velocity validity recovery check failed (usec)
 
-	// variables used to check for navigation failure after takeoff
-	hrt_abstime	_time_last_innov_pass{0};	///< last time velocity and position innovations passed
-	hrt_abstime	_time_last_innov_fail{0};	///< last time velocity and position innovations failed
-	bool		_nav_test_passed{false};	///< true if the post takeoff navigation test has passed
-	bool		_nav_test_failed{false};	///< true if the post takeoff navigation test has failed
-
 	static constexpr hrt_abstime GPS_VALID_TIME{3_s};
 	systemlib::Hysteresis _vehicle_gps_position_valid{false};
 

src/modules/commander/failsafe/emscripten.cpp

@@ -89,6 +89,19 @@ int param_get(param_t param, void *val)
 	return -1;
 }
 
+void param_set_used(param_t param)
+{
+	std::map<param_t, Param> &used_params = failsafe_instance.params();
+
+	if (used_params.find(param) != used_params.end()) {
+		return;
+	}
+
+	Param p;
+	memcpy(&p.val, &px4::parameters[param].val, sizeof(p.val));
+	used_params[param] = p;
+}
+
 std::vector<std::string> get_used_params()
 {
 	std::vector<std::string> ret;

src/modules/ekf2/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-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
@@ -31,8 +31,6 @@
 #
 #############################################################################
 
-add_subdirectory(Utility)
-
 # Symforce code generation TODO:fixme
 # execute_process(
 #     COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
@@ -91,8 +89,11 @@ px4_add_module(
 		EKF/ekf_helper.cpp
 		EKF/EKFGSF_yaw.cpp
 		EKF/estimator_interface.cpp
+		EKF/ev_control.cpp
 		EKF/ev_height_control.cpp
+		EKF/ev_pos_control.cpp
 		EKF/ev_vel_control.cpp
+		EKF/ev_yaw_control.cpp
 		EKF/fake_height_control.cpp
 		EKF/fake_pos_control.cpp
 		EKF/gnss_height_control.cpp
@@ -123,7 +124,6 @@ px4_add_module(
 		geo
 		hysteresis
 		perf
-		EKF2Utility
 		px4_work_queue
 		world_magnetic_model
 	UNITY_BUILD

src/modules/ekf2/EKF/CMakeLists.txt

@@ -42,8 +42,11 @@ add_library(ecl_EKF
 	ekf_helper.cpp
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
+	ev_control.cpp
 	ev_height_control.cpp
+	ev_pos_control.cpp
 	ev_vel_control.cpp
+	ev_yaw_control.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -80,6 +80,11 @@ void Ekf::controlBaroHeightFusion()
 						   innov_gate,
 						   aid_src);
 
+		// filtered innovation for preflight checks
+		if (!aid_src.innovation_rejected) {
+			_baro_hgt_innov_lpf.update(aid_src.innovation);
+		}
+
 		// Compensate for positive static pressure transients (negative vertical position innovations)
 		// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
 		if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) {

src/modules/ekf2/EKF/bias_estimator.hpp

@@ -64,7 +64,9 @@ public:
 		float innov_test_ratio{INFINITY};
 	};
 
+	BiasEstimator() {}
 	BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {};
+
 	virtual ~BiasEstimator() = default;
 
 	void reset()

src/modules/ekf2/EKF/common.h

@@ -79,6 +79,11 @@ static constexpr float BADACC_BIAS_PNOISE = 4.9f;  ///< The delta velocity proce
 // ground effect compensation
 static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
 
+enum class PositionFrame : uint8_t {
+	LOCAL_FRAME_NED = 0,
+	LOCAL_FRAME_FRD = 1,
+};
+
 enum class VelocityFrame : uint8_t {
 	LOCAL_FRAME_NED = 0,
 	LOCAL_FRAME_FRD = 1,
@@ -115,6 +120,12 @@ enum HeightSensor : uint8_t {
 	UNKNOWN  = 4
 };
 
+enum class PositionSensor : uint8_t {
+	UNKNOWN = 0,
+	GNSS    = 1,
+	EV      = 2,
+};
+
 enum GnssCtrl : uint8_t {
 	HPOS  = (1<<0),
 	VPOS  = (1<<1),
@@ -140,11 +151,11 @@ enum SensorFusionMask : uint16_t {
 	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
 	USE_OPT_FLOW     = (1<<1),      ///< set to true to use optical flow data
 	INHIBIT_ACC_BIAS = (1<<2),      ///< set to true to inhibit estimation of accelerometer delta velocity bias
-	USE_EXT_VIS_POS  = (1<<3),      ///< set to true to use external vision position data
-	USE_EXT_VIS_YAW  = (1<<4),      ///< set to true to use external vision quaternion data for yaw
+	DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
+	DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
-	ROTATE_EXT_VIS   = (1<<6),      ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
-	DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
+	DEPRECATED_ROTATE_EXT_VIS  = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
+	DEPRECATED_USE_GPS_YAW     = (1<<7), ///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
 	DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
 };
 
@@ -238,9 +249,10 @@ struct extVisionSample {
 	Vector3f    pos{};         ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
 	Vector3f    vel{};         ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
 	Quatf       quat{};        ///< quaternion defining rotation from body to earth frame
-	Vector3f    posVar{};      ///< XYZ position variances (m**2)
+	Vector3f    position_var{};    ///< XYZ position variances (m**2)
 	Vector3f    velocity_var{};    ///< XYZ velocity variances ((m/sec)**2)
 	Vector3f    orientation_var{}; ///< orientation variance (rad**2)
+	PositionFrame pos_frame = PositionFrame::LOCAL_FRAME_FRD;
 	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
 	int8_t     quality{};     ///< quality indicator between 0 and 100
@@ -283,6 +295,7 @@ struct parameters {
 	// measurement source control
 	int32_t fusion_mode{};         ///< bitmasked integer that selects some aiding sources
 	int32_t height_sensor_ref{HeightSensor::BARO};
+	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
 	int32_t baro_ctrl{1};
 	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
 	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
@@ -325,7 +338,7 @@ struct parameters {
 	const float initial_wind_uncertainty{1.0f};     ///< 1-sigma initial uncertainty in wind velocity (m/sec)
 
 	// position and velocity fusion
-	float gps_vel_noise{5.0e-1f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
+	float gps_vel_noise{0.5f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
 	float gps_pos_noise{0.5f};              ///< minimum allowed observation noise for gps position fusion (m)
 	float gps_hgt_bias_nsd{0.13f};          ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
 	float pos_noaid_noise{10.0f};           ///< observation noise for non-aiding position fusion (m)

src/modules/ekf2/EKF/control.cpp

@@ -183,10 +183,6 @@ void Ekf::controlFusionModes()
 		}
 	}
 
-	if (_ext_vision_buffer) {
-		_ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
-	}
-
 	if (_airspeed_buffer) {
 		_tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
 	}
@@ -203,7 +199,7 @@ void Ekf::controlFusionModes()
 	controlDragFusion();
 	controlHeightFusion();
 
-	// Additional data odoemtery data from an external estimator can be fused.
+	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
 
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
@@ -221,213 +217,6 @@ void Ekf::controlFusionModes()
 	updateDeadReckoningStatus();
 }
 
-void Ekf::controlExternalVisionFusion()
-{
-	// Check for new external vision data
-	if (_ev_data_ready) {
-
-		bool reset = false;
-
-		if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) {
-			reset = true;
-		}
-
-		if (_inhibit_ev_yaw_use) {
-			stopEvYawFusion();
-		}
-
-		// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
-		// needs to be calculated and the observations rotated into the EKF frame of reference
-		if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
-				|| (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) {
-
-			// rotate EV measurements into the EKF Navigation frame
-			calcExtVisRotMat();
-		}
-
-		// external vision aiding selection logic
-		if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
-
-			// check for a external vision measurement that has fallen behind the fusion time horizon
-			if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
-				// turn on use of external vision measurements for position
-				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
-					startEvPosFusion();
-				}
-			}
-		}
-
-		// external vision yaw aiding selection logic
-		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw
-		    && _control_status.flags.tilt_align) {
-
-			// don't start using EV data unless data is arriving frequently
-			if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
-				if (resetYawToEv()) {
-					_control_status.flags.yaw_align = true;
-					startEvYawFusion();
-				}
-			}
-		}
-
-		// determine if we should use the horizontal position observations
-		if (_control_status.flags.ev_pos) {
-			resetEstimatorAidStatus(_aid_src_ev_pos);
-
-			if (reset && _control_status_prev.flags.ev_pos) {
-				if (!_fuse_hpos_as_odom) {
-					resetHorizontalPositionToVision();
-				}
-			}
-
-			Vector3f ev_pos_obs_var;
-
-			// correct position and height for offset relative to IMU
-			const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_ev_sample_delayed.pos -= pos_offset_earth;
-
-			// Use an incremental position fusion method for EV position data if GPS is also used
-			if (_params.gnss_ctrl & GnssCtrl::HPOS) {
-				_fuse_hpos_as_odom = true;
-
-			} else {
-				_fuse_hpos_as_odom = false;
-			}
-
-			if (_fuse_hpos_as_odom) {
-				if (!_hpos_prev_available) {
-					// no previous observation available to calculate position change
-					_hpos_prev_available = true;
-
-				} else {
-					// calculate the change in position since the last measurement
-					// rotate measurement into body frame is required when fusing with GPS
-					Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos);
-
-					// use the change in position since the last measurement
-					_aid_src_ev_pos.observation[0] = ev_delta_pos(0);
-					_aid_src_ev_pos.observation[1] = ev_delta_pos(1);
-
-					_aid_src_ev_pos.innovation[0] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
-					_aid_src_ev_pos.innovation[1] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
-
-					// observation 1-STD error, incremental pos observation is expected to have more uncertainty
-					Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
-					ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
-					ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f));
-					ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f));
-
-					_aid_src_ev_pos.observation_variance[0] = ev_pos_obs_var(0);
-					_aid_src_ev_pos.observation_variance[1] = ev_pos_obs_var(1);
-
-					_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
-					_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
-				}
-			} else {
-				// use the absolute position
-				Vector3f ev_pos_meas = _ev_sample_delayed.pos;
-				Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
-
-				if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-					ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
-					ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
-				}
-
-				_aid_src_ev_pos.observation[0] = ev_pos_meas(0);
-				_aid_src_ev_pos.observation[1] = ev_pos_meas(1);
-
-				_aid_src_ev_pos.observation_variance[0] = fmaxf(ev_pos_var(0, 0), sq(0.01f));
-				_aid_src_ev_pos.observation_variance[1] = fmaxf(ev_pos_var(1, 1), sq(0.01f));
-
-				_aid_src_ev_pos.innovation[0] = _state.pos(0) - _aid_src_ev_pos.observation[0];
-				_aid_src_ev_pos.innovation[1] = _state.pos(1) - _aid_src_ev_pos.observation[1];
-
-				_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
-				_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
-
-				// check if we have been deadreckoning too long
-				if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
-					resetHorizontalPositionToVision();
-				}
-			}
-
-			// innovation gate size
-			const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
-			setEstimatorAidStatusTestRatio(_aid_src_ev_pos, ev_pos_innov_gate);
-
-			_aid_src_ev_pos.timestamp_sample = _ev_sample_delayed.time_us;
-			_aid_src_ev_pos.fusion_enabled = true;
-
-			fuseHorizontalPosition(_aid_src_ev_pos);
-		}
-
-		// determine if we should use the yaw observation
-		resetEstimatorAidStatus(_aid_src_ev_yaw);
-		const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat);
-		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f);
-
-		if (PX4_ISFINITE(measured_hdg)) {
-			_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
-			_aid_src_ev_yaw.observation = measured_hdg;
-			_aid_src_ev_yaw.observation_variance = ev_yaw_obs_var;
-			_aid_src_ev_yaw.fusion_enabled = _control_status.flags.ev_yaw;
-
-			if (_control_status.flags.ev_yaw) {
-				if (reset && _control_status_prev.flags.ev_yaw) {
-					resetYawToEv();
-				}
-
-				const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
-
-				fuseYaw(innovation, ev_yaw_obs_var, _aid_src_ev_yaw);
-
-			} else {
-				// populate estimator_aid_src_ev_yaw with delta heading innovations for logging
-				// use the change in yaw since the last measurement
-				const float measured_hdg_prev = getEulerYaw(_ev_sample_delayed_prev.quat);
-
-				// calculate the change in yaw since the last measurement
-				const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev);
-
-				_aid_src_ev_yaw.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yaw_pred_prev - ev_delta_yaw);
-			}
-		}
-
-
-		bool ev_reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
-
-		// determine if we should use the horizontal position observations
-		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum);
-
-		const bool starting_conditions_passing = quality_sufficient
-						   && ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL)
-						   && ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
-						   && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
-						   && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
-
-		// EV velocity
-		controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
-
-		// EV height
-		controlEvHeightFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
-
-
-		// record observation and estimate for use next time
-		_ev_sample_delayed_prev = _ev_sample_delayed;
-		_hpos_pred_prev = _state.pos.xy();
-		_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
-
-	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt)
-		   && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) {
-
-		// Turn off EV fusion mode if no data has been received
-		stopEvFusion();
-		_warning_events.flags.vision_data_stopped = true;
-		ECL_WARN("vision data stopped");
-	}
-}
-
 void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
 {
 	if (!(_params.gnss_ctrl & GnssCtrl::YAW)

src/modules/ekf2/EKF/ekf.h

@@ -48,6 +48,7 @@
 #include "EKFGSF_yaw.h"
 #include "bias_estimator.hpp"
 #include "height_bias_estimator.hpp"
+#include "position_bias_estimator.hpp"
 
 #include <uORB/topics/estimator_aid_source1d.h>
 #include <uORB/topics/estimator_aid_source2d.h>
@@ -379,9 +380,6 @@ public:
 	// return a bitmask integer that describes which state estimates can be used for flight control
 	void get_ekf_soln_status(uint16_t *status) const;
 
-	// return the quaternion defining the rotation from the External Vision to the EKF reference frame
-	matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); };
-
 	// use the latest IMU data at the current time horizon.
 	Quatf calculate_quaternion() const;
 
@@ -410,6 +408,8 @@ public:
 	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
 	const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
 
+	const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
+
 	const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
 	const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
 
@@ -476,23 +476,18 @@ private:
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
 
-	// variables used when position data is being fused using a relative position odometry model
-	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
-	Vector2f _hpos_pred_prev{};		///< previous value of NE position state used by odometry fusion (m)
-	float _yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
-	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
-	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
+	float _ev_yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
 	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 	bool _rng_data_ready{false};
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
-	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
 	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
+
 	uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
 	uint64_t _time_last_v_pos_aiding{0};
 	uint64_t _time_last_v_vel_aiding{0};
@@ -508,7 +503,9 @@ private:
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
+	uint8_t _nb_ev_pos_reset_available{0};
 	uint8_t _nb_ev_vel_reset_available{0};
+	uint8_t _nb_ev_yaw_reset_available{0};
 
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
@@ -708,7 +705,6 @@ private:
 	void resetHorizontalVelocityToZero();
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
-	void resetHorizontalPositionToVision();
 	void resetHorizontalPositionToLastKnown();
 
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
@@ -776,19 +772,12 @@ private:
 	// return true if successful
 	bool resetMagHeading();
 
-	// reset the heading using the external vision measurements
-	// return true if successful
-	bool resetYawToEv();
-
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
 
 	// modify output filter to match the the EKF state at the fusion time horizon
 	void alignOutputFilter();
 
-	// update the rotation matrix which transforms EV navigation frame measurements into NED
-	void calcExtVisRotMat();
-
 	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
 	{
 		for (unsigned i = 0; i < 3; i++) {
@@ -851,8 +840,16 @@ private:
 
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
+
 	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
+
+	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s& aid_src);
+	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
+	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
+	void stopEvPosFusion();
+
 	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src);
+	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
 
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion();
@@ -1008,11 +1005,6 @@ private:
 	void startGpsYawFusion(const gpsSample &gps_sample);
 	void stopGpsYawFusion();
 
-	void startEvPosFusion();
-	void startEvYawFusion();
-
-	void stopEvFusion();
-	void stopEvPosFusion();
 	void stopEvVelFusion();
 	void stopEvYawFusion();
 
@@ -1036,11 +1028,13 @@ private:
 	EKFGSF_yaw _yawEstimator{};
 
 	uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
+	uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
 
 	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
 	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
 	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
+	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
 
 	void runYawEKFGSF();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -124,22 +124,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 	_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
 }
 
-void Ekf::resetHorizontalPositionToVision()
-{
-	_information_events.flags.reset_pos_to_vision = true;
-	ECL_INFO("reset position to ev position");
-	Vector3f _ev_pos = _ev_sample_delayed.pos;
-
-	if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-		_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
-	}
-
-	resetHorizontalPositionTo(Vector2f(_ev_pos), _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
-
-	// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
-	_hpos_prev_available = false;
-}
-
 void Ekf::resetHorizontalPositionToLastKnown()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
@@ -178,6 +162,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 
 	_state_reset_status.reset_count.posNE++;
 
+	_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
+	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
+
 	// Reset the timout timer
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
 }
@@ -193,7 +180,6 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }
 
-
 void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
 {
 	const float old_vert_pos = _state.pos(2);
@@ -275,8 +261,8 @@ void Ekf::alignOutputFilter()
 bool Ekf::resetMagHeading()
 {
 	// prevent a reset being performed more than once on the same frame
-	if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
-		return true;
+	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
+		return false;
 	}
 
 	const Vector3f mag_init = _mag_lpf.getState();
@@ -320,17 +306,6 @@ bool Ekf::resetMagHeading()
 	return false;
 }
 
-bool Ekf::resetYawToEv()
-{
-	const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
-	const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f));
-
-	resetQuatStateYaw(yaw_new, yaw_new_variance);
-	_R_ev_to_ekf.setIdentity();
-
-	return true;
-}
-
 // Return the magnetic declination in radians to be used by the alignment and fusion processing
 float Ekf::getMagDeclination()
 {
@@ -755,6 +730,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 
 	} else if (_control_status.flags.gps_yaw) {
 		mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
+
 	} else {
 		mag = NAN;
 	}
@@ -1092,7 +1068,7 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 void Ekf::stopMagFusion()
 {
 	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		ECL_INFO("stopping mag fusion");
+		ECL_INFO("stopping all mag fusion");
 		stopMag3DFusion();
 		stopMagHdgFusion();
 		clearMagCov();
@@ -1166,14 +1142,6 @@ void Ekf::updateGroundEffect()
 	}
 }
 
-// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
-void Ekf::calcExtVisRotMat()
-{
-	// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
-	const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
-	_R_ev_to_ekf = Dcmf(q_error);
-}
-
 // Increase the yaw error variance of the quaternions
 // Argument is additional yaw variance in rad**2
 void Ekf::increaseQuatYawErrVariance(float yaw_variance)
@@ -1320,52 +1288,6 @@ void Ekf::stopGpsYawFusion()
 	}
 }
 
-void Ekf::startEvPosFusion()
-{
-	_control_status.flags.ev_pos = true;
-	resetHorizontalPositionToVision();
-	_information_events.flags.starting_vision_pos_fusion = true;
-	ECL_INFO("starting vision pos fusion");
-}
-
-void Ekf::startEvYawFusion()
-{
-	// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
-	_control_status.flags.ev_yaw = true;
-	_control_status.flags.mag_dec = false;
-
-	stopMagHdgFusion();
-	stopMag3DFusion();
-
-	_information_events.flags.starting_vision_yaw_fusion = true;
-	ECL_INFO("starting vision yaw fusion");
-}
-
-void Ekf::stopEvFusion()
-{
-	stopEvPosFusion();
-	stopEvVelFusion();
-	stopEvYawFusion();
-	stopEvHgtFusion();
-}
-
-void Ekf::stopEvPosFusion()
-{
-	if (_control_status.flags.ev_pos) {
-		ECL_INFO("stopping EV pos fusion");
-		_control_status.flags.ev_pos = false;
-		resetEstimatorAidStatus(_aid_src_ev_pos);
-	}
-}
-
-void Ekf::stopEvYawFusion()
-{
-	if (_control_status.flags.ev_yaw) {
-		ECL_INFO("stopping EV yaw fusion");
-		_control_status.flags.ev_yaw = false;
-	}
-}
-
 void Ekf::stopAuxVelFusion()
 {
 	ECL_INFO("stopping aux vel fusion");

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -542,7 +542,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
 
-	if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl > 0)) {
+	if (_params.ev_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -117,6 +117,25 @@ public:
 			_time_last_in_air = _imu_sample_delayed.time_us;
 		}
 
+		// reset all preflight innovation filters on takeoff or landing
+		if (_control_status.flags.in_air != in_air) {
+			_baro_hgt_innov_lpf.reset();
+			_gnss_hgt_innov_lpf.reset();
+			_rng_hgt_innov_lpf.reset();
+			_ev_hgt_innov_lpf.reset();
+
+			_mag_heading_innov_lpf.reset();
+			_gnss_yaw_innov_lpf.reset();
+			_ev_yaw_innov_lpf.reset();
+
+			_ev_pos_innov_lpf.reset();
+			_gnss_pos_innov_lpf.reset();
+
+			_gnss_vel_innov_lpf.reset();
+			_ev_vel_innov_lpf.reset();
+			_optical_flow_vel_innov_lpf.reset();
+		}
+
 		_control_status.flags.in_air = in_air;
 	}
 
@@ -258,7 +277,6 @@ public:
 
 	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
 	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
-	const extVisionSample &get_ev_sample_delayed() const { return _ev_sample_delayed; }
 
 	const bool &global_origin_valid() const { return _NED_origin_initialised; }
 	const MapProjection &global_origin() const { return _pos_ref; }
@@ -269,6 +287,97 @@ public:
 	float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
 	float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
 
+	float filteredHorizontalVelocityInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.gps) {
+			innov = math::max(innov, _gnss_vel_innov_lpf.getState().xy().norm());
+		}
+
+		if (_control_status.flags.ev_vel) {
+			innov = math::max(innov, _ev_vel_innov_lpf.getState().xy().norm());
+		}
+
+		if (_control_status.flags.opt_flow) {
+			innov = math::max(innov, _optical_flow_vel_innov_lpf.getState().norm());
+		}
+
+		return innov;
+	}
+
+	float filteredVerticalVelocityInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.gps) {
+			innov = math::max(innov, _gnss_vel_innov_lpf.getState()(2));
+		}
+
+		if (_control_status.flags.ev_pos) {
+			innov = math::max(innov, _ev_vel_innov_lpf.getState()(2));
+		}
+
+		return innov;
+	}
+
+	float filteredHorizontalPositionInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.gps) {
+			innov = math::max(innov, _gnss_pos_innov_lpf.getState().norm());
+		}
+
+		if (_control_status.flags.ev_pos) {
+			innov = math::max(innov, _ev_pos_innov_lpf.getState().norm());
+		}
+
+		return innov;
+	}
+
+	float filteredVerticalPositionInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.baro_hgt) {
+			innov = math::max(innov, _baro_hgt_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.gps_hgt) {
+			innov = math::max(innov, _gnss_hgt_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.ev_hgt) {
+			innov = math::max(innov, _ev_hgt_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.rng_hgt) {
+			innov = math::max(innov, _rng_hgt_innov_lpf.getState());
+		}
+
+		return innov;
+	}
+
+	float filteredHeadingInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.mag_hdg) {
+			innov = math::max(innov, _mag_heading_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.ev_yaw) {
+			innov = math::max(innov, _ev_yaw_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.gps_yaw) {
+			innov = math::max(innov, _gnss_yaw_innov_lpf.getState());
+		}
+
+		return innov;
+	}
+
 protected:
 
 	EstimatorInterface() = default;
@@ -305,8 +414,7 @@ protected:
 	sensor::SensorRangeFinder _range_sensor{};
 	airspeedSample _airspeed_sample_delayed{};
 	flowSample _flow_sample_delayed{};
-	extVisionSample _ev_sample_delayed{};
-	extVisionSample _ev_sample_delayed_prev{};
+	extVisionSample _ev_sample_prev{};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 
 	RangeFinderConsistencyCheck _rng_consistency_check;
@@ -401,6 +509,25 @@ protected:
 	warning_event_status_u _warning_events{};
 	information_event_status_u _information_events{};
 
+	// Preflight low pass filter time constant inverse (1/sec)
+	static constexpr float INNOV_LPF_TAU_INV = 0.2f;
+
+	AlphaFilter<float>    _baro_hgt_innov_lpf{INNOV_LPF_TAU_INV};       ///< filtered height innovations (m)
+	AlphaFilter<float>    _gnss_hgt_innov_lpf{INNOV_LPF_TAU_INV};       ///< filtered height innovations (m)
+	AlphaFilter<float>    _rng_hgt_innov_lpf{INNOV_LPF_TAU_INV};        ///< filtered height innovations (m)
+	AlphaFilter<float>    _ev_hgt_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered height innovations (m)
+
+	AlphaFilter<float>    _mag_heading_innov_lpf{INNOV_LPF_TAU_INV};    ///< filtered heading innovations (rad)
+	AlphaFilter<float>    _gnss_yaw_innov_lpf{INNOV_LPF_TAU_INV};       ///< filtered yaw innovations (rad)
+	AlphaFilter<float>    _ev_yaw_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered yaw innovations (rad)
+
+	AlphaFilter<Vector2f> _ev_pos_innov_lpf{INNOV_LPF_TAU_INV};           ///< filtered position innovations (m)
+	AlphaFilter<Vector2f> _gnss_pos_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered position innovations (m)
+
+	AlphaFilter<Vector3f> _ev_vel_innov_lpf{INNOV_LPF_TAU_INV};           ///< filtered velocity innovations (m/s)
+	AlphaFilter<Vector3f> _gnss_vel_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered velocity innovations (m/s)
+	AlphaFilter<Vector2f> _optical_flow_vel_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered velocity innovations (m/s)
+
 private:
 
 	inline void setDragData(const imuSample &imu);

src/modules/ekf2/EKF/ev_control.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ev_control.cpp
+ * Control functions for ekf external vision control
+ */
+
+#include "ekf.h"
+
+void Ekf::controlExternalVisionFusion()
+{
+	_ev_pos_b_est.predict(_dt_ekf_avg);
+
+	// Check for new external vision data
+	extVisionSample ev_sample;
+
+	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &ev_sample)) {
+
+		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
+
+		// determine if we should use the horizontal position observations
+		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
+
+		const bool starting_conditions_passing = quality_sufficient
+				&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
+				&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
+				&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
+				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
+
+		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
+		controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
+		controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
+		controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
+
+		if (quality_sufficient) {
+			_ev_sample_prev = ev_sample;
+		}
+
+		// record corresponding yaw state for future EV delta heading innovation (logging only)
+		_ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
+
+	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw
+		    || _control_status.flags.ev_hgt)
+		   && isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) {
+
+		// Turn off EV fusion mode if no data has been received
+		stopEvPosFusion();
+		stopEvVelFusion();
+		stopEvYawFusion();
+		stopEvHgtFusion();
+
+		_warning_events.flags.vision_data_stopped = true;
+		ECL_WARN("vision data stopped");
+	}
+}

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -53,7 +53,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 
 	// rotate measurement into correct earth frame if required
 	Vector3f pos{ev_sample.pos};
-	Matrix3f pos_cov{matrix::diag(ev_sample.posVar)};
+	Matrix3f pos_cov{matrix::diag(ev_sample.position_var)};
 
 	// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
 	// TODO: only necessary if there's a roll/pitch offset between VIO and EKF
@@ -65,7 +65,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 			const Dcmf R_ev_to_ekf(q_error);
 
 			pos = R_ev_to_ekf * ev_sample.pos;
-			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.posVar) * R_ev_to_ekf.transpose();
+			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
 
 			// increase minimum variance to include EV orientation variance
 			// TODO: do this properly
@@ -90,6 +90,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 					   math::max(_params.ev_pos_innov_gate, 1.f),
 					   aid_src);
 
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_hgt_innov_lpf.update(aid_src.innovation);
+	}
+
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
 	if (measurement_valid && quality_sufficient) {
@@ -98,9 +103,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 		bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
 	}
 
-	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS)) && measurement_valid;
+	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
+			&& measurement_valid;
 
-	const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing;
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing;
 
 	if (_control_status.flags.ev_hgt) {
 		aid_src.fusion_enabled = true;

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -0,0 +1,309 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ev_pos_control.cpp
+ * Control functions for ekf external vision position fusion
+ */
+
+#include "ekf.h"
+
+static constexpr const char *EV_AID_SRC_NAME = "EV position";
+
+
+void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
+			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src)
+{
+	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
+					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
+
+	// determine if we should use EV position aiding
+	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
+					     && _control_status.flags.tilt_align
+					     && PX4_ISFINITE(ev_sample.pos(0))
+					     && PX4_ISFINITE(ev_sample.pos(1));
+
+	// correct position for offset relative to IMU
+	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+
+	const bool bias_fusion_was_active = _ev_pos_b_est.fusionActive();
+
+	// rotate measurement into correct earth frame if required
+	Vector3f pos{NAN, NAN, NAN};
+	Matrix3f pos_cov{};
+
+	switch (ev_sample.pos_frame) {
+	case PositionFrame::LOCAL_FRAME_NED:
+		if (_control_status.flags.yaw_align) {
+			pos = ev_sample.pos - pos_offset_earth;
+			pos_cov = matrix::diag(ev_sample.position_var);
+
+			if (_control_status.flags.gps) {
+				_ev_pos_b_est.setFusionActive();
+
+			} else {
+				_ev_pos_b_est.setFusionInactive();
+			}
+
+		} else {
+			continuing_conditions_passing = false;
+			_ev_pos_b_est.setFusionInactive();
+			_ev_pos_b_est.reset();
+		}
+
+		break;
+
+	case PositionFrame::LOCAL_FRAME_FRD:
+		if (_control_status.flags.ev_yaw) {
+			// using EV frame
+			pos = ev_sample.pos - pos_offset_earth;
+			pos_cov = matrix::diag(ev_sample.position_var);
+
+			_ev_pos_b_est.setFusionInactive();
+			_ev_pos_b_est.reset();
+
+		} else {
+			// rotate EV to the EKF reference frame
+			const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
+			const Dcmf R_ev_to_ekf = Dcmf(q_error);
+
+			pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth;
+			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
+
+			// increase minimum variance to include EV orientation variance
+			// TODO: do this properly
+			const float orientation_var_max = ev_sample.orientation_var.max();
+
+			for (int i = 0; i < 2; i++) {
+				pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
+			}
+
+			if (_control_status.flags.gps) {
+				_ev_pos_b_est.setFusionActive();
+
+			} else {
+				_ev_pos_b_est.setFusionInactive();
+			}
+		}
+
+		break;
+
+	default:
+		continuing_conditions_passing = false;
+		_ev_pos_b_est.setFusionInactive();
+		_ev_pos_b_est.reset();
+		break;
+	}
+
+	// increase minimum variance if GPS active (position reference)
+	if (_control_status.flags.gps) {
+		for (int i = 0; i < 2; i++) {
+			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
+		}
+	}
+
+	const Vector2f measurement{pos(0), pos(1)};
+
+	const Vector2f measurement_var{
+		math::max(pos_cov(0, 0), sq(_params.ev_pos_noise), sq(0.01f)),
+		math::max(pos_cov(1, 1), sq(_params.ev_pos_noise), sq(0.01f))
+	};
+
+	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
+
+	// bias fusion activated (GPS activated)
+	if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
+		if (quality_sufficient) {
+			// reset the bias estimator
+			_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+
+		} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
+			// otherwise stop EV position, when quality is good again it will restart with reset bias
+			stopEvPosFusion();
+		}
+	}
+
+	updateHorizontalPositionAidSrcStatus(ev_sample.time_us,
+					     measurement - _ev_pos_b_est.getBias(),        // observation
+					     measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance
+					     math::max(_params.ev_pos_innov_gate, 1.f),    // innovation gate
+					     aid_src);
+
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_pos_innov_lpf.update(Vector2f(aid_src.innovation));
+	}
+
+	// update the bias estimator before updating the main filter but after
+	// using its current state to compute the vertical position innovation
+	if (measurement_valid && quality_sufficient) {
+		_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
+		_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
+		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(P(7, 7), P(8, 8)));
+	}
+
+	if (!measurement_valid) {
+		continuing_conditions_passing = false;
+	}
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing;
+
+	if (_control_status.flags.ev_pos) {
+		aid_src.fusion_enabled = true;
+
+		if (continuing_conditions_passing) {
+			const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive());
+			const bool reset = ev_reset || yaw_alignment_changed || bias_estimator_change;
+
+			updateEvPosFusion(measurement, measurement_var, quality_sufficient, reset, aid_src);
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", EV_AID_SRC_NAME);
+			stopEvPosFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			startEvPosFusion(measurement, measurement_var, aid_src);
+		}
+	}
+}
+
+void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src)
+{
+	// activate fusion
+	// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
+	if (_control_status.flags.gps) {
+		ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
+		_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+		_ev_pos_b_est.setFusionActive();
+
+	} else {
+		ECL_INFO("starting %s fusion, resetting state", EV_AID_SRC_NAME);
+		//_position_sensor_ref = PositionSensor::EV;
+		_information_events.flags.reset_pos_to_vision = true;
+		resetHorizontalPositionTo(measurement, measurement_var);
+		_ev_pos_b_est.reset();
+	}
+
+	aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+	_nb_ev_pos_reset_available = 5;
+	_information_events.flags.starting_vision_pos_fusion = true;
+	_control_status.flags.ev_pos = true;
+}
+
+void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src)
+{
+	if (reset) {
+
+		if (quality_sufficient) {
+
+			if (!_control_status.flags.gps) {
+				ECL_INFO("reset to %s", EV_AID_SRC_NAME);
+				_information_events.flags.reset_pos_to_vision = true;
+				resetHorizontalPositionTo(measurement, measurement_var);
+				_ev_pos_b_est.reset();
+
+			} else {
+				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+			}
+
+			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+		} else {
+			// EV has reset, but quality isn't sufficient
+			// we have no choice but to stop EV and try to resume once quality is acceptable
+			stopEvPosFusion();
+			return;
+		}
+
+	} else if (quality_sufficient) {
+		fuseHorizontalPosition(aid_src);
+
+	} else {
+		aid_src.innovation_rejected = true;
+	}
+
+	const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second
+
+	if (is_fusion_failing) {
+		bool pos_xy_fusion_failing = isTimedOut(_time_last_hor_pos_fuse, _params.no_aid_timeout_max);
+
+		if ((_nb_ev_pos_reset_available > 0) && quality_sufficient) {
+			// Data seems good, attempt a reset
+			ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
+
+			if (_control_status.flags.gps && !pos_xy_fusion_failing) {
+				// reset EV position bias
+				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+
+			} else {
+				_information_events.flags.reset_pos_to_vision = true;
+
+				if (_control_status.flags.gps) {
+					resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
+					_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+
+				} else {
+					resetHorizontalPositionTo(measurement, measurement_var);
+					_ev_pos_b_est.reset();
+				}
+			}
+
+			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+			if (_control_status.flags.in_air) {
+				_nb_ev_pos_reset_available--;
+			}
+
+		} else {
+			// A reset did not fix the issue but all the starting checks are not passing
+			// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+			ECL_WARN("stopping %s, fusion failing", EV_AID_SRC_NAME);
+			stopEvPosFusion();
+		}
+	}
+}
+
+void Ekf::stopEvPosFusion()
+{
+	if (_control_status.flags.ev_pos) {
+		resetEstimatorAidStatus(_aid_src_ev_pos);
+
+		_control_status.flags.ev_pos = false;
+	}
+}

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -130,11 +130,17 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 				   math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate
 				   aid_src);
 
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_vel_innov_lpf.update(Vector3f(aid_src.innovation));
+	}
+
 	if (!measurement_valid) {
 		continuing_conditions_passing = false;
 	}
 
-	const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing
 			&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
 
 	if (_control_status.flags.ev_vel) {

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -0,0 +1,191 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ev_yaw_control.cpp
+ * Control functions for ekf external vision yaw fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
+			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "EV yaw";
+
+	resetEstimatorAidStatus(aid_src);
+	aid_src.timestamp_sample = ev_sample.time_us;
+	aid_src.observation = getEulerYaw(ev_sample.quat);
+	aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
+	aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
+
+	// determine if we should use EV yaw aiding
+	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
+					     && _control_status.flags.tilt_align
+					     && !_inhibit_ev_yaw_use
+					     && PX4_ISFINITE(aid_src.observation)
+					     && PX4_ISFINITE(aid_src.observation_variance);
+
+	// if GPS enabled don't allow EV yaw if EV isn't NED
+	if (_control_status.flags.gps && _control_status.flags.yaw_align
+	    && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
+	   ) {
+		continuing_conditions_passing = false;
+
+		// use delta yaw for innovation logging
+		aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev)
+					     - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat)));
+	}
+
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_yaw_innov_lpf.update(aid_src.innovation);
+	}
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing
+			&& isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6);
+
+	if (_control_status.flags.ev_yaw) {
+		aid_src.fusion_enabled = true;
+
+		if (continuing_conditions_passing) {
+
+			if (ev_reset) {
+
+				if (quality_sufficient) {
+					ECL_INFO("reset to %s", AID_SRC_NAME);
+					//_information_events.flags.reset_yaw_to_vision = true; // TODO
+					resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
+					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+				} else {
+					// EV has reset, but quality isn't sufficient
+					// we have no choice but to stop EV and try to resume once quality is acceptable
+					stopEvYawFusion();
+					return;
+				}
+
+			} else if (quality_sufficient) {
+				fuseYaw(aid_src.innovation, aid_src.observation_variance, aid_src);
+
+			} else {
+				aid_src.innovation_rejected = true;
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max);
+
+			if (is_fusion_failing) {
+				if ((_nb_ev_yaw_reset_available > 0) && quality_sufficient) {
+					// Data seems good, attempt a reset
+					//_information_events.flags.reset_yaw_to_vision = true; // TODO
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetQuatStateYaw(aid_src.innovation, aid_src.observation_variance);
+					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_ev_yaw_reset_available--;
+					}
+
+				} else if (starting_conditions_passing) {
+					// Data seems good, but previous reset did not fix the issue
+					// something else must be wrong, declare the sensor faulty and stop the fusion
+					//_control_status.flags.ev_yaw_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopEvYawFusion();
+
+				} else {
+					// A reset did not fix the issue but all the starting checks are not passing
+					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					stopEvYawFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
+			stopEvYawFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			// activate fusion
+			if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) {
+
+				if (_control_status.flags.yaw_align) {
+					ECL_INFO("starting %s fusion", AID_SRC_NAME);
+
+				} else {
+					// reset yaw to EV and set yaw_align
+					ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+					resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
+					_control_status.flags.yaw_align = true;
+				}
+
+				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				_information_events.flags.starting_vision_yaw_fusion = true;
+				_control_status.flags.ev_yaw = true;
+
+			} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
+				// turn on fusion of external vision yaw measurements and disable all other heading fusion
+				stopMagFusion();
+				stopGpsYawFusion();
+				stopGpsFusion();
+
+				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+
+				// reset yaw to EV
+				resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
+				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+				_information_events.flags.starting_vision_yaw_fusion = true;
+				_control_status.flags.yaw_align = false;
+				_control_status.flags.ev_yaw = true;
+			}
+
+			if (_control_status.flags.ev_yaw) {
+				_nb_ev_yaw_reset_available = 5;
+			}
+		}
+	}
+}
+
+void Ekf::stopEvYawFusion()
+{
+	if (_control_status.flags.ev_yaw) {
+		resetEstimatorAidStatus(_aid_src_ev_yaw);
+
+		_control_status.flags.ev_yaw = false;
+	}
+}

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -95,7 +95,6 @@ void Ekf::controlFakePosFusion()
 			if (starting_conditions_passing) {
 				ECL_INFO("start fake position fusion");
 				_control_status.flags.fake_pos = true;
-				_fuse_hpos_as_odom = false; // TODO: needed?
 				resetFakePosFusion();
 
 				if (_control_status.flags.tilt_align) {

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -74,6 +74,11 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 						   innov_gate,
 						   aid_src);
 
+		// filtered innovation for preflight checks
+		if (!aid_src.innovation_rejected) {
+			_gnss_hgt_innov_lpf.update(aid_src.innovation);
+		}
+
 		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
 		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -57,16 +57,23 @@ void Ekf::controlGpsFusion()
 		controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
 
 		// GNSS velocity
+		const Vector3f velocity{gps_sample.vel};
 		const float vel_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
 		const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
 		updateVelocityAidSrcStatus(gps_sample.time_us,
-					   gps_sample.vel,                                             // observation
+					   velocity,                                                   // observation
 					   vel_obs_var,                                                // observation variance
 					   math::max(_params.gps_vel_innov_gate, 1.f),                 // innovation gate
 					   _aid_src_gnss_vel);
 		_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
 
+		// filtered innovation for preflight checks
+		if (!_aid_src_gnss_vel.innovation_rejected) {
+			_gnss_vel_innov_lpf.update(Vector3f(_aid_src_gnss_vel.innovation));
+		}
+
 		// GNSS position
+		const Vector2f position{gps_sample.pos};
 		// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
 		float pos_noise = math::max(gps_sample.hacc, _params.gps_pos_noise);
 
@@ -81,12 +88,39 @@ void Ekf::controlGpsFusion()
 		const float pos_var = sq(pos_noise);
 		const Vector2f pos_obs_var(pos_var, pos_var);
 		updateHorizontalPositionAidSrcStatus(gps_sample.time_us,
-						     gps_sample.pos,                             // observation
+						     position,                                   // observation
 						     pos_obs_var,                                // observation variance
 						     math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
 						     _aid_src_gnss_pos);
 		_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
 
+		// filtered innovation for preflight checks
+		if (!_aid_src_gnss_pos.innovation_rejected) {
+			_gnss_pos_innov_lpf.update(Vector2f(_aid_src_gnss_pos.innovation));
+		}
+
+		// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start
+		if (_control_status.flags.tilt_align && _NED_origin_initialised
+		    && gps_checks_passing && !gps_checks_failing) {
+
+			if (!_control_status.flags.yaw_align) {
+				if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align) {
+
+					// give mag a chance to start and yaw align if currently blocked by EV yaw
+					const bool mag_enabled = (_params.mag_fusion_type <= MagFuseType::MAG_3D);
+					const bool mag_available = (_mag_counter != 0);
+
+					if (mag_enabled && mag_available
+					    && !_control_status.flags.mag_field_disturbed
+					    && !_control_status.flags.mag_fault) {
+
+						stopEvYawFusion();
+					}
+				}
+			}
+		}
+
+
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
 		const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
@@ -134,12 +168,6 @@ void Ekf::controlGpsFusion()
 					stopGpsFusion();
 					_warning_events.flags.gps_quality_poor = true;
 					ECL_WARN("GPS quality poor - stopping use");
-
-					// TODO: move this to EV control logic
-					// Reset position state to external vision if we are going to use absolute values
-					if (_control_status.flags.ev_pos && !(_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS)) {
-						resetHorizontalPositionToVision();
-					}
 				}
 
 			} else { // mandatory conditions are not passing
@@ -150,44 +178,48 @@ void Ekf::controlGpsFusion()
 			if (starting_conditions_passing) {
 				// Do not use external vision for yaw if using GPS because yaw needs to be
 				// defined relative to an NED reference frame
-				if (_control_status.flags.ev_yaw
-				    || _mag_inhibit_yaw_reset_req
-				    || _mag_yaw_reset_req) {
-
-					_mag_yaw_reset_req = true;
-
+				if (_control_status.flags.ev_yaw) {
 					// Stop the vision for yaw fusion and do not allow it to start again
 					stopEvYawFusion();
 					_inhibit_ev_yaw_use = true;
-
-				} else {
-					ECL_INFO("starting GPS fusion");
-					_information_events.flags.starting_gps_fusion = true;
-
-					// reset position
-					_information_events.flags.reset_pos_to_gps = true;
-					resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
-
-					// when already using another velocity source velocity reset is not necessary
-					if (!isHorizontalAidingActive()) {
-						_information_events.flags.reset_vel_to_gps = true;
-						resetVelocityTo(gps_sample.vel, vel_obs_var);
-					}
-
-					_control_status.flags.gps = true;
 				}
 
+				ECL_INFO("starting GPS fusion");
+				_information_events.flags.starting_gps_fusion = true;
+
+				// when already using another velocity source velocity reset is not necessary
+				if (!isHorizontalAidingActive()
+				    || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+				    || !_control_status_prev.flags.yaw_align
+				   ) {
+					// reset velocity
+					_information_events.flags.reset_vel_to_gps = true;
+					resetVelocityTo(velocity, vel_obs_var);
+					_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
+				}
+
+				// reset position
+				_information_events.flags.reset_pos_to_gps = true;
+				resetHorizontalPositionTo(position, pos_obs_var);
+				_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
+
+				_control_status.flags.gps = true;
+
 			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
 				// If no mag is used, align using the yaw estimator (if available)
 				if (resetYawToEKFGSF()) {
 					_information_events.flags.yaw_aligned_to_imu_gps = true;
-					ECL_INFO("Yaw aligned using IMU and GPS");
+					ECL_INFO("GPS yaw aligned using IMU, resetting vel and pos");
 
-					ECL_INFO("reset velocity and position to GPS");
+					// reset velocity
 					_information_events.flags.reset_vel_to_gps = true;
+					resetVelocityTo(velocity, vel_obs_var);
+					_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
+
+					// reset position
 					_information_events.flags.reset_pos_to_gps = true;
-					resetVelocityTo(gps_sample.vel, vel_obs_var);
-					resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
+					resetHorizontalPositionTo(position, pos_obs_var);
+					_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
 				}
 			}
 		}

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -139,6 +139,9 @@ void Ekf::fuseGpsYaw()
 	if (is_fused) {
 		_time_last_heading_fuse = _imu_sample_delayed.time_us;
 		gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
+
+		// filtered innovation for preflight checks
+		_gnss_yaw_innov_lpf.update(gnss_yaw.innovation);
 	}
 }
 

src/modules/ekf2/EKF/mag_control.cpp

@@ -218,6 +218,12 @@ bool Ekf::canResetMagHeading() const
 
 void Ekf::runInAirYawReset()
 {
+	// prevent a reset being performed more than once on the same frame
+	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us)
+	    || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
+		return;
+	}
+
 	if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
 		bool has_realigned_yaw = false;
 
@@ -252,6 +258,8 @@ void Ekf::runInAirYawReset()
 				);
 
 			resetMagCov();
+
+			has_realigned_yaw = true;
 		}
 
 		if (!has_realigned_yaw && canResetMagHeading()) {
@@ -296,14 +304,19 @@ void Ekf::check3DMagFusionSuitability()
 
 void Ekf::checkYawAngleObservability()
 {
-	// Check if there has been enough change in horizontal velocity to make yaw observable
-	// Apply hysteresis to check to avoid rapid toggling
-	_yaw_angle_observable = _yaw_angle_observable
-				? _accel_lpf_NE.norm() > _params.mag_acc_gate
-				: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
+	if (_control_status.flags.gps) {
+		// Check if there has been enough change in horizontal velocity to make yaw observable
+		// Apply hysteresis to check to avoid rapid toggling
+		if (_yaw_angle_observable) {
+			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
 
-	_yaw_angle_observable = _yaw_angle_observable
-				&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
+		} else {
+			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
+		}
+
+	} else {
+		_yaw_angle_observable = false;
+	}
 }
 
 void Ekf::checkMagBiasObservability()
@@ -366,9 +379,7 @@ bool Ekf::shouldInhibitMag() const
 	// has explicitly stopped magnetometer use.
 	const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
 
-	const bool heading_not_required_for_navigation = !_control_status.flags.gps
-			&& !_control_status.flags.ev_pos
-			&& !_control_status.flags.ev_vel;
+	const bool heading_not_required_for_navigation = !_control_status.flags.gps;
 
 	return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
 }
@@ -419,6 +430,11 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 		_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
 
 		fuseYaw(innovation, obs_var, _aid_src_mag_heading);
+
+		// filtered innovation for preflight checks
+		if (!_aid_src_mag_heading.innovation_rejected) {
+			_mag_heading_innov_lpf.update(_aid_src_mag_heading.innovation);
+		}
 	}
 }
 

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -115,6 +115,9 @@ void Ekf::fuseOptFlow()
 		return;
 	}
 
+	// filtered optical flow velocity innovation (for preflight checks)
+	_optical_flow_vel_innov_lpf.update(Vector2f(_state.vel(0) - _flow_vel_ne(0), _state.vel(1) - _flow_vel_ne(1)));
+
 	bool fused[2] {false, false};
 
 	// fuse observation axes sequentially

src/modules/ekf2/EKF/position_bias_estimator.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_bias_estimator.hpp
+ */
+
+#pragma once
+
+#include "bias_estimator.hpp"
+
+class PositionBiasEstimator
+{
+public:
+	PositionBiasEstimator(uint8_t sensor, const uint8_t &sensor_ref):
+		_sensor(sensor),
+		_sensor_ref(sensor_ref)
+	{}
+	virtual ~PositionBiasEstimator() = default;
+
+	bool fusionActive() const { return _is_sensor_fusion_active; }
+
+	void setFusionActive() { _is_sensor_fusion_active = true; }
+	void setFusionInactive() { _is_sensor_fusion_active = false; }
+
+	void predict(float dt)
+	{
+		if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) {
+			_bias[0].predict(dt);
+			_bias[1].predict(dt);
+		}
+	}
+
+	void fuseBias(Vector2f bias, Vector2f bias_var)
+	{
+		if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) {
+			_bias[0].fuseBias(bias(0), bias_var(0));
+			_bias[1].fuseBias(bias(1), bias_var(1));
+		}
+	}
+
+	void setBias(const Vector2f &bias)
+	{
+		_bias[0].setBias(bias(0));
+		_bias[1].setBias(bias(1));
+	}
+
+	void setProcessNoiseSpectralDensity(float nsd)
+	{
+		_bias[0].setProcessNoiseSpectralDensity(nsd);
+		_bias[1].setProcessNoiseSpectralDensity(nsd);
+	}
+	// void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; }
+	// void setInnovGate(float gate_size) { _gate_size = gate_size; }
+
+	void setMaxStateNoise(const Vector2f &max_noise)
+	{
+		_bias[0].setMaxStateNoise(max_noise(0));
+		_bias[1].setMaxStateNoise(max_noise(1));
+	}
+
+	Vector2f getBias() const { return Vector2f(_bias[0].getBias(), _bias[1].getBias()); }
+	float getBias(int index) const { return _bias[index].getBias(); }
+
+	Vector2f getBiasVar() const { return Vector2f(_bias[0].getBiasVar(), _bias[1].getBiasVar()); }
+	float getBiasVar(int index) const { return _bias[index].getBiasVar(); }
+
+	const BiasEstimator::status &getStatus(int index) const { return _bias[index].getStatus(); }
+
+	void reset()
+	{
+		_bias[0].reset();
+		_bias[1].reset();
+	}
+
+private:
+	BiasEstimator _bias[2] {};
+
+	const uint8_t _sensor;
+	const uint8_t &_sensor_ref;
+
+	bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool
+};

src/modules/ekf2/EKF/range_height_control.cpp

@@ -63,6 +63,11 @@ void Ekf::controlRangeHeightFusion()
 						   innov_gate,
 						   aid_src);
 
+		// filtered innovation for preflight checks
+		if (!aid_src.innovation_rejected) {
+			_rng_hgt_innov_lpf.update(aid_src.innovation);
+		}
+
 		// update the bias estimator before updating the main filter but after
 		// using its current state to compute the vertical position innovation
 		if (measurement_valid && _range_sensor.isDataHealthy()) {

src/modules/ekf2/EKF2.cpp

@@ -207,23 +207,67 @@ bool EKF2::multi_init(int imu, int mag)
 {
 	// advertise all topics to ensure consistent uORB instance numbering
 	_ekf2_timestamps_pub.advertise();
-	_estimator_baro_bias_pub.advertise();
-	_estimator_ev_hgt_bias_pub.advertise();
 	_estimator_event_flags_pub.advertise();
-	_estimator_gnss_hgt_bias_pub.advertise();
-	_estimator_gps_status_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
 	_estimator_innovations_pub.advertise();
 	_estimator_optical_flow_vel_pub.advertise();
-	_estimator_rng_hgt_bias_pub.advertise();
 	_estimator_sensor_bias_pub.advertise();
 	_estimator_states_pub.advertise();
 	_estimator_status_flags_pub.advertise();
 	_estimator_status_pub.advertise();
-	_estimator_visual_odometry_aligned_pub.advertise();
 	_yaw_est_pub.advertise();
 
+	// baro advertise
+	if (_param_ekf2_baro_ctrl.get()) {
+		_estimator_aid_src_baro_hgt_pub.advertise();
+		_estimator_baro_bias_pub.advertise();
+	}
+
+	// EV advertise
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS)) {
+		_estimator_aid_src_ev_hgt_pub.advertise();
+		_estimator_ev_pos_bias_pub.advertise();
+	}
+
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::HPOS)) {
+		_estimator_aid_src_ev_pos_pub.advertise();
+		_estimator_ev_pos_bias_pub.advertise();
+	}
+
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VEL)) {
+		_estimator_aid_src_ev_vel_pub.advertise();
+	}
+
+	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
+		_estimator_aid_src_ev_yaw_pub.advertise();
+	}
+
+	// GNSS advertise
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
+		_estimator_aid_src_gnss_hgt_pub.advertise();
+		_estimator_gnss_hgt_bias_pub.advertise();
+	}
+
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::HPOS)) {
+		_estimator_aid_src_gnss_pos_pub.advertise();
+		_estimator_gps_status_pub.advertise();
+	}
+
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VEL)) {
+		_estimator_aid_src_gnss_vel_pub.advertise();
+	}
+
+	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
+		_estimator_aid_src_gnss_yaw_pub.advertise();
+	}
+
+	// RNG advertise
+	if (_param_ekf2_rng_ctrl.get()) {
+		_estimator_aid_src_rng_hgt_pub.advertise();
+		_estimator_rng_hgt_bias_pub.advertise();
+	}
+
 	_attitude_pub.advertise();
 	_local_position_pub.advertise();
 	_global_position_pub.advertise();
@@ -545,15 +589,13 @@ void EKF2::Run()
 		UpdateAirspeedSample(ekf2_timestamps);
 		UpdateAuxVelSample(ekf2_timestamps);
 		UpdateBaroSample(ekf2_timestamps);
+		UpdateExtVisionSample(ekf2_timestamps);
 		UpdateFlowSample(ekf2_timestamps);
 		UpdateGpsSample(ekf2_timestamps);
 		UpdateMagSample(ekf2_timestamps);
 		UpdateRangeSample(ekf2_timestamps);
 		UpdateSystemFlagsSample(ekf2_timestamps);
 
-		vehicle_odometry_s ev_odom;
-		const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
-
 		// run the EKF update and output
 		const hrt_abstime ekf_update_start = hrt_absolute_time();
 
@@ -569,7 +611,7 @@ void EKF2::Run()
 			PublishBaroBias(now);
 			PublishGnssHgtBias(now);
 			PublishRngHgtBias(now);
-			PublishEvHgtBias(now);
+			PublishEvPosBias(now);
 			PublishEventFlags(now);
 			PublishGpsStatus(now);
 			PublishInnovations(now);
@@ -593,11 +635,6 @@ void EKF2::Run()
 			perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
 		}
 
-		// publish external visual odometry after fixed frame alignment if new odometry is received
-		if (new_ev_odom) {
-			PublishOdometryAligned(now, ev_odom);
-		}
-
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
@@ -666,12 +703,32 @@ void EKF2::VerifyParams()
 	}
 
 	// EV EKF2_AID_MASK -> EKF2_EV_CTRL
-	if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) {
+	if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)
+	    || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)
+	    || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)
+	   ) {
 
-		_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VEL));
-		_param_ekf2_ev_ctrl.commit();
+		// EKF2_EV_CTRL set VEL bit
+		if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) {
+			_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VEL));
+		}
+
+		// EKF2_EV_CTRL set HPOS/VPOS bits
+		if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)) {
+			_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get()
+						| static_cast<int32_t>(EvCtrl::HPOS) | static_cast<int32_t>(EvCtrl::VPOS));
+		}
+
+		// EKF2_EV_CTRL set YAW bit
+		if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)) {
+			_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::YAW));
+		}
 
 		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL));
+		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS));
+		_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW));
+
+		_param_ekf2_ev_ctrl.commit();
 		_param_ekf2_aid_mask.commit();
 
 		mavlink_log_critical(&_mavlink_log_pub, "EKF2 EV use EKF2_EV_CTRL instead of EKF2_AID_MASK\n");
@@ -787,15 +844,35 @@ void EKF2::PublishRngHgtBias(const hrt_abstime &timestamp)
 	}
 }
 
-void EKF2::PublishEvHgtBias(const hrt_abstime &timestamp)
+void EKF2::PublishEvPosBias(const hrt_abstime &timestamp)
 {
-	if (_ekf.get_ev_sample_delayed().time_us != 0) {
-		const BiasEstimator::status &status = _ekf.getEvHgtBiasEstimatorStatus();
+	if (_ekf.aid_src_ev_hgt().timestamp_sample) {
 
-		if (fabsf(status.bias - _last_ev_hgt_bias_published) > 0.001f) {
-			_estimator_ev_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_ev_sample_delayed().time_us, timestamp));
+		estimator_bias3d_s bias{};
 
-			_last_ev_hgt_bias_published = status.bias;
+		// height
+		BiasEstimator::status bias_est_status[3];
+		bias_est_status[0] = _ekf.getEvPosBiasEstimatorStatus(0);
+		bias_est_status[1] = _ekf.getEvPosBiasEstimatorStatus(1);
+		bias_est_status[2] = _ekf.getEvHgtBiasEstimatorStatus();
+
+		for (int i = 0; i < 3; i++) {
+			bias.bias[i] = bias_est_status[i].bias;
+			bias.bias_var[i] = bias_est_status[i].bias_var;
+
+			bias.innov[i] = bias_est_status[i].innov;
+			bias.innov_var[i] = bias_est_status[i].innov_var;
+			bias.innov_test_ratio[i] = bias_est_status[i].innov_test_ratio;
+		}
+
+		const Vector3f bias_vec{bias.bias};
+
+		if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) {
+			bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample;
+			bias.timestamp = hrt_absolute_time();
+			_estimator_ev_pos_bias_pub.publish(bias);
+
+			_last_ev_bias_published = Vector3f(bias.bias);
 		}
 	}
 }
@@ -990,29 +1067,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 
 	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
-
-	// calculate noise filtered velocity innovations which are used for pre-flight checking
-	if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) {
-		// fully reset on takeoff or landing
-		_preflt_checker.reset();
-	}
-
-	if (!_ekf.control_status_flags().in_air) {
-		// TODO: move to run before publications
-		_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
-		_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
-		_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.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
-
-		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
-	}
 }
 
 void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
@@ -1210,43 +1264,6 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp)
 	_odometry_pub.publish(odom);
 }
 
-void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom)
-{
-	const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame
-	const Dcmf ev_rot_mat(quat_ev2ekf);
-
-	vehicle_odometry_s aligned_ev_odom{ev_odom};
-
-	// Rotate external position and velocity into EKF navigation frame
-	const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.position);
-	aligned_pos.copyTo(aligned_ev_odom.position);
-
-	switch (ev_odom.velocity_frame) {
-	case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: {
-			const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.velocity);
-			aligned_vel.copyTo(aligned_ev_odom.velocity);
-			break;
-		}
-
-	case vehicle_odometry_s::VELOCITY_FRAME_FRD: {
-			const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.velocity);
-			aligned_vel.copyTo(aligned_ev_odom.velocity);
-			break;
-		}
-	}
-
-	aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
-
-	// Compute orientation in EKF navigation frame
-	Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q);
-	ev_quat_aligned.normalize();
-
-	ev_quat_aligned.copyTo(aligned_ev_odom.q);
-
-	aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-	_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
-}
-
 void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 {
 	// estimator_sensor_bias
@@ -1349,12 +1366,6 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 
 	status.time_slip = _last_time_slip_us * 1e-6f;
 
-	status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
-	status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
-	status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
-	status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
-	status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
-
 	status.accel_device_id = _device_id_accel;
 	status.baro_device_id = _device_id_baro;
 	status.gyro_device_id = _device_id_gyro;
@@ -1390,6 +1401,30 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		_innov_check_fail_status_changes++;
 	}
 
+	if (!_ekf.control_status_flags().in_air) {
+
+		bool fail_vel_horiz = (_ekf.filteredHorizontalVelocityInnovation() > kVelocityInnovationTestLimit);
+		bool fail_vel_vert  = (_ekf.filteredVerticalVelocityInnovation() > kVelocityInnovationTestLimit);
+		bool fail_pos_horiz = (_ekf.filteredHorizontalPositionInnovation() > kPositionInnovationTestLimit);
+		bool fail_height    = (_ekf.filteredVerticalPositionInnovation() > kPositionInnovationTestLimit);
+		bool fail_heading   = (_ekf.filteredHeadingInnovation() > kHeadingInnovationTestLimit);
+
+		if (fail_vel_horiz    != _pre_flt_fail_innov_vel_horiz
+		    || fail_vel_vert  != _pre_flt_fail_innov_vel_vert
+		    || fail_pos_horiz != _pre_flt_fail_innov_pos_horiz
+		    || fail_height    != _pre_flt_fail_innov_height
+		    || fail_heading   != _pre_flt_fail_innov_heading
+		   ) {
+			_pre_flt_fail_innov_vel_horiz = fail_vel_horiz;
+			_pre_flt_fail_innov_vel_vert  = fail_vel_vert;
+			_pre_flt_fail_innov_pos_horiz = fail_pos_horiz;
+			_pre_flt_fail_innov_height    = fail_height;
+			_pre_flt_fail_innov_heading   = fail_heading;
+
+			update = true;
+		}
+	}
+
 	if (update) {
 		estimator_status_flags_s status_flags{};
 		status_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
@@ -1462,6 +1497,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_optflow_x                = _ekf.innov_check_fail_status_flags().reject_optflow_X;
 		status_flags.reject_optflow_y                = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
 
+		status_flags.pre_flt_fail_innov_vel_horiz = _pre_flt_fail_innov_vel_horiz;
+		status_flags.pre_flt_fail_innov_vel_vert  = _pre_flt_fail_innov_vel_vert;
+		status_flags.pre_flt_fail_innov_pos_horiz = _pre_flt_fail_innov_pos_horiz;
+		status_flags.pre_flt_fail_innov_height    = _pre_flt_fail_innov_height;
+		status_flags.pre_flt_fail_innov_heading   = _pre_flt_fail_innov_heading;
+
 		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
 
@@ -1700,12 +1741,14 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
-bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
+bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF external vision sample
 	bool new_ev_odom = false;
 	const unsigned last_generation = _ev_odom_sub.get_last_generation();
 
+	vehicle_odometry_s ev_odom;
+
 	if (_ev_odom_sub.update(&ev_odom)) {
 		if (_msg_missed_odometry_perf == nullptr) {
 			_msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed");
@@ -1764,38 +1807,38 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		}
 
 		// check for valid position data
-		if (Vector3f(ev_odom.position).isAllFinite()) {
-			bool position_valid = true;
+		const Vector3f ev_odom_pos(ev_odom.position);
+		const Vector3f ev_odom_pos_var(ev_odom.position_variance);
 
-			// switch (ev_odom.pose_frame) {
-			// case vehicle_odometry_s::POSE_FRAME_NED:
-			// 	ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
-			// 	break;
+		if (ev_odom_pos.isAllFinite()) {
+			bool position_frame_valid = false;
 
-			// case vehicle_odometry_s::POSE_FRAME_FRD:
-			// 	ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
-			// 	break;
+			switch (ev_odom.pose_frame) {
+			case vehicle_odometry_s::POSE_FRAME_NED:
+				ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
+				position_frame_valid = true;
+				break;
 
-			// default:
-			// 	position_valid = false;
-			// 	break;
-			// }
+			case vehicle_odometry_s::POSE_FRAME_FRD:
+				ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
+				position_frame_valid = true;
+				break;
+			}
 
-			if (position_valid) {
-				ev_data.pos(0) = ev_odom.position[0];
-				ev_data.pos(1) = ev_odom.position[1];
-				ev_data.pos(2) = ev_odom.position[2];
+			if (position_frame_valid) {
+				ev_data.pos = ev_odom_pos;
 
 				const float evp_noise_var = sq(_param_ekf2_evp_noise.get());
 
 				// position measurement error from ev_data or parameters
-				if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.position_variance).isAllFinite()) {
-					ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]);
-					ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]);
-					ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]);
+				if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_pos_var.isAllFinite()) {
+
+					ev_data.position_var(0) = fmaxf(evp_noise_var, ev_odom_pos_var(0));
+					ev_data.position_var(1) = fmaxf(evp_noise_var, ev_odom_pos_var(1));
+					ev_data.position_var(2) = fmaxf(evp_noise_var, ev_odom_pos_var(2));
 
 				} else {
-					ev_data.posVar.setAll(evp_noise_var);
+					ev_data.position_var.setAll(evp_noise_var);
 				}
 
 				new_ev_odom = true;

src/modules/ekf2/EKF2.hpp

@@ -42,7 +42,6 @@
 #define EKF2_HPP
 
 #include "EKF/ekf.h"
-#include "Utility/PreFlightChecker.hpp"
 
 #include "EKF2Selector.hpp"
 
@@ -69,6 +68,7 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/estimator_bias.h>
+#include <uORB/topics/estimator_bias3d.h>
 #include <uORB/topics/estimator_event_flags.h>
 #include <uORB/topics/estimator_gps_status.h>
 #include <uORB/topics/estimator_innovations.h>
@@ -142,7 +142,7 @@ private:
 	void PublishBaroBias(const hrt_abstime &timestamp);
 	void PublishGnssHgtBias(const hrt_abstime &timestamp);
 	void PublishRngHgtBias(const hrt_abstime &timestamp);
-	void PublishEvHgtBias(const hrt_abstime &timestamp);
+	void PublishEvPosBias(const hrt_abstime &timestamp);
 	estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
 					      uint64_t timestamp, uint32_t device_id = 0);
 	void PublishEventFlags(const hrt_abstime &timestamp);
@@ -165,7 +165,7 @@ private:
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
-	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
+	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
 	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -220,6 +220,20 @@ private:
 	uint64_t _start_time_us = 0;		///< system time at EKF start (uSec)
 	int64_t _last_time_slip_us = 0;		///< Last time slip (uSec)
 
+	// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
+	static constexpr float kVelocityInnovationTestLimit = 0.5f;
+	// Maximum permissible position innovation to pass pre-flight checks (m)
+	static constexpr float kPositionInnovationTestLimit = 1.5f;
+	// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
+	static constexpr float kHeadingInnovationTestLimit = 0.5f;
+
+	// preflight failure checks
+	bool _pre_flt_fail_innov_heading{false};
+	bool _pre_flt_fail_innov_vel_horiz{false};
+	bool _pre_flt_fail_innov_vel_vert{false};
+	bool _pre_flt_fail_innov_pos_horiz{false};
+	bool _pre_flt_fail_innov_height{false};
+
 	perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
 	perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
 	perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
@@ -291,7 +305,7 @@ private:
 	float _last_baro_bias_published{};
 	float _last_gnss_hgt_bias_published{};
 	float _last_rng_hgt_bias_published{};
-	float _last_ev_hgt_bias_published{};
+	matrix::Vector3f _last_ev_bias_published{};
 
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
 	hrt_abstime _airspeed_validated_timestamp_last{0};
@@ -340,7 +354,7 @@ private:
 	uORB::PublicationMulti<estimator_bias_s>             _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
 	uORB::PublicationMulti<estimator_bias_s>             _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
 	uORB::PublicationMulti<estimator_bias_s>             _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)};
-	uORB::PublicationMulti<estimator_bias_s>             _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)};
+	uORB::PublicationMulti<estimator_bias3d_s>           _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
 	uORB::PublicationMultiData<estimator_event_flags_s>  _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
 	uORB::PublicationMulti<estimator_gps_status_s>       _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
@@ -350,7 +364,6 @@ private:
 	uORB::PublicationMulti<estimator_states_s>           _estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::PublicationMulti<estimator_status_flags_s>     _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
 	uORB::PublicationMulti<estimator_status_s>           _estimator_status_pub{ORB_ID(estimator_status)};
-	uORB::PublicationMulti<vehicle_odometry_s>           _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
 	uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
 	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
 
@@ -386,9 +399,6 @@ private:
 	uORB::PublicationMulti<vehicle_odometry_s>           _odometry_pub;
 	uORB::PublicationMulti<wind_s>              _wind_pub;
 
-
-	PreFlightChecker _preflt_checker;
-
 	Ekf _ekf;
 
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)