EV HGT bias est predict central

arm

- Previously, due to the way MAVSDK's `health_all_ok` was implemented,
vehicle often didn't have a valid global position estimate (although
function returned true), and it wouldn't arm, and the SITL would fail
- Also sometimes as vehicle didn't have manual control, it entered weird
states where it wasn't able to arm as well
- This adds a check to make sure vehicle is able to arm, directly from
the Health struct

.github/ISSUE_TEMPLATE/2_Feature_request.md

@@ -5,7 +5,7 @@ labels: feature-request
 
 ---
 
-For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Slack (you can find an invite link on this project README).
+For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Discord (you can find an invite link on this project README).
 
 ## Describe problem solved by the proposed feature
 A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...

.github/PULL_REQUEST_TEMPLATE.md

@@ -1,4 +1,4 @@
-Please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
+Please use [PX4 Discuss](http://discuss.px4.io/) or [Discord](https://discord.gg/dronecode) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
 
 ## Describe problem solved by this pull request
 A clear and concise description of the problem this proposed change will solve. Or, what it will improve.

Jenkinsfile

@@ -94,21 +94,22 @@ pipeline {
 
         stage('failsafe docs') {
           agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
+            docker { image 'px4io/px4-dev-nuttx-focal:2021-08-18' }
           }
           steps {
             sh '''#!/bin/bash -l
-			echo $0;
+            echo $0;
             git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk;
             cd _emscripten_sdk;
             ./emsdk install latest;
             ./emsdk activate latest;
+            cd ..;
             . ./_emscripten_sdk/emsdk_env.sh;
-			make failsafe_web;
-			cd build/px4_sitl_default_failsafe_web;
-			mkdir -p failsafe_sim;
-			cp index.* parameters.json failsafe_sim;
-			'''
+            make failsafe_web;
+            cd build/px4_sitl_default_failsafe_web;
+            mkdir -p failsafe_sim;
+            cp index.* parameters.json failsafe_sim;
+            '''
             dir('build/px4_sitl_default_failsafe_web') {
               archiveArtifacts(artifacts: 'failsafe_sim/*')
               stash includes: 'failsafe_sim/*', name: 'failsafe_sim'

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

@@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision

@@ -10,6 +10,7 @@
 # EKF2: Vision position and heading
 param set-default EKF2_AID_MASK 24
 param set-default EKF2_EV_DELAY 5
+param set-default EKF2_EV_CTRL 15
 param set-default EKF2_GPS_CTRL 0
 
 # LPE: Vision + baro

ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter

@@ -70,7 +70,7 @@ param set-default MPC_XY_VEL_D_ACC 0.1
 param set-default NAV_ACC_RAD 5
 
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.5
+param set-default VT_FW_DIFTHR_S_Y 0.5
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -22,7 +22,7 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -191,7 +191,10 @@ then
 	ist8308 -X -q start
 	ist8310 -X -q start
 	lis2mdl -X -q start
-	lis3mdl -X -q start
+	if ! lis3mdl -X -q start
+	then
+		lis3mdl -X -q -a 0x1c start
+	fi
 	qmc5883l -X -q start
 	rm3100 -X -q start
 

Tools/px_uploader.py

@@ -582,7 +582,35 @@ class uploader(object):
         self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
 
     # upload the firmware
-    def upload(self, fw, force=False, boot_delay=None):
+    def upload(self, fw_list, force=False, boot_delay=None, boot_check=False):
+        # select correct binary
+        found_suitable_firmware = False
+        for file in fw_list:
+            fw = firmware(file)
+            if self.board_type == fw.property('board_id'):
+                if len(fw_list) > 1: print("using firmware binary {}".format(file))
+                found_suitable_firmware = True
+                break
+
+        if not found_suitable_firmware:
+            msg = "Firmware not suitable for this board (Firmware board_type=%u board_id=%u)" % (
+                self.board_type, fw.property('board_id'))
+            print("WARNING: %s" % msg)
+            if force:
+                if len(fw_list) > 1:
+                    raise FirmwareNotSuitableException("force flashing failed, more than one file provided, none suitable")
+                print("FORCED WRITE, FLASHING ANYWAY!")
+            else:
+                raise FirmwareNotSuitableException(msg)
+
+        percent = fw.property('image_size') / fw.property('image_maxsize')
+        binary_size = float(fw.property('image_size'))
+        binary_max_size = float(fw.property('image_maxsize'))
+        percent = (binary_size / binary_max_size) * 100
+
+        print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%) " % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
+        print()
+
         # Make sure we are doing the right thing
         start = _time()
         if self.board_type != fw.property('board_id'):
@@ -764,7 +792,7 @@ def main():
     parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading')
     parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash')
     parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot')
-    parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
+    parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)")
     args = parser.parse_args()
 
     if args.use_protocol_splitter_format:
@@ -776,17 +804,7 @@ def main():
         print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)")
         print("==========================================================================================================")
 
-    # Load the firmware file
-    fw = firmware(args.firmware)
-
-    percent = fw.property('image_size') / fw.property('image_maxsize')
-    binary_size = float(fw.property('image_size'))
-    binary_max_size = float(fw.property('image_maxsize'))
-    percent = (binary_size / binary_max_size) * 100
-
-    print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%), waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
-    print()
-
+    print("Waiting for bootloader...")
     # tell any GCS that might be connected to the autopilot to give up
     # control of the serial port
 
@@ -889,7 +907,7 @@ def main():
 
                 try:
                     # ok, we have a bootloader, try flashing it
-                    up.upload(fw, force=args.force, boot_delay=args.boot_delay)
+                    up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay)
 
                     # if we made this far without raising exceptions, the upload was successful
                     successful = True

boards/modalai/voxl2/default.px4board

@@ -2,3 +2,4 @@ CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_MODULES_MUORB_APPS=y
+CONFIG_SYSTEMCMDS_PERF=y

boards/nxp/ucans32k146/src/board_config.h

@@ -99,14 +99,14 @@ __BEGIN_DECLS
 
 /* Timer I/O PWM and capture
  *
- * ?? PWM outputs are configured.
+ * 2 PWM outputs are configured.
  * ?? Timer inputs are configured.
  *
  * Pins:
  *      Defined in board.h
  */
 
-#define DIRECT_PWM_OUTPUT_CHANNELS  1
+#define DIRECT_PWM_OUTPUT_CHANNELS  2
 
 
 #define BOARD_HAS_LED_PWM              1

boards/nxp/ucans32k146/src/timer_config.cpp

@@ -105,11 +105,13 @@
 #define rPWMLOAD(t)    REG(t,S32K1XX_FTM_PWMLOAD_OFFSET)
 
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::FTM1),
 	initIOTimer(Timer::FTM2),
 };
 
 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::FTM2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::FTM1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin1}),
 };
 
 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -154,17 +156,26 @@ void ucans32k_timer_initialize(void)
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM0_SC) = regval;
 
+	regval = _REG(S32K1XX_FTM1_SC);
+	regval &= ~(FTM_SC_CLKS_MASK);
+	regval |= FTM_SC_CLKS_FTM;
+	_REG(S32K1XX_FTM1_SC) = regval;
+
 	regval = _REG(S32K1XX_FTM2_SC);
 	regval &= ~(FTM_SC_CLKS_MASK);
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM2_SC) = regval;
 
-	/* Enabled System Clock Gating Control for FTM0, and FTM2 */
+	/* Enabled System Clock Gating Control for FTM0, FTM1 and FTM2 */
 
 	regval = _REG(S32K1XX_PCC_FTM0);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM0) = regval;
 
+	regval = _REG(S32K1XX_PCC_FTM1);
+	regval |= PCC_CGC;
+	_REG(S32K1XX_PCC_FTM1) = regval;
+
 	regval = _REG(S32K1XX_PCC_FTM2);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM2) = regval;

boards/px4/fmu-v6x/default.px4board

@@ -74,6 +74,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=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

msg/ControlAllocatorStatus.msg

@@ -1,12 +1,10 @@
 uint64 timestamp                        # time since system start (microseconds)
 
 bool torque_setpoint_achieved           # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_torque             # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
                                         # Computed as: unallocated_torque = torque_setpoint - allocated_torque
 
 bool thrust_setpoint_achieved           # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_thrust             # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_thrust           # Unallocated thrust. Equal to 0 if the setpoint was achieved.
                                         # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust
 

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/VehicleAttitudeSetpoint.msg

@@ -13,7 +13,7 @@ float32[4] q_d			# Desired quaternion for quaternion control
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body		# Normalized thrust command in body NED frame [-1,1]
 
-bool reset_rate_integrals	# Reset roll/pitch/yaw integrals (navigation logic change)
+bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)
 
 bool fw_control_yaw		# control heading with rudder (used for auto takeoff on runway)
 

msg/VehicleLandDetected.msg

@@ -12,6 +12,7 @@ bool has_low_throttle
 
 bool vertical_movement
 bool horizontal_movement
+bool rotational_movement
 
 bool close_to_ground_or_skipped_check
 

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

msg/VehicleRatesSetpoint.msg

@@ -8,3 +8,5 @@ float32 yaw		# [rad/s] yaw rate setpoint
 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body	# Normalized thrust command in body NED frame [-1,1]
+
+bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change)

platforms/common/include/px4_platform_common/defines.h

@@ -136,5 +136,9 @@ __END_DECLS
 #define M_LOG2_E_F		0.69314718f
 #define M_INVLN2_F		1.44269504f	// 1 / log(2)
 
+/* The M_PI, as stated above, is not C standard. If you need it and
+ * it isn't in your math.h file then you can use this instead. */
+#define M_PI_PRECISE	3.141592653589793238462643383279502884
+
 #define M_DEG_TO_RAD 		0.017453292519943295
 #define M_RAD_TO_DEG 		57.295779513082323

platforms/common/spi.cpp

@@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version()
 #if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
 	int hw_version_revision = board_get_hw_version();
 #else
-	int hw_version_revision = (board_get_hw_version() << 16) | board_get_hw_revision();
+	int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision());
 #endif
 
 

platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h

@@ -45,8 +45,8 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			1
-#define MAX_TIMER_IO_CHANNELS		1
+#define MAX_IO_TIMERS			2
+#define MAX_TIMER_IO_CHANNELS		2
 
 #define MAX_LED_TIMERS			1
 #define MAX_TIMER_LED_CHANNELS		3

platforms/qurt/CMakeLists.txt

@@ -38,4 +38,5 @@ QURT_LIB(LIB_NAME px4
 		${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
 	LINK_LIBS
 		${module_libraries}
+		px4_layer
 	)

platforms/qurt/cmake/px4_impl_os.cmake

@@ -126,6 +126,7 @@ function(px4_os_add_flags)
 
 		-Wno-unknown-warning-option
 		-Wno-cast-align
+		--include=${PX4_SOURCE_DIR}/platforms/qurt/include/qurt_reqs.h
 	)
 
 	# Clear -rdynamic flag which fails for hexagon

platforms/qurt/include/px4_arch/micro_hal.h

@@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ * 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
+ * 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.
+ *
+ ****************************************************************************/
+
+// Placeholder
+

platforms/qurt/include/qurt_reqs.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ * 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+// This file is meant to tackle the dependencies found in PX4
+// that have not been implemented in the Hexagon SDK yet.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef unsigned long useconds_t;
+int usleep(useconds_t usec);
+
+#ifdef __cplusplus
+}
+#endif

platforms/qurt/include/sys/ioctl.h

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ * 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+// This file is meant to tackle the dependencies on IOCTL found in PX4.
+// As QURT does not have IOCTL natively, this file exists to define those
+// functions/params found throughout the code base.
+
+#pragma once
+
+#define	IOCPARM_MASK	0x1fff		/* parameter length, at most 13 bits */
+#define	IOC_VOID	0x20000000	/* no parameters */
+
+#define	_IOC(inout,group,num,len)	((unsigned long) \
+		((inout) | (((len) & IOCPARM_MASK) << 16) | ((group) << 8) | (num)))
+#define	_IO(g,n)	_IOC(IOC_VOID,	(g), (n), 0)

platforms/qurt/src/px4/CMakeLists.txt

@@ -32,3 +32,13 @@
 ############################################################################
 
 # Placeholder
+set(QURT_LAYER_SRCS
+	drv_hrt.cpp
+	tasks.cpp
+	)
+
+add_library(px4_layer
+        ${QURT_LAYER_SRCS}
+)
+
+target_link_libraries(px4_layer PRIVATE work_queue)

platforms/qurt/src/px4/drv_hrt.cpp

@@ -0,0 +1,326 @@
+/****************************************************************************
+ *
+ * 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
+ * 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/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/workqueue.h>
+#include <drivers/drv_hrt.h>
+
+#include <semaphore.h>
+#include <time.h>
+#include <string.h>
+#include <errno.h>
+
+#include "hrt_work.h"
+
+static constexpr unsigned HRT_INTERVAL_MIN = 50;
+static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
+
+static struct sq_queue_s	callout_queue;
+
+static uint64_t			latency_baseline;
+
+static uint64_t			latency_actual;
+
+const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+static px4_sem_t 	_hrt_lock;
+static struct work_s	_hrt_work;
+
+static int32_t dsp_offset = 0;
+
+static void hrt_latency_update();
+
+static void hrt_call_reschedule();
+static void hrt_call_invoke();
+
+hrt_abstime hrt_absolute_time_offset()
+{
+	return 0;
+}
+
+static void hrt_lock()
+{
+	px4_sem_wait(&_hrt_lock);
+}
+
+static void hrt_unlock()
+{
+	px4_sem_post(&_hrt_lock);
+}
+
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+{
+	return 0;
+}
+
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	int rv = clock_gettime(clk_id, tp);
+	hrt_abstime temp_abstime = ts_to_abstime(tp);
+
+	if (dsp_offset < 0) {
+		hrt_abstime temp_offset = -dsp_offset;
+
+		if (temp_offset >= temp_abstime) { temp_abstime = 0; }
+
+		else { temp_abstime -= temp_offset; }
+
+	} else {
+		temp_abstime += (hrt_abstime) dsp_offset;
+	}
+
+	tp->tv_sec = temp_abstime / 1000000;
+	tp->tv_nsec = (temp_abstime % 1000000) * 1000;
+	return rv;
+}
+
+hrt_abstime hrt_absolute_time()
+{
+	struct timespec ts;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
+}
+
+int hrt_set_absolute_time_offset(int32_t time_diff_us)
+{
+	dsp_offset = time_diff_us;
+	return 0;
+}
+
+hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
+}
+
+void hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	*t = hrt_absolute_time();
+}
+
+bool	hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+void	hrt_cancel(struct hrt_call *entry)
+{
+	hrt_lock();
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+	entry->period = 0;
+	hrt_unlock();
+}
+
+static void hrt_latency_update()
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	latency_counters[index]++;
+}
+
+void	hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
+
+void	hrt_init()
+{
+	sq_init(&callout_queue);
+
+	int sem_ret = px4_sem_init(&_hrt_lock, 0, 1);
+
+	if (sem_ret) {
+		PX4_ERR("SEM INIT FAIL: %s", strerror(errno));
+	}
+
+	memset(&_hrt_work, 0, sizeof(_hrt_work));
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == nullptr) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == nullptr) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != nullptr);
+	}
+}
+
+static void
+hrt_tim_isr(void *p)
+{
+	latency_actual = hrt_absolute_time();
+	hrt_latency_update();
+	hrt_call_invoke();
+	hrt_lock();
+	hrt_call_reschedule();
+	hrt_unlock();
+}
+
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	hrt_abstime	delay = HRT_INTERVAL_MAX;
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	if (next != nullptr) {
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			delay = HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			delay = next->deadline - now;
+		}
+	}
+
+	latency_baseline = now + delay;
+	hrt_work_cancel(&_hrt_work);
+	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
+	hrt_lock();
+
+	if (entry->deadline != 0) {
+		sq_rem(&entry->link, &callout_queue);
+	}
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+	hrt_unlock();
+}
+
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+static void
+hrt_call_invoke()
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	hrt_lock();
+
+	while (true) {
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == nullptr) {
+			break;
+		}
+
+		if (call->deadline > now) {
+			break;
+		}
+
+		sq_rem(&call->link, &callout_queue);
+		deadline = call->deadline;
+		call->deadline = 0;
+
+		if (call->callout) {
+			hrt_unlock();
+			call->callout(call->arg);
+			hrt_lock();
+		}
+
+		if (call->period != 0) {
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
+
+			hrt_call_enter(call);
+		}
+	}
+
+	hrt_unlock();
+}

platforms/qurt/src/px4/tasks.cpp

@@ -0,0 +1,379 @@
+/****************************************************************************
+ *
+ * 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
+ * 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/log.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/workqueue.h>
+
+#include <drivers/drv_hrt.h>
+#include <pthread.h>
+#include "hrt_work.h"
+
+#define PX4_TASK_STACK_SIZE 8192
+#define PX4_TASK_MAX_NAME_LENGTH 32
+#define PX4_TASK_MAX_ARGC 32
+#define PX4_TASK_MAX_ARGV_LENGTH 32
+#define PX4_MAX_TASKS 24
+
+typedef struct task_entry {
+	pthread_t tid;
+	char name[PX4_TASK_MAX_NAME_LENGTH + 4];
+	char stack[PX4_TASK_STACK_SIZE];
+	pthread_attr_t attr;
+	px4_main_t main_entry;
+	int argc;
+	char argv_storage[PX4_TASK_MAX_ARGC][PX4_TASK_MAX_ARGV_LENGTH];
+	char *argv[PX4_TASK_MAX_ARGC];
+	bool isused;
+
+	task_entry() : isused(false) {}
+} task_entry_t;
+
+static task_entry_t taskmap[PX4_MAX_TASKS];
+
+static bool task_mutex_initialized = false;
+static pthread_mutex_t task_mutex;
+
+static void *entry_adapter(void *ptr)
+{
+	task_entry_t *data;
+	data = (task_entry_t *) ptr;
+
+	if (data->main_entry) { data->main_entry(data->argc, data->argv); }
+
+	else { PX4_ERR("No valid task entry points"); }
+
+	pthread_exit(nullptr);
+	return nullptr;
+
+}
+
+static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_main_t main_entry, char *const argv[])
+{
+	int retcode = 0;
+	int i = 0;
+	int task_index = 0;
+	char *p = (char *)argv;
+
+	PX4_INFO("Creating pthread %s\n", name);
+
+	if (task_mutex_initialized == false) {
+		task_mutex_initialized = true;
+		pthread_mutex_init(&task_mutex, nullptr);
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (task_index = 0; task_index < PX4_MAX_TASKS; task_index++) {
+		if (taskmap[task_index].isused == false) { break; }
+	}
+
+	if (task_index == PX4_MAX_TASKS) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Hit maximum number of threads");
+		return -1;
+	}
+
+	taskmap[task_index].argc = 0;
+
+	while (p) {
+		taskmap[task_index].argc++;
+		p = argv[taskmap[task_index].argc];
+	}
+
+	if (taskmap[task_index].argc >= PX4_TASK_MAX_ARGC) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Too many arguments for thread %d", taskmap[task_index].argc);
+		return -1;
+	}
+
+	for (i = 0; i < PX4_TASK_MAX_ARGC; i++) {
+		if (i < taskmap[task_index].argc) {
+			int argument_length = strlen(argv[i]);
+
+			if (argument_length >= PX4_TASK_MAX_ARGV_LENGTH) {
+				pthread_mutex_unlock(&task_mutex);
+				PX4_ERR("Argument %d is too long %d", i, argument_length);
+				return -1;
+
+			} else {
+				//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
+				taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i];
+			}
+
+		} else {
+			taskmap[task_index].argv[i] = nullptr;
+			break;
+		}
+	}
+
+	taskmap[task_index].main_entry = main_entry;
+
+	if ((priority > 255) || (priority < 0)) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Invalid priority %d", priority);
+		return -1;
+	}
+
+	priority = 255 - priority;
+
+	if (priority < 5) { priority = 5; }
+
+	if (priority > 250) { priority = 250; }
+
+	if (strlen(name) >= PX4_TASK_MAX_NAME_LENGTH) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Task name is too long %s", name);
+		return -1;
+	}
+
+	strcpy(taskmap[task_index].name, "PX4_");
+	strcpy(&taskmap[task_index].name[4], name);
+
+	struct sched_param param;
+	param.sched_priority = priority;
+
+	pthread_attr_init(&taskmap[task_index].attr);
+	//pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name);
+	//pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack);
+	pthread_attr_setstacksize(&taskmap[task_index].attr, PX4_TASK_STACK_SIZE);
+	pthread_attr_setschedparam(&taskmap[task_index].attr, &param);
+
+	retcode = pthread_create(&taskmap[task_index].tid, &taskmap[task_index].attr, entry_adapter,
+				 (void *) &taskmap[task_index]);
+
+	if (retcode != PX4_OK) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Couldn't create pthread %s", name);
+		return -1;
+
+	} else {
+		PX4_INFO("Successfully created px4 task %s with tid %u",
+			 taskmap[task_index].name,
+			 (unsigned int) taskmap[task_index].tid);
+	}
+
+	taskmap[task_index].isused = true;
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return i;
+}
+
+px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
+			      char *const argv[])
+{
+	if (entry == nullptr) {
+		PX4_ERR("Entry function pointer is null");
+		return -1;
+	}
+
+	return px4_task_spawn_internal(name, priority, entry, argv);
+}
+
+int px4_task_delete(px4_task_t id)
+{
+	int rv = 0;
+
+	PX4_ERR("Ignoring px4_task_delete for task %d", id);
+
+	pthread_t pid;
+	PX4_WARN("Called px4_task_delete");
+
+	if (id < PX4_MAX_TASKS && taskmap[id].isused) {
+		pid = taskmap[id].tid;
+
+	} else {
+		return -EINVAL;
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	if (pthread_self() == pid) {
+		pthread_join(pid, nullptr);
+		taskmap[id].isused = false;
+		pthread_mutex_unlock(&task_mutex);
+		pthread_exit(nullptr);
+
+	} else {
+		rv = pthread_cancel(pid);
+	}
+
+	taskmap[id].isused = false;
+
+	return rv;
+}
+
+void px4_task_exit(int ret)
+{
+	PX4_ERR("Ignoring px4_task_exit with return value %d", ret);
+
+	int i;
+	pthread_t pid = pthread_self();
+
+	for (i = 0; i < PX4_MAX_TASKS; ++i) {
+		if (taskmap[i].tid == pid) {
+			pthread_mutex_lock(&task_mutex);
+			taskmap[i].isused = false;
+			break;
+		}
+	}
+
+	if (i >= PX4_MAX_TASKS)  {
+		PX4_ERR("px4_task_exit: self task not found!");
+
+	} else {
+		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name);
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
+int px4_task_kill(px4_task_t id, int sig)
+{
+	int rv = 0;
+	pthread_t pid;
+	PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name);
+
+	if (id < PX4_MAX_TASKS && taskmap[id].tid != 0) {
+		pthread_mutex_lock(&task_mutex);
+		pid = taskmap[id].tid;
+		pthread_mutex_unlock(&task_mutex);
+
+	} else {
+		return -EINVAL;
+	}
+
+	rv = pthread_kill(pid, sig);
+
+	return rv;
+}
+
+void px4_show_tasks()
+{
+	int idx = 0;
+	int count = 0;
+
+	PX4_INFO("Active Tasks:");
+
+	for (; idx < PX4_MAX_TASKS; idx++) {
+		if (taskmap[idx].isused) {
+			PX4_INFO("   %-10s %u", taskmap[idx].name,
+				 (unsigned int) taskmap[idx].tid);
+			count++;
+		}
+	}
+
+	if (count == 0) {
+		PX4_INFO("No running tasks");
+	}
+}
+
+px4_task_t px4_getpid()
+{
+	pthread_t pid = pthread_self();
+	px4_task_t ret = -1;
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			ret = i;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+	return ret;
+
+}
+
+
+const char *px4_get_taskname()
+{
+	pthread_t pid = pthread_self();
+	const char *prog_name = "UnknownApp";
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			prog_name = taskmap[i].name;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return prog_name;
+
+}
+
+static void timer_cb(void *data)
+{
+	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
+
+	sem_post(sem);
+}
+
+int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
+{
+	work_s _hpwork = {};
+
+	struct timespec ts_now;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
+
+	hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
+
+	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
+	sem_wait(sem);
+	hrt_work_cancel(&_hpwork);
+	return 0;
+}
+
+int px4_prctl(int option, const char *arg2, pthread_t pid)
+{
+	int rv = -1;
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
+			return rv;
+		}
+	}
+
+	return rv;
+}

src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp

@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstdlib>
 
 namespace InvenSense_ICM20649
 {

src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp

@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstdlib>
 
 namespace InvenSense_ICM42670P
 {

src/drivers/magnetometer/lis2mdl/lis2mdl.h

@@ -80,8 +80,8 @@
 #define CFG_REG_C_BDU                   (1 << 4) /* avoids reading of incorrect data due to async reads */
 
 /* interface factories */
-extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 #define LIS2MDLL_ADDRESS        0x1e
 

src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp

@@ -56,7 +56,7 @@
 class LIS2MDL_I2C : public device::I2C
 {
 public:
-	LIS2MDL_I2C(int bus, int bus_frequency);
+	LIS2MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_I2C() = default;
 
 	virtual int     read(unsigned address, void *data, unsigned count);
@@ -68,16 +68,16 @@ protected:
 };
 
 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency);
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency)
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_I2C(bus, bus_frequency);
+	return new LIS2MDL_I2C(config);
 }
 
-LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency)
+LIS2MDL_I2C::LIS2MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }
 

src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp

@@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS2MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;
 
 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS2MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS2MDL_I2C_interface(config);
 
 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS2MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS2MDL_SPI_interface(config);
 	}
 
 	if (interface == nullptr) {
@@ -94,6 +94,7 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS2MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS2MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;
 
@@ -112,8 +113,6 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 		return -1;
 	}
 
-	cli.i2c_address = LIS2MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL);
 
 	if (!strcmp(verb, "start")) {

src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp

@@ -61,7 +61,7 @@
 class LIS2MDL_SPI : public device::SPI
 {
 public:
-	LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS2MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_SPI() = default;
 
 	virtual int     init();
@@ -70,16 +70,16 @@ public:
 };
 
 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS2MDL_SPI(config);
 }
 
-LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency)
+LIS2MDL_SPI::LIS2MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }
 

src/drivers/magnetometer/lis3mdl/lis3mdl.h

@@ -84,8 +84,8 @@
 #define CNTL_REG5_DEFAULT 0x00
 
 /* interface factories */
-extern device::Device *LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS3MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 enum OPERATING_MODE {
 	CONTINUOUS = 0,

src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp

@@ -56,7 +56,7 @@
 class LIS3MDL_I2C : public device::I2C
 {
 public:
-	LIS3MDL_I2C(int bus, int bus_frequency);
+	LIS3MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_I2C() = default;
 
 	virtual int     read(unsigned address, void *data, unsigned count);
@@ -68,16 +68,16 @@ protected:
 };
 
 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency);
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency)
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_I2C(bus, bus_frequency);
+	return new LIS3MDL_I2C(config);
 }
 
-LIS3MDL_I2C::LIS3MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, LIS3MDLL_ADDRESS, bus_frequency)
+LIS3MDL_I2C::LIS3MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }
 

src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp

@@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS3MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;
 
 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS3MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS3MDL_I2C_interface(config);
 
 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS3MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS3MDL_SPI_interface(config);
 	}
 
 	if (interface == nullptr) {
@@ -90,6 +90,7 @@ void LIS3MDL::print_usage()
 	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1e);
 	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
 	PRINT_MODULE_USAGE_COMMAND("reset");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
@@ -100,6 +101,7 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS3MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS3MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;
 
@@ -118,8 +120,6 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 		return -1;
 	}
 
-	cli.i2c_address = LIS3MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS3MDL);
 
 	if (!strcmp(verb, "start")) {

src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp

@@ -61,7 +61,7 @@
 class LIS3MDL_SPI : public device::SPI
 {
 public:
-	LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS3MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_SPI() = default;
 
 	virtual int     init();
@@ -70,16 +70,16 @@ public:
 };
 
 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);
 
 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS3MDL_SPI(config);
 }
 
-LIS3MDL_SPI::LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, devid, spi_mode, bus_frequency)
+LIS3MDL_SPI::LIS3MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }
 

src/drivers/osd/msp_osd/msp_osd.cpp

@@ -121,7 +121,7 @@ MspOsd::MspOsd(const char *device) :
 	strcpy(_device, device);
 
 	// _is_initialized = true;
-	PX4_INFO("MSP OSD prepared to run on %s", _device);
+	PX4_INFO("MSP OSD running on %s", _device);
 }
 
 MspOsd::~MspOsd()
@@ -504,10 +504,10 @@ int MspOsd::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-Msp OSD!
+MSP telemetry streamer
 
 ### Implementation
-Does the things for the DJI Air Unit OSD
+Converts uORB messages to MSP telemetry packets
 
 ### Examples
 CLI usage example:

src/lib/matrix/matrix/Euler.hpp

@@ -93,11 +93,11 @@ public:
 	{
 		theta() = std::asin(-dcm(2, 0));
 
-		if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
+		if ((std::fabs(theta() - Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(dcm(1, 2), dcm(0, 2));
 
-		} else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
+		} else if ((std::fabs(theta() + Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(-dcm(1, 2), -dcm(0, 2));
 

src/lib/matrix/matrix/helper_functions.hpp

@@ -95,7 +95,7 @@ Integer wrap(Integer x, Integer low, Integer high)
 template<typename Type>
 Type wrap_pi(Type x)
 {
-	return wrap(x, Type(-M_PI), Type(M_PI));
+	return wrap(x, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }
 
 /**
@@ -104,7 +104,7 @@ Type wrap_pi(Type x)
 template<typename Type>
 Type wrap_2pi(Type x)
 {
-	return wrap(x, Type(0), Type((2 * M_PI)));
+	return wrap(x, Type(0), Type((2 * M_PI_PRECISE)));
 }
 
 /**
@@ -132,7 +132,7 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high
 template<typename Type>
 Type unwrap_pi(const Type last_angle, const Type new_angle)
 {
-	return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI));
+	return unwrap(last_angle, new_angle, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }
 
 /**

src/lib/matrix/matrix/math.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <assert.h>
+#include <px4_platform_common/defines.h>
 
 #include "helper_functions.hpp"
 

src/lib/matrix/test/MatrixAttitudeTest.cpp

@@ -200,8 +200,8 @@ TEST(MatrixAttitudeTest, Attitude)
 	}
 
 	// constants
-	double deg2rad = M_PI / 180.0;
-	double rad2deg = 180.0 / M_PI;
+	double deg2rad = M_PI_PRECISE / 180.0;
+	double rad2deg = 180.0 / M_PI_PRECISE;
 
 	// euler dcm round trip check
 	for (double roll = -90; roll <= 90; roll += 90) {
@@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude)
 	EXPECT_EQ(q, q_true);
 
 	// from axis angle, with length of vector the rotation
-	float n = float(std::sqrt(4 * M_PI * M_PI / 3));
+	float n = float(std::sqrt(4 * M_PI_F * M_PI_F / 3));
 	q = AxisAnglef(n, n, n);
 	EXPECT_EQ(q, Quatf(-1, 0, 0, 0));
 	q = AxisAnglef(0, 0, 0);

src/lib/matrix/test/MatrixHelperTest.cpp

@@ -81,20 +81,20 @@ TEST(MatrixHelperTest, Helper)
 
 	// wrap pi
 	EXPECT_FLOAT_EQ(wrap_pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI_PRECISE)));
 	EXPECT_FLOAT_EQ(wrap_pi(3.), 3.);
-	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI_PRECISE));
 	EXPECT_FALSE(std::isfinite(wrap_pi(NAN)));
 
 	// wrap 2pi
 	EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI_PRECISE));
 	EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.));
-	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE)));
 	EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));
 
 	// Equality checks

src/lib/matrix/test/MatrixUnwrapTest.cpp

@@ -5,7 +5,7 @@ using namespace matrix;
 
 TEST(MatrixUnwrapTest, UnwrapFloats)
 {
-	const float M_TWO_PI_F = float(M_PI * 2);
+	const float M_TWO_PI_F = float(M_PI_F * 2);
 
 	float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
@@ -34,7 +34,7 @@ TEST(MatrixUnwrapTest, UnwrapFloats)
 
 TEST(MatrixUnwrapTest, UnwrapDoubles)
 {
-	const double M_TWO_PI = M_PI * 2;
+	const double M_TWO_PI = M_PI_PRECISE * 2;
 
 	double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};

src/lib/parameters/param_translation.cpp

@@ -250,5 +250,14 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y
+	{
+		if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) {
+			strcpy(node->name, "VT_FW_DIFTHR_S_Y");
+			PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y");
+			return true;
+		}
+	}
+
 	return false;
 }

src/lib/perf/perf_counter.cpp

@@ -419,35 +419,25 @@ perf_print_counter(perf_counter_t handle)
 		return;
 	}
 
-	perf_print_counter_fd(1, handle);
-}
-
-void
-perf_print_counter_fd(int fd, perf_counter_t handle)
-{
-	if (handle == nullptr) {
-		return;
-	}
-
 	switch (handle->type) {
 	case PC_COUNT:
-		dprintf(fd, "%s: %" PRIu64 " events\n",
-			handle->name,
-			((struct perf_ctr_count *)handle)->event_count);
+		PX4_INFO_RAW("%s: %" PRIu64 " events\n",
+			     handle->name,
+			     ((struct perf_ctr_count *)handle)->event_count);
 		break;
 
 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 			float rms = sqrtf(pce->M2 / (pce->event_count - 1));
-			dprintf(fd, "%s: %" PRIu64 " events, %" PRIu64 "us elapsed, %.2fus avg, min %" PRIu32 "us max %" PRIu32
-				"us %5.3fus rms\n",
-				handle->name,
-				pce->event_count,
-				pce->time_total,
-				(pce->event_count == 0) ? 0 : (double)pce->time_total / (double)pce->event_count,
-				pce->time_least,
-				pce->time_most,
-				(double)(1e6f * rms));
+			PX4_INFO_RAW("%s: %" PRIu64 " events, %" PRIu64 "us elapsed, %.2fus avg, min %" PRIu32 "us max %" PRIu32
+				     "us %5.3fus rms\n",
+				     handle->name,
+				     pce->event_count,
+				     pce->time_total,
+				     (pce->event_count == 0) ? 0 : (double)pce->time_total / (double)pce->event_count,
+				     pce->time_least,
+				     pce->time_most,
+				     (double)(1e6f * rms));
 			break;
 		}
 
@@ -455,13 +445,13 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 			float rms = sqrtf(pci->M2 / (pci->event_count - 1));
 
-			dprintf(fd, "%s: %" PRIu64 " events, %.2fus avg, min %" PRIu32 "us max %" PRIu32 "us %5.3fus rms\n",
-				handle->name,
-				pci->event_count,
-				(pci->event_count == 0) ? 0 : (double)(pci->time_last - pci->time_first) / (double)pci->event_count,
-				pci->time_least,
-				pci->time_most,
-				(double)(1e6f * rms));
+			PX4_INFO_RAW("%s: %" PRIu64 " events, %.2fus avg, min %" PRIu32 "us max %" PRIu32 "us %5.3fus rms\n",
+				     handle->name,
+				     pci->event_count,
+				     (pci->event_count == 0) ? 0 : (double)(pci->time_last - pci->time_first) / (double)pci->event_count,
+				     pci->time_least,
+				     pci->time_most,
+				     (double)(1e6f * rms));
 			break;
 		}
 
@@ -593,13 +583,13 @@ perf_iterate_all(perf_callback cb, void *user)
 }
 
 void
-perf_print_all(int fd)
+perf_print_all(void)
 {
 	pthread_mutex_lock(&perf_counters_mutex);
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != nullptr) {
-		perf_print_counter_fd(fd, handle);
+		perf_print_counter(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
 
@@ -607,19 +597,19 @@ perf_print_all(int fd)
 }
 
 void
-perf_print_latency(int fd)
+perf_print_latency(void)
 {
 	latency_info_t latency;
-	dprintf(fd, "bucket [us] : events\n");
+	PX4_INFO_RAW("bucket [us] : events\n");
 
 	for (int i = 0; i < get_latency_bucket_count(); i++) {
 		latency = get_latency(i, i);
-		dprintf(fd, "       %4i : %li\n", latency.bucket, (long int)latency.counter);
+		PX4_INFO_RAW("       %4i : %li\n", latency.bucket, (long int)latency.counter);
 	}
 
 	// print the overflow bucket value
 	latency = get_latency(get_latency_bucket_count() - 1, get_latency_bucket_count());
-	dprintf(fd, " >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
+	PX4_INFO_RAW(" >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
 }
 
 void

src/lib/perf/perf_counter.h

@@ -174,14 +174,6 @@ __EXPORT extern void		perf_reset(perf_counter_t handle);
  */
 __EXPORT extern void		perf_print_counter(perf_counter_t handle);
 
-/**
- * Print one performance counter to a fd.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
- * @param handle		The counter to print.
- */
-__EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
-
 /**
  * Print one performance counter to a buffer.
  *
@@ -194,10 +186,8 @@ __EXPORT extern int		perf_print_counter_buffer(char *buffer, int length, perf_co
 
 /**
  * Print all of the performance counters.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
  */
-__EXPORT extern void		perf_print_all(int fd);
+__EXPORT extern void		perf_print_all(void);
 
 
 typedef void (*perf_callback)(perf_counter_t handle, void *user);
@@ -216,10 +206,8 @@ __EXPORT extern void	perf_iterate_all(perf_callback cb, void *user);
 
 /**
  * Print hrt latency counters.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
  */
-__EXPORT extern void		perf_print_latency(int fd);
+__EXPORT extern void		perf_print_latency(void);
 
 /**
  * Reset all of the performance counters.

src/modules/angular_velocity_controller/AngularVelocityController.cpp

@@ -218,10 +218,10 @@ AngularVelocityController::Run()
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			Vector3f integral = _control.getIntegral();
-			rate_ctrl_status.timestamp = hrt_absolute_time();
 			rate_ctrl_status.rollspeed_integ = integral(0);
 			rate_ctrl_status.pitchspeed_integ = integral(1);
 			rate_ctrl_status.yawspeed_integ = integral(2);
+			rate_ctrl_status.timestamp = hrt_absolute_time();
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);
 
 			// publish controller output
@@ -238,14 +238,12 @@ void
 AngularVelocityController::publish_angular_acceleration_setpoint()
 {
 	Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
-
 	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
-	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
 	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
 	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
 	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
-
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
 }
 
@@ -253,14 +251,12 @@ void
 AngularVelocityController::publish_torque_setpoint()
 {
 	Vector3f torque_sp = _control.getTorqueSetpoint();
-
 	vehicle_torque_setpoint_s v_torque_sp = {};
-	v_torque_sp.timestamp = hrt_absolute_time();
 	v_torque_sp.timestamp_sample = _timestamp_sample;
 	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
 	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
 	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
+	v_torque_sp.timestamp = hrt_absolute_time();
 	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
 }
 
@@ -268,12 +264,11 @@ void
 AngularVelocityController::publish_thrust_setpoint()
 {
 	vehicle_thrust_setpoint_s v_thrust_sp = {};
-	v_thrust_sp.timestamp = hrt_absolute_time();
 	v_thrust_sp.timestamp_sample = _timestamp_sample;
 	v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
 	v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
 	v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
-
+	v_thrust_sp.timestamp = hrt_absolute_time();
 	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
 }
 

src/modules/commander/Commander.cpp

@@ -1575,7 +1575,7 @@ void Commander::updateParameters()
 			       && _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 	const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status)
 			      && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-	const bool is_ground = is_ground_rover(_vehicle_status);
+	const bool is_ground = is_ground_vehicle(_vehicle_status);
 
 	/* disable manual override for all systems that rely on electronic stabilization */
 	if (is_rotary) {

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

@@ -35,11 +35,6 @@
 
 using namespace time_literals;
 
-OffboardChecks::OffboardChecks()
-{
-	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
-}
-
 void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 {
 	reporter.failsafeFlags().offboard_control_signal_lost = true;
@@ -47,9 +42,10 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 	offboard_control_mode_s offboard_control_mode;
 
 	if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
-		bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
-					  || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
-					  || offboard_control_mode.actuator;
+		bool data_is_recent = hrt_absolute_time() < offboard_control_mode.timestamp + _param_com_of_loss_t.get() * 1_s;
+		bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity
+					   || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
+					   || offboard_control_mode.actuator) && data_is_recent;
 
 		if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
 			offboard_available = false;
@@ -62,9 +58,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 			offboard_available = false;
 		}
 
-		_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
-
 		// This is a mode requirement, no need to report
-		reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
+		reporter.failsafeFlags().offboard_control_signal_lost = !offboard_available;
 	}
 }

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

@@ -37,19 +37,17 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/offboard_control_mode.h>
-#include <lib/hysteresis/hysteresis.h>
 
 class OffboardChecks : public HealthAndArmingCheckBase
 {
 public:
-	OffboardChecks();
+	OffboardChecks() = default;
 	~OffboardChecks() = default;
 
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
 	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	systemlib::Hysteresis _offboard_available{false};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t

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

@@ -119,7 +119,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
 					    events::ID("check_rc_dl_no_dllink"),
 					    log_level, "No connection to the ground control station");
 
-		if (reporter.mavlink_log_pub()) {
+		if (gcs_connection_required && reporter.mavlink_log_pub()) {
 			mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t");
 		}
 

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

@@ -32,6 +32,8 @@
  ****************************************************************************/
 
 #include "sdcardCheck.hpp"
+#include <dirent.h>
+#include <string.h>
 
 #ifdef __PX4_DARWIN
 #include <sys/param.h>
@@ -42,38 +44,85 @@
 
 void SdCardChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_com_arm_sdcard.get() <= 0) {
-		return;
-	}
+	if (_param_com_arm_sdcard.get() > 0) {
 
-	struct statfs statfs_buf;
+		struct statfs statfs_buf;
 
-	if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
-		// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
-		_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
-	}
-
-	if (!_sdcard_detected) {
-		NavModes affected_modes{NavModes::None};
-
-		if (_param_com_arm_sdcard.get() == 2) {
-			// disallow arming without sd card
-			affected_modes = NavModes::All;
+		if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
+			// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
+			_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
 		}
 
+		if (!_sdcard_detected) {
+			NavModes affected_modes{NavModes::None};
+
+			if (_param_com_arm_sdcard.get() == 2) {
+				// disallow arming without sd card
+				affected_modes = NavModes::All;
+			}
+
+			/* EVENT
+			 * @description
+			 * Insert an SD Card to the autopilot and reboot the system.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(affected_modes, health_component_t::system,
+						    events::ID("check_missing_fmu_sdcard"),
+						    events::Log::Error, "Missing FMU SD Card");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			}
+		}
+	}
+
+#ifdef __PX4_NUTTX
+	// Check for hardfault files
+
+	if (!_hardfault_checked_once && _param_com_arm_hardfault_check.get()) {
+		_hardfault_checked_once = true;
+
+		DIR *dp = opendir(PX4_STORAGEDIR);
+
+		if (dp != nullptr) {
+
+			struct dirent *result;
+
+			while ((result = readdir(dp)) && !_hardfault_file_present) {
+
+				// Check for pattern fault_*.log
+				if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) {
+					_hardfault_file_present = true;
+				}
+			}
+
+			closedir(dp);
+		}
+	}
+
+	if (_hardfault_file_present && _param_com_arm_hardfault_check.get()) {
 		/* EVENT
 		 * @description
-		 * Insert an SD Card to the autopilot and reboot the system.
+		 * The SD card contains crash dump files, service the vehicle before continuing to fly.
 		 *
 		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+		 * Check how to debug hardfaults on <a>https://docs.px4.io/main/en/debug/gdb_debugging.html#hard-fault-debugging</a>.
+		 * When completed, remove the files 'fault_*.log' on the SD card.
+		 *
+		 * This check can be configured via <param>COM_ARM_HFLT_CHK</param> parameter.
 		 * </profile>
 		 */
-		reporter.armingCheckFailure(affected_modes, health_component_t::system, events::ID("check_missing_fmu_sdcard"),
-					    events::Log::Error, "Missing FMU SD Card");
+		reporter.healthFailure(NavModes::All, health_component_t::system,
+				       events::ID("check_hardfault_present"),
+				       events::Log::Error, "Crash dumps present on SD card");
 
 		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service");
 		}
 	}
+
+#endif /* __PX4_NUTTX */
 }

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

@@ -46,7 +46,13 @@ public:
 private:
 	bool _sdcard_detected{false};
 
+#ifdef __PX4_NUTTX
+	bool _hardfault_checked_once {false};
+	bool _hardfault_file_present{false};
+#endif
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard
+					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard,
+					(ParamBool<px4::params::COM_ARM_HFLT_CHK>) _param_com_arm_hardfault_check
 				       )
 };

src/modules/commander/commander_helper.cpp

@@ -118,19 +118,9 @@ bool is_fixed_wing(const vehicle_status_s &current_status)
 	return current_status.system_type == VEHICLE_TYPE_FIXED_WING;
 }
 
-bool is_ground_rover(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
-}
-
-bool is_boat(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_BOAT;
-}
-
 bool is_ground_vehicle(const vehicle_status_s &current_status)
 {
-	return is_ground_rover(current_status) || is_boat(current_status);
+	return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
 }
 
 // End time for currently blinking LED message, 0 if no blink message

src/modules/commander/commander_helper.h

@@ -55,8 +55,6 @@ bool is_rotary_wing(const vehicle_status_s &current_status);
 bool is_vtol(const vehicle_status_s &current_status);
 bool is_vtol_tailsitter(const vehicle_status_s &current_status);
 bool is_fixed_wing(const vehicle_status_s &current_status);
-bool is_ground_rover(const vehicle_status_s &current_status);
-bool is_boat(const vehicle_status_s &current_status);
 bool is_ground_vehicle(const vehicle_status_s &current_status);
 
 int buzzer_init(void);

src/modules/commander/commander_params.c

@@ -1010,6 +1010,17 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
  */
 PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
 
+/**
+ * Enable FMU SD card hardfault detection check
+ *
+ * This check detects if there are hardfault files present on the
+ * SD card. If so, and the parameter is enabled, arming is prevented.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
+
 /**
  * Enforced delay between arming and further navigation
  *

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -199,10 +199,10 @@ public:
 	virtual uint32_t getStoppedMotors() const { return 0; }
 
 	/**
-	 * Fill in the allocated and unallocated torque and thrust.
-	 * Should return false if not filled in and the effectivenes matrix should be used instead
+	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
+	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
 	 */
-	virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; }
+	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
 
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -217,48 +217,36 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit
 	}
 }
 
-bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const
+void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
 {
-	status.torque_setpoint_achieved = true;
-	status.thrust_setpoint_achieved = true;
 
 	// Note: the values '-1', '1' and '0' are just to indicate a negative,
 	// positive or no saturation to the rate controller. The actual magnitude is not used.
 	if (_saturation_flags.roll_pos) {
 		status.unallocated_torque[0] = 1.f;
-		status.torque_setpoint_achieved = false;
 
 	} else if (_saturation_flags.roll_neg) {
 		status.unallocated_torque[0] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}
 
 	if (_saturation_flags.pitch_pos) {
 		status.unallocated_torque[1] = 1.f;
-		status.torque_setpoint_achieved = false;
 
 	} else if (_saturation_flags.pitch_neg) {
 		status.unallocated_torque[1] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}
 
 	if (_saturation_flags.yaw_pos) {
 		status.unallocated_torque[2] = 1.f;
-		status.torque_setpoint_achieved = false;
 
 	} else if (_saturation_flags.yaw_neg) {
 		status.unallocated_torque[2] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}
 
 	if (_saturation_flags.thrust_pos) {
 		status.unallocated_thrust[2] = 1.f;
-		status.thrust_setpoint_achieved = false;
 
 	} else if (_saturation_flags.thrust_neg) {
 		status.unallocated_thrust[2] = -1.f;
-		status.thrust_setpoint_achieved = false;
 	}
-
-	return true;
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -79,7 +79,7 @@ public:
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
-	bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override;
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -233,10 +233,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
 	return num_actuators;
 }
 
-uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control)
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
+		float collective_tilt_control)
 {
-	if (!PX4_ISFINITE(tilt_control)) {
-		tilt_control = -1.f;
+	if (!PX4_ISFINITE(collective_tilt_control)) {
+		collective_tilt_control = -1.f;
 	}
 
 	uint32_t nontilted_motors = 0;
@@ -250,8 +251,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv
 		}
 
 		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
-		float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f);
-		float tilt_direction = math::radians((float)tilt.tilt_direction);
+		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
+		const float tilt_direction = math::radians((float)tilt.tilt_direction);
 		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
 	}
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -54,7 +54,7 @@ bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
 {
-	if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+	if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
@@ -66,9 +66,9 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// Update matrix with tilts in vertical position when update is triggered by a manual
 	// configuration (parameter) change. This is to make sure the normalization
 	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
-	const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f :
-					   _last_tilt_control;
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
+	const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ?
+			-1.f : _last_collective_tilt_control;
+	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
 			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
@@ -86,7 +86,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 
 	// If it was an update coming from a config change, then make sure to update matrix in
 	// the next iteration again with the correct tilt (but without updating the normalization scale).
-	_combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+	_collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
 
 	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }
@@ -113,24 +113,24 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		actuator_controls_s actuator_controls_1;
 
 		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			float control_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
+			float control_collective_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
 
-			// set control_tilt to exactly -1 or 1 if close to these end points
-			control_tilt = control_tilt < -0.99f ? -1.f : control_tilt;
-			control_tilt = control_tilt > 0.99f ? 1.f : control_tilt;
+			// set control_collective_tilt to exactly -1 or 1 if close to these end points
+			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
 
-			// initialize _last_tilt_control
-			if (!PX4_ISFINITE(_last_tilt_control)) {
-				_last_tilt_control = control_tilt;
+			// initialize _last_collective_tilt_control
+			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+				_last_collective_tilt_control = control_collective_tilt;
 
-			} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
-				_combined_tilt_updated = true;
-				_last_tilt_control = control_tilt;
+			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+				_collective_tilt_updated = true;
+				_last_collective_tilt_control = control_collective_tilt;
 			}
 
 			for (int i = 0; i < _tilts.count(); ++i) {
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
-					actuator_sp(i + _first_tilt_idx) += control_tilt;
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
 			}
 		}
@@ -147,6 +147,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			}
 		}
 	}
+
+	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
+	// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
+	// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
+	if (matrix_index == 0 && _tilts.hasYawControl()) {
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
+
+		if (control_sp(2) < -1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
+
+		} else if (control_sp(2) > 1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
+		}
+	}
 }
 
 void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
@@ -170,3 +185,23 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 		break;
 	}
 }
+
+void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+	// only handle matrix 0 (motors and tilts)
+	if (matrix_index == 1) {
+		return;
+	}
+
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -81,8 +81,11 @@ public:
 	const char *name() const override { return "VTOL Tiltrotor"; }
 
 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
+
 protected:
-	bool _combined_tilt_updated{true};
+	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;
@@ -93,8 +96,15 @@ protected:
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 	int _first_tilt_idx{0}; ///< applies to matrix 0
 
-	float _last_tilt_control{NAN};
+	float _last_collective_tilt_control{NAN};
 
 	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+
+	struct YawTiltSaturationFlags {
+		bool tilt_yaw_pos;
+		bool tilt_yaw_neg;
+	};
+
+	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -589,33 +589,29 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 
 	// TODO: disabled motors (?)
 
-	if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) {
-		// Allocated control
-		const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
-		control_allocator_status.allocated_torque[0] = allocated_control(0);
-		control_allocator_status.allocated_torque[1] = allocated_control(1);
-		control_allocator_status.allocated_torque[2] = allocated_control(2);
-		control_allocator_status.allocated_thrust[0] = allocated_control(3);
-		control_allocator_status.allocated_thrust[1] = allocated_control(4);
-		control_allocator_status.allocated_thrust[2] = allocated_control(5);
+	// Allocated control
+	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
 
-		// Unallocated control
-		const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
-				allocated_control;
-		control_allocator_status.unallocated_torque[0] = unallocated_control(0);
-		control_allocator_status.unallocated_torque[1] = unallocated_control(1);
-		control_allocator_status.unallocated_torque[2] = unallocated_control(2);
-		control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
-		control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
-		control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
+	// Unallocated control
+	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
+			allocated_control;
+	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
+	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
+	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
+	control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
+	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
+	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
 
-		// Allocation success flags
-		control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
-				unallocated_control(2)).norm_squared() < 1e-6f);
-		control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
-				unallocated_control(5)).norm_squared() < 1e-6f);
-	}
+	// override control_allocator_status in customized saturation logic for certain effectiveness types
+	_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);
 
+	// Allocation success flags
+	control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
+			control_allocator_status.unallocated_torque[1],
+			control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
+	control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
+			control_allocator_status.unallocated_thrust[1],
+			control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);
 
 	// Actuator saturation
 	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();

src/modules/ekf2/CMakeLists.txt

@@ -91,7 +91,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

src/modules/ekf2/EKF/CMakeLists.txt

@@ -42,7 +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/bias_estimator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -57,14 +57,16 @@ class BiasEstimator
 {
 public:
 	struct status {
-		float bias;
-		float bias_var;
-		float innov;
-		float innov_var;
-		float innov_test_ratio;
+		float bias{0.f};
+		float bias_var{0.f};
+		float innov{0.f};
+		float innov_var{0.f};
+		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()
@@ -107,7 +109,7 @@ private:
 	float _time_since_last_negative_innov{0.f};
 	float _time_since_last_positive_innov{0.f};
 
-	status _status;
+	status _status{};
 
 	void constrainStateVar();
 	float computeKalmanGain(float innov_var) const;

src/modules/ekf2/EKF/common.h

@@ -67,7 +67,7 @@ using math::Utilities::updateYawInRotMat;
 
 // maximum sensor intervals in usec
 #define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
-#define EV_MAX_INTERVAL   (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
+#define EV_MAX_INTERVAL   (uint64_t)1e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
 #define GPS_MAX_INTERVAL  (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
 #define RNG_MAX_INTERVAL  (uint64_t)2e5 ///< Maximum allowable time interval between range finder  measurements (uSec)
 
@@ -78,9 +78,15 @@ using math::Utilities::updateYawInRotMat;
 // ground effect compensation
 #define 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_FRD = 0,
-	BODY_FRAME_FRD  = 1
+	LOCAL_FRAME_NED = 0,
+	LOCAL_FRAME_FRD = 1,
+	BODY_FRAME_FRD  = 2
 };
 
 enum GeoDeclinationMask : uint8_t {
@@ -113,6 +119,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),
@@ -126,17 +138,24 @@ enum RngCtrl : uint8_t {
 	ENABLED     = 2
 };
 
+enum class EvCtrl : uint8_t {
+	HPOS = (1<<0),
+	VPOS = (1<<1),
+	VEL  = (1<<2),
+	YAW  = (1<<3)
+};
+
 enum SensorFusionMask : uint16_t {
 	// Bit locations for fusion_mode
 	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)
-	USE_EXT_VIS_VEL  = (1<<8),      ///< set to true to use external vision velocity data
+	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
 };
 
 struct gpsMessage {
@@ -226,11 +245,13 @@ 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    velVar{};      ///< XYZ velocity variances ((m/sec)**2)
-	float       angVar{};      ///< angular heading variance (rad**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
 };
 
 struct dragSample {
@@ -262,9 +283,11 @@ 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};
+	int32_t ev_ctrl{0};
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
@@ -303,7 +326,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)
@@ -336,6 +359,7 @@ struct parameters {
 	float arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
 
 	// synthetic sideslip fusion
+	int32_t beta_fusion_enabled{0};
 	float beta_innov_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
 	float beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
@@ -356,6 +380,10 @@ struct parameters {
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
 
 	// vision position fusion
+	float ev_vel_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
+	float ev_pos_noise{0.1f};               ///< minimum allowed observation noise for EV position fusion (m)
+	float ev_att_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
+	int32_t ev_quality_minimum{0};          ///< vision minimum acceptable quality integer
 	float ev_vel_innov_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
@@ -394,11 +422,11 @@ struct parameters {
 	float acc_bias_learn_gyr_lim{3.0f};     ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
 	float acc_bias_learn_tc{0.5f};          ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
 
-	const unsigned reset_timeout_max{7000000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
-	const unsigned no_aid_timeout_max{1000000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
+	const unsigned reset_timeout_max{7'000'000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
+	const unsigned no_aid_timeout_max{1'000'000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
 	const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec)
 
-	int32_t valid_timeout_max{5000000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
+	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
 	// static barometer pressure position error coefficient along body axes
 	float static_pressure_coef_xp{0.0f};    // (-)

src/modules/ekf2/EKF/control.cpp

@@ -86,10 +86,7 @@ void Ekf::controlFusionModes()
 	}
 
 	if (_gps_buffer) {
-		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
-
 		// check for arrival of new sensor data at the fusion time horizon
-		_time_prev_gps_us = _gps_sample_delayed.time_us;
 		_gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 
 		if (_gps_data_ready) {
@@ -103,8 +100,6 @@ void Ekf::controlFusionModes()
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_gps_sample_delayed.pos -= pos_offset_earth.xy();
 			_gps_sample_delayed.hgt += pos_offset_earth(2);
-
-			_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
 		}
 	}
 
@@ -161,10 +156,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);
 	}
@@ -181,7 +172,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
@@ -199,236 +190,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.fusion_mode & SensorFusionMask::USE_EXT_VIS_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();
-				}
-
-				// turn on use of external vision measurements for velocity
-				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) {
-					startEvVelFusion();
-				}
-			}
-		}
-
-		// 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)) {
-					// only reset velocity if we have no another source of aiding constraining it
-					if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
-					    isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) {
-
-						if (_control_status.flags.ev_vel) {
-							resetVelocityToVision();
-						}
-					}
-
-					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 velocity observations
-		if (_control_status.flags.ev_vel) {
-			if (reset && _control_status_prev.flags.ev_vel) {
-				resetVelocityToVision();
-			}
-
-			// check if we have been deadreckoning too long
-			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
-				// only reset velocity if we have no another source of aiding constraining it
-				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
-				    isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) {
-					resetVelocityToVision();
-				}
-			}
-
-			resetEstimatorAidStatus(_aid_src_ev_vel);
-
-			const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
-
-			const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
-
-			updateVelocityAidSrcStatus(_ev_sample_delayed.time_us, getVisionVelocityInEkfFrame(), obs_var, innov_gate, _aid_src_ev_vel);
-
-			_aid_src_ev_vel.fusion_enabled = true;
-
-			fuseVelocity(_aid_src_ev_vel);
-		}
-
-		// 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.angVar, 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);
-			}
-		}
-
-		// 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)
-		   && !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)
@@ -444,13 +205,14 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 
 		const bool continuing_conditions_passing = !gps_checks_failing;
 
+		const bool gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
 		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GPS_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _control_status.flags.tilt_align
 				&& gps_checks_passing
 				&& !is_gps_yaw_data_intermittent
-				&& !_gps_intermittent;
+				&& !gps_intermittent;
 
 		if (_control_status.flags.gps_yaw) {
 
@@ -570,30 +332,31 @@ void Ekf::controlAirDataFusion()
 
 void Ekf::controlBetaFusion()
 {
-	if (_control_status.flags.fake_pos) {
-		return;
-	}
+	_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
+		&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
 
-	// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
-	const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
+	if (_control_status.flags.fuse_beta) {
 
-	if (beta_fusion_time_triggered &&
-	    _control_status.flags.fuse_beta &&
-	    _control_status.flags.in_air) {
-		updateSideslip(_aid_src_sideslip);
-		_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
+		// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
+		const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
 
-		// If starting wind state estimation, reset the wind states and covariances before fusing any data
-		if (!_control_status.flags.wind) {
-			// activate the wind states
-			_control_status.flags.wind = true;
-			// reset the timeout timers to prevent repeated resets
-			_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
-			resetWind();
-		}
+		if (beta_fusion_time_triggered) {
 
-		if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
-			fuseSideslip(_aid_src_sideslip);
+			updateSideslip(_aid_src_sideslip);
+			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
+
+			// If starting wind state estimation, reset the wind states and covariances before fusing any data
+			if (!_control_status.flags.wind) {
+				// activate the wind states
+				_control_status.flags.wind = true;
+				// reset the timeout timers to prevent repeated resets
+				_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
+				resetWind();
+			}
+
+			if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
+				fuseSideslip(_aid_src_sideslip);
+			}
 		}
 	}
 }
@@ -601,16 +364,14 @@ void Ekf::controlBetaFusion()
 void Ekf::controlDragFusion()
 {
 	if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
-	    !_control_status.flags.fake_pos && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
+	    !_control_status.flags.fake_pos && _control_status.flags.in_air) {
 
 		if (!_control_status.flags.wind) {
 			// reset the wind states and covariances when starting drag accel fusion
 			_control_status.flags.wind = true;
 			resetWind();
-
 		}
 
-
 		dragSample drag_sample;
 
 		if (_drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &drag_sample)) {

src/modules/ekf2/EKF/ekf.cpp

@@ -204,8 +204,6 @@ bool Ekf::initialiseFilter()
 	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
-	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
-	_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
 
 	// reset the output predictor state history to match the EKF initial values
 	alignOutputFilter();

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>
@@ -385,9 +386,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;
 
@@ -413,6 +411,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; }
 
@@ -472,23 +472,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
 	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};
@@ -504,6 +499,10 @@ 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)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
@@ -519,14 +518,10 @@ private:
 	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
 	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
 	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
-	uint64_t _mag_use_not_inhibit_us{0};	///< last system time in usec before magnetometer use was inhibited
 	float _last_static_yaw{NAN};		///< last yaw angle recorded when on ground motion checks were passing (rad)
 
-	bool _mag_inhibit_yaw_reset_req{false};	///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
-	bool _synthetic_mag_z_active{false};	///< true if we are generating synthetic magnetometer Z measurements
-	bool _is_yaw_fusion_inhibited{false};		///< true when yaw sensor use is being inhibited
 
 	SquareMatrix24f P{};	///< state covariance matrix
 
@@ -595,6 +590,9 @@ private:
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
 
+	uint64_t _last_ev_fail_us{0};		///< last system time in usec that the EV failed it's checks
+	uint64_t _last_ev_pass_us{0};		///< last system time in usec that the EV passed it's checks
+
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
 	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
@@ -633,7 +631,6 @@ private:
 
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
-	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
@@ -700,11 +697,9 @@ private:
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var);
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, float vel_var) { resetHorizontalVelocityTo(new_horz_vel, Vector2f(vel_var, vel_var)); }
 
-	void resetVelocityToVision();
 	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);
@@ -771,23 +766,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();
-
-	Vector3f getVisionVelocityInEkfFrame() const;
-
-	Vector3f getVisionVelocityVarianceInEkfFrame() const;
-
 	// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
 	// that is optimized by exploring the sparsity in H
 	template <size_t ...Idxs>
@@ -872,6 +856,10 @@ private:
 
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
+	void controlEvHeightFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source1d_s& aid_src);
+	void controlEvPosFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source2d_s& aid_src);
+	void controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source3d_s& aid_src);
+	void controlEvYawFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source1d_s& aid_src);
 
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion();
@@ -889,12 +877,12 @@ private:
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 
-	void checkHaglYawResetReq();
+	bool haglYawResetReq() const;
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
-	void runOnGroundYawReset();
 	bool canResetMagHeading() const;
-	void runInAirYawReset();
+	void runYawReset();
+	bool resetMagStates();
 
 	void selectMagAuto();
 	void check3DMagFusionSuitability();
@@ -903,7 +891,6 @@ private:
 	bool canUse3DMagFusion() const;
 
 	void checkMagDeclRequired();
-	void checkMagInhibition();
 	bool shouldInhibitMag() const;
 	bool magFieldStrengthDisturbed(const Vector3f &mag) const;
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
@@ -943,7 +930,6 @@ private:
 	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
 	void controlRangeHeightFusion();
-	void controlEvHeightFusion(const extVisionSample &ev_sample);
 
 	bool isConditionalRangeAidSuitable();
 
@@ -1026,17 +1012,10 @@ private:
 	void stopAirspeedFusion();
 
 	void stopGpsFusion();
-	void stopGpsPosFusion();
-	void stopGpsVelFusion();
 
 	void startGpsYawFusion(const gpsSample &gps_sample);
 	void stopGpsYawFusion();
 
-	void startEvPosFusion();
-	void startEvVelFusion();
-	void startEvYawFusion();
-
-	void stopEvFusion();
 	void stopEvPosFusion();
 	void stopEvVelFusion();
 	void stopEvYawFusion();
@@ -1061,14 +1040,16 @@ 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};
 
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
-	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
+	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate in an emergency
 
 	void runYawEKFGSF();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -44,13 +44,6 @@
 #include <mathlib/mathlib.h>
 #include <cstdlib>
 
-void Ekf::resetVelocityToVision()
-{
-	_information_events.flags.reset_vel_to_vision = true;
-	ECL_INFO("reset to vision velocity");
-	resetVelocityTo(getVisionVelocityInEkfFrame(), getVisionVelocityVarianceInEkfFrame());
-}
-
 void Ekf::resetHorizontalVelocityToZero()
 {
 	_information_events.flags.reset_vel_to_zero = true;
@@ -115,22 +108,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;
@@ -161,6 +138,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 	_state_reset_status.posNE_change = delta_horz_pos;
 	_state_reset_status.posNE_counter++;
 
+	_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;
 }
@@ -176,7 +156,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);
@@ -249,8 +228,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();
@@ -294,17 +273,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.angVar, 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()
 {
@@ -722,7 +690,7 @@ void Ekf::resetMagBiasAndYaw()
 	_saved_mag_bf_variance.zero();
 
 	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		_mag_yaw_reset_req = true;
+		stopMagFusion();
 	}
 
 	_control_status.flags.mag_fault = false;
@@ -750,6 +718,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;
 	}
@@ -1084,9 +1053,12 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 
 void Ekf::stopMagFusion()
 {
-	stopMag3DFusion();
-	stopMagHdgFusion();
-	clearMagCov();
+	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+		ECL_INFO("stopping all mag fusion");
+		stopMag3DFusion();
+		stopMagHdgFusion();
+		clearMagCov();
+	}
 }
 
 void Ekf::stopMag3DFusion()
@@ -1156,64 +1128,6 @@ void Ekf::updateGroundEffect()
 	}
 }
 
-Vector3f Ekf::getVisionVelocityInEkfFrame() const
-{
-	Vector3f vel;
-	// correct velocity for offset relative to IMU
-	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-
-	// rotate measurement into correct earth frame if required
-	switch (_ev_sample_delayed.vel_frame) {
-	case VelocityFrame::BODY_FRAME_FRD:
-		vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
-		break;
-
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-			vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
-
-		} else {
-			vel = _ev_sample_delayed.vel - vel_offset_earth;
-		}
-
-		break;
-	}
-
-	return vel;
-}
-
-Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
-{
-	Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar);
-
-	// rotate measurement into correct earth frame if required
-	switch (_ev_sample_delayed.vel_frame) {
-	case VelocityFrame::BODY_FRAME_FRD:
-		ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
-		break;
-
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
-			ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
-		}
-
-		break;
-	}
-
-	return ev_vel_cov.diag();
-}
-
-// 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)
@@ -1306,8 +1220,10 @@ void Ekf::stopAirspeedFusion()
 void Ekf::stopGpsFusion()
 {
 	if (_control_status.flags.gps) {
-		stopGpsPosFusion();
-		stopGpsVelFusion();
+		ECL_INFO("stopping GPS position and velocity fusion");
+
+		resetEstimatorAidStatus(_aid_src_gnss_pos);
+		resetEstimatorAidStatus(_aid_src_gnss_vel);
 
 		_control_status.flags.gps = false;
 	}
@@ -1321,23 +1237,6 @@ void Ekf::stopGpsFusion()
 	_inhibit_ev_yaw_use = false;
 }
 
-void Ekf::stopGpsPosFusion()
-{
-	if (_control_status.flags.gps) {
-		ECL_INFO("stopping GPS position fusion");
-		_control_status.flags.gps = false;
-
-		resetEstimatorAidStatus(_aid_src_gnss_pos);
-	}
-}
-
-void Ekf::stopGpsVelFusion()
-{
-	ECL_INFO("stopping GPS velocity fusion");
-
-	resetEstimatorAidStatus(_aid_src_gnss_vel);
-}
-
 void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
 {
 	if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
@@ -1360,68 +1259,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::startEvVelFusion()
-{
-	_control_status.flags.ev_vel = true;
-	resetVelocityToVision();
-	_information_events.flags.starting_vision_vel_fusion = true;
-	ECL_INFO("starting vision vel 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();
-}
-
-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::stopEvVelFusion()
-{
-	if (_control_status.flags.ev_vel) {
-		ECL_INFO("stopping EV vel fusion");
-		_control_status.flags.ev_vel = false;
-		resetEstimatorAidStatus(_aid_src_ev_vel);
-	}
-}
-
-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");
@@ -1484,32 +1321,27 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 // Returns true if the reset was successful
 bool Ekf::resetYawToEKFGSF()
 {
-	if (!isYawEmergencyEstimateAvailable()) {
+	if (!isYawEmergencyEstimateAvailable() || !isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
 		return false;
 	}
 
-	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
-
-	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
-	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
-	_control_status.flags.yaw_align = true;
-
-	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
-		// and cause another navigation failure
-		_control_status.flags.mag_fault = true;
-		_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
-
-	} else if (_control_status.flags.gps_yaw) {
-		_control_status.flags.gps_yaw_fault = true;
-		_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
-
-	} else if (_control_status.flags.ev_yaw) {
-		_inhibit_ev_yaw_use = true;
+	// 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 false;
 	}
 
+	// otherwise reset yaw
+	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
+
+	_information_events.flags.yaw_aligned_to_imu_gps = true;
+	ECL_INFO("Yaw aligned using IMU and GPS");
+
+	_control_status.flags.yaw_align = true;
+
 	_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
-	_ekfgsf_yaw_reset_count++;
+
+	// reset mag states
+	resetMagStates();
 
 	return true;
 }
@@ -1546,6 +1378,11 @@ void Ekf::runYawEKFGSF()
 
 	const Vector3f imu_gyro_bias = getGyroBias();
 	_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
+
+	// reset when landed
+	if (!_control_status.flags.in_air) {
+		_ekfgsf_yaw_reset_count = 0;
+	}
 }
 
 void Ekf::resetGpsDriftCheckFilters()

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -496,7 +496,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 | SensorFusionMask::USE_EXT_VIS_VEL)) {
+	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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2015-2022 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -44,9 +44,9 @@
 
 #if defined(MODULE_NAME)
 #include <px4_platform_common/log.h>
-# define ECL_INFO PX4_DEBUG
-# define ECL_WARN PX4_DEBUG
-# define ECL_ERR  PX4_DEBUG
+# define ECL_INFO PX4_INFO // TODO
+# define ECL_WARN PX4_WARN // TODO
+# define ECL_ERR  PX4_ERR // TODO
 # define ECL_DEBUG PX4_DEBUG
 #else
 # define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__)
@@ -132,9 +132,6 @@ public:
 	// set vehicle is fixed wing status
 	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
 
-	// set flag if synthetic sideslip measurement should be fused
-	void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); }
-
 	// set flag if static pressure rise due to ground effect is expected
 	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
 	// flag will clear after GNDEFFECT_TIMEOUT uSec
@@ -259,7 +256,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; }
@@ -306,8 +302,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;

src/modules/ekf2/EKF/ev_control.cpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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);
+	_ev_hgt_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);
+
+		if (quality_sufficient) {
+			_last_ev_pass_us = _imu_sample_delayed.time_us;
+
+		} else {
+			_last_ev_fail_us = _imu_sample_delayed.time_us;
+		}
+
+		const bool ev_quality_passing = isTimedOut(_last_ev_fail_us, (uint64_t)1e6);
+		const bool ev_quality_failing = isTimedOut(_last_ev_pass_us, (uint64_t)1e6);
+
+		bool starting_conditions_passing = quality_sufficient
+						   && ev_quality_passing
+						   && !ev_quality_failing
+						   && !ev_reset
+						   && ((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
+						   && 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;
+			_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

@@ -38,74 +38,80 @@
 
 #include "ekf.h"
 
-void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
+void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset,
+				bool quality_sufficient, estimator_aid_source1d_s &aid_src)
 {
-	static constexpr const char *HGT_SRC_NAME = "EV";
+	static constexpr const char *AID_SRC_NAME = "EV height";
 
-	auto &aid_src = _aid_src_ev_hgt;
 	HeightBiasEstimator &bias_est = _ev_hgt_b_est;
+	//bias_est.predict(_dt_ekf_avg); // handled by controlExternalVisionFusion()
 
-	bias_est.predict(_dt_ekf_avg);
+	// 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;
 
-	if (_ev_data_ready) {
+	// rotate measurement into correct earth frame if required
+	Vector3f pos{ev_sample.pos};
+	Matrix3f pos_cov{matrix::diag(ev_sample.position_var)};
 
-		const float measurement = ev_sample.pos(2);
-		const float measurement_var = ev_sample.posVar(2);
+	// 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
+	if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) {
 
-		const float innov_gate = math::max(_params.ev_pos_innov_gate, 1.f);
+		const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
 
-		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
+		if (q_error.isAllFinite()) {
+			const Dcmf R_ev_to_ekf(q_error);
 
-		updateVerticalPositionAidSrcStatus(ev_sample.time_us,
-						   measurement - bias_est.getBias(),
-						   measurement_var + bias_est.getBiasVar(),
-						   innov_gate,
-						   aid_src);
+			pos = R_ev_to_ekf * ev_sample.pos;
+			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
 
-		// update the bias estimator before updating the main filter but after
-		// using its current state to compute the vertical position innovation
-		if (measurement_valid) {
-			bias_est.setMaxStateNoise(sqrtf(measurement_var));
-			bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
+			// increase minimum variance to include EV orientation variance
+			// TODO: do this properly
+			const float orientation_var_max = math::max(ev_sample.orientation_var(0), ev_sample.orientation_var(1));
+			pos_cov(2, 2) = math::max(pos_cov(2, 2), orientation_var_max);
 		}
+	}
 
-		const bool continuing_conditions_passing = ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) || (_params.height_sensor_ref == HeightSensor::EV)) // TODO: (_params.ev_ctrl & EvCtrl::VPOS)
-				&& measurement_valid;
+	const float measurement = pos(2) - pos_offset_earth(2);
+	float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
 
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL);
+	// increase minimum variance if GPS active
+	if (_control_status.flags.gps_hgt) {
+		measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
+	}
 
-		if (_control_status.flags.ev_hgt) {
-			aid_src.fusion_enabled = true;
+	const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 
-			if (continuing_conditions_passing) {
+	updateVerticalPositionAidSrcStatus(ev_sample.time_us,
+					   measurement - bias_est.getBias(),
+					   measurement_var + bias_est.getBiasVar(),
+					   math::max(_params.ev_pos_innov_gate, 1.f),
+					   aid_src);
 
-				fuseVerticalPosition(aid_src);
+	// 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) {
+		bias_est.setMaxStateNoise(sqrtf(measurement_var));
+		bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
+		bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
+	}
 
-				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
+	const bool continuing_conditions_passing = ((_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
+			|| (_params.height_sensor_ref == HeightSensor::EV))
+			&& measurement_valid;
 
-				if (isHeightResetRequired()) {
-					// All height sources are failing
-					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
+	starting_conditions_passing &= continuing_conditions_passing;
 
-					_information_events.flags.reset_hgt_to_ev = true;
-					resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
-					bias_est.setBias(-_state.pos(2) + measurement);
+	if (_control_status.flags.ev_hgt) {
+		aid_src.fusion_enabled = true;
 
-					// reset vertical velocity
-					if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) {
-						resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2));
+		if (continuing_conditions_passing) {
 
-					} else {
-						resetVerticalVelocityToZero();
-					}
+			if (ev_reset) {
 
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
-
-				} else if ((_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) && !aid_src.fused) {
-					// fusion failed and EV sample indicates reset
-					ECL_INFO("%s height reset", HGT_SRC_NAME);
+				if (quality_sufficient) {
+					ECL_INFO("reset to %s", AID_SRC_NAME);
 
 					if (_height_sensor_ref == HeightSensor::EV) {
 						_information_events.flags.reset_hgt_to_ev = true;
@@ -118,43 +124,91 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
 
 					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
 
-				} else if (is_fusion_failing) {
-					// Some other height source is still working
-					ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
+				} 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
 					stopEvHgtFusion();
+					return;
 				}
 
+			} else if (quality_sufficient) {
+				fuseVerticalPosition(aid_src);
+
 			} else {
-				ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
+				aid_src.innovation_rejected = true;
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
+
+			if (isHeightResetRequired() && quality_sufficient) {
+				// All height sources are failing
+				ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME);
+				_information_events.flags.reset_hgt_to_ev = true;
+				resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
+				bias_est.setBias(-_state.pos(2) + measurement);
+
+				// reset vertical velocity
+				if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
+
+					// correct velocity for offset relative to IMU
+					const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+					const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+
+					switch (ev_sample.vel_frame) {
+					case VelocityFrame::LOCAL_FRAME_NED:
+					case VelocityFrame::LOCAL_FRAME_FRD: {
+							const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
+							resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
+						}
+						break;
+
+					case VelocityFrame::BODY_FRAME_FRD: {
+							const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
+							const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
+							resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
+						}
+						break;
+					}
+
+				} else {
+					resetVerticalVelocityToZero();
+				}
+
+				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+			} else if (is_fusion_failing) {
+				// 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);
 				stopEvHgtFusion();
 			}
 
 		} else {
-			if (starting_conditions_passing) {
-				if (_params.height_sensor_ref == HeightSensor::EV) {
-					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::EV;
-
-					_information_events.flags.reset_hgt_to_ev = true;
-					resetVerticalPositionTo(measurement, measurement_var);
-					bias_est.reset();
-
-				} else {
-					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
-					bias_est.setBias(-_state.pos(2) + measurement);
-				}
-
-				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
-				bias_est.setFusionActive();
-				_control_status.flags.ev_hgt = true;
-			}
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
+			stopEvHgtFusion();
 		}
 
-	} else if (_control_status.flags.ev_hgt
-		   && !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
-		// No data anymore. Stop until it comes back.
-		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
-		stopEvHgtFusion();
+	} else {
+		if (starting_conditions_passing) {
+			// activate fusion, only reset if necessary
+			if (_params.height_sensor_ref == HeightSensor::EV) {
+				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+				_information_events.flags.reset_hgt_to_ev = true;
+				resetVerticalPositionTo(measurement, measurement_var);
+
+				_height_sensor_ref = HeightSensor::EV;
+				bias_est.reset();
+
+			} else {
+				ECL_INFO("starting %s fusion", AID_SRC_NAME);
+				bias_est.setBias(-_state.pos(2) + measurement);
+			}
+
+			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			bias_est.setFusionActive();
+			_control_status.flags.ev_hgt = true;
+		}
 	}
 }
 

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -0,0 +1,299 @@
+/****************************************************************************
+ *
+ *   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"
+
+void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset,
+			     bool quality_sufficient, estimator_aid_source2d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "EV position";
+
+	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);
+
+	// 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;
+	}
+
+	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());
+
+			if (ev_reset || yaw_alignment_changed || bias_estimator_change) {
+
+				if (quality_sufficient) {
+
+					if (!_control_status.flags.gps) {
+						ECL_INFO("reset to %s", 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", 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 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_pos_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopEvPosFusion();
+
+				} 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);
+					stopEvPosFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
+			stopEvPosFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			// activate fusion
+			// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
+			if (_control_status.flags.gps) {
+				ECL_INFO("starting %s fusion", 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", 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 = 3;
+			_information_events.flags.starting_vision_pos_fusion = true;
+			_control_status.flags.ev_pos = true;
+		}
+	}
+}
+
+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

@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   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_vel_control.cpp
+ * Control functions for ekf external vision velocity fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset,
+			     bool quality_sufficient, estimator_aid_source3d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "EV velocity";
+
+	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 velocity aiding
+	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
+					     && _control_status.flags.tilt_align
+					     && ev_sample.vel.isAllFinite();
+
+	// correct velocity for offset relative to IMU
+	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+
+	// rotate measurement into correct earth frame if required
+	Vector3f vel{NAN, NAN, NAN};
+	Matrix3f vel_cov{};
+
+	switch (ev_sample.vel_frame) {
+	case VelocityFrame::LOCAL_FRAME_NED:
+		if (_control_status.flags.yaw_align) {
+			vel = ev_sample.vel - vel_offset_earth;
+			vel_cov = matrix::diag(ev_sample.velocity_var);
+
+		} else {
+			continuing_conditions_passing = false;
+		}
+
+		break;
+
+	case VelocityFrame::LOCAL_FRAME_FRD:
+		if (_control_status.flags.ev_yaw) {
+			// using EV frame
+			vel = ev_sample.vel - vel_offset_earth;
+			vel_cov = matrix::diag(ev_sample.velocity_var);
+
+		} 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);
+
+			vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
+			vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_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++) {
+				vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max);
+			}
+		}
+
+		break;
+
+	case VelocityFrame::BODY_FRAME_FRD:
+		vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
+		vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
+		break;
+
+	default:
+		continuing_conditions_passing = false;
+		break;
+	}
+
+	// increase minimum variance if GPS active (position reference)
+	if (_control_status.flags.gps) {
+		for (int i = 0; i < 2; i++) {
+			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
+		}
+	}
+
+	const Vector3f measurement{vel};
+
+	const Vector3f measurement_var{
+		math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)),
+		math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)),
+		math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f))
+	};
+
+	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
+
+	updateVelocityAidSrcStatus(ev_sample.time_us,
+				   measurement,                               // observation
+				   measurement_var,                           // observation variance
+				   math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate
+				   aid_src);
+
+	if (!measurement_valid) {
+		continuing_conditions_passing = false;
+	}
+
+	starting_conditions_passing &= continuing_conditions_passing
+				       && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
+
+	if (_control_status.flags.ev_vel) {
+		aid_src.fusion_enabled = true;
+
+		if (continuing_conditions_passing) {
+
+			if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) {
+
+				if (quality_sufficient) {
+					ECL_INFO("reset to %s", AID_SRC_NAME);
+					_information_events.flags.reset_vel_to_vision = true;
+					resetVelocityTo(measurement, measurement_var);
+					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
+					stopEvVelFusion();
+					return;
+				}
+
+			} else if (quality_sufficient) {
+				fuseVelocity(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) {
+
+				if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) {
+					// Data seems good, attempt a reset
+					_information_events.flags.reset_vel_to_vision = true;
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetVelocityTo(measurement, measurement_var);
+					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_ev_vel_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_vel_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopEvVelFusion();
+
+				} 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);
+					stopEvVelFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
+			stopEvVelFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			// activate fusion, only reset if necessary
+			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
+				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+				_information_events.flags.reset_vel_to_vision = true;
+				resetVelocityTo(measurement, measurement_var);
+
+			} else {
+				ECL_INFO("starting %s fusion", AID_SRC_NAME);
+			}
+
+			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+
+			_nb_ev_vel_reset_available = 3;
+			_information_events.flags.starting_vision_vel_fusion = true;
+			_control_status.flags.ev_vel = true;
+		}
+	}
+}
+
+void Ekf::stopEvVelFusion()
+{
+	if (_control_status.flags.ev_vel) {
+		resetEstimatorAidStatus(_aid_src_ev_vel);
+
+		_control_status.flags.ev_vel = false;
+	}
+}

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ *   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, bool starting_conditions_passing, bool ev_reset,
+			     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 only allow EV yaw if EV is 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) - _yaw_pred_prev)
+					- wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat)));
+	}
+
+	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 = 3;
+			}
+		}
+	}
+}
+
+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

@@ -117,7 +117,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 					// reset vertical velocity
 					if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & GnssCtrl::VEL)) {
 						// use 1.5 as a typical ratio of vacc/hacc
-						resetVerticalVelocityTo(gps_sample.vel(2), sq(1.5f * gps_sample.sacc));
+						resetVerticalVelocityTo(gps_sample.vel(2), sq(1.5f * math::max(gps_sample.sacc, _params.gps_vel_noise)));
 
 					} else {
 						resetVerticalVelocityToZero();

src/modules/ekf2/EKF/gps_control.cpp

@@ -41,11 +41,6 @@
 
 void Ekf::controlGpsFusion()
 {
-	if (!((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
-		stopGpsFusion();
-		return;
-	}
-
 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {
 
@@ -59,16 +54,19 @@ void Ekf::controlGpsFusion()
 		controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
 
 		// GNSS velocity
-		const float vel_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
+		const Vector3f velocity{gps_sample.vel};
+		const float vel_noise = math::max(gps_sample.sacc, _params.gps_vel_noise);
+		const float vel_var = sq(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);
 
 		// 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);
 
@@ -83,17 +81,49 @@ 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);
 
 		// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
-		if (gps_checks_passing && !gps_checks_failing) {
-			_yawEstimator.setVelocity(gps_sample.vel.xy(), gps_sample.sacc);
+		if (_gps_speed_valid && velocity.isAllFinite()
+		    && (gps_sample.sacc > FLT_EPSILON) && (gps_sample.sacc <= _params.req_sacc)) {
+
+			_yawEstimator.setVelocity(velocity.xy(), vel_noise);
 		}
 
+		// allow GPS to perform yaw align or in flight mag alignment
+		if (_control_status.flags.tilt_align && _NED_origin_initialised
+		    && gps_checks_passing && !gps_checks_failing) {
+
+			if (!_control_status.flags.yaw_align
+			    || (_control_status.flags.mag_hdg && !_control_status.flags.mag_aligned_in_flight)
+			   ) {
+				if (resetYawToEKFGSF()) {
+					// Yaw aligned using IMU and GPS
+
+				} else if (resetYawToGps(gps_sample.yaw)) {
+					ECL_INFO("Yaw aligned using GPS yaw");
+
+				} else if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align && !_control_status.flags.in_air) {
+
+					// 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))
@@ -102,7 +132,12 @@ void Ekf::controlGpsFusion()
 				&& _NED_origin_initialised;
 
 		const bool continuing_conditions_passing = mandatory_conditions_passing && !gps_checks_failing;
-		const bool starting_conditions_passing = continuing_conditions_passing && gps_checks_passing;
+
+		const bool starting_conditions_passing = continuing_conditions_passing
+				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)
+				&& _gps_checks_passed
+				&& gps_checks_passing
+				&& !gps_checks_failing;
 
 		if (_control_status.flags.gps) {
 			if (mandatory_conditions_passing) {
@@ -112,42 +147,72 @@ void Ekf::controlGpsFusion()
 					fuseVelocity(_aid_src_gnss_vel);
 					fuseHorizontalPosition(_aid_src_gnss_pos);
 
-					if (shouldResetGpsFusion()) {
-						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
+					const bool is_vel_fusion_failing = isTimedOut(_aid_src_gnss_vel.time_last_fuse, _params.reset_timeout_max);
+					const bool is_pos_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse, _params.reset_timeout_max);
 
-						/* A reset is not performed when getting GPS back after a significant period of no data
-						 * because the timeout could have been caused by bad GPS.
-						 * The total number of resets allowed per boot cycle is limited.
-						 */
-						if (isYawFailure()
-						    && _control_status.flags.in_air
-						    && !was_gps_signal_lost
-						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
-						    && isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
+					// A reset is not performed when getting GPS back after a significant period of no data because the timeout could have been caused by bad GPS.
+					// The total number of resets allowed per boot cycle is limited.
+					const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
+
+					const bool yaw_failure = isYawFailure() && !was_gps_signal_lost
+								 && (_ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit)
+								 && isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000);
+
+					const bool should_reset_gps_fusion = shouldResetGpsFusion();
+
+					if (should_reset_gps_fusion || (yaw_failure && (is_vel_fusion_failing || is_pos_fusion_failing))) {
+
+						bool yaw_reset = false;
+
+						if (yaw_failure) {
 							// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
 							// to improve its estimate if the previous reset was not successful.
 							if (resetYawToEKFGSF()) {
 								ECL_WARN("GPS emergency yaw reset");
+								_ekfgsf_yaw_reset_count++;
+								yaw_reset = true;
+
+								if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+									// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
+									// and cause another navigation failure
+									if (_ekfgsf_yaw_reset_count > 1) {
+										_control_status.flags.mag_fault = true;
+										_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
+									}
+								}
+
+								if (_control_status.flags.gps_yaw) {
+									_control_status.flags.gps_yaw_fault = true;
+									_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = 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;
+								}
 							}
 						}
 
-						ECL_WARN("GPS fusion timeout, resetting velocity and position");
-						_information_events.flags.reset_vel_to_gps = true;
-						_information_events.flags.reset_pos_to_gps = true;
-						resetVelocityTo(gps_sample.vel, vel_obs_var);
-						resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
+						if (should_reset_gps_fusion || is_vel_fusion_failing || yaw_reset) {
+							// 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;
+						}
+
+						if (should_reset_gps_fusion || is_pos_fusion_failing || yaw_reset) {
+							// 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;
+						}
 					}
 
 				} else {
 					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
@@ -158,54 +223,42 @@ 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;
 				}
 
-			} 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");
+				// 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;
 
-					ECL_INFO("reset velocity and position to GPS");
+				// 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;
-					_information_events.flags.reset_pos_to_gps = true;
-					resetVelocityTo(gps_sample.vel, vel_obs_var);
-					resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
+					resetVelocityTo(velocity, vel_obs_var);
+					_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
 				}
+
+				_information_events.flags.starting_gps_fusion = true;
+				ECL_INFO("starting GPS fusion");
+				_control_status.flags.gps = true;
 			}
 		}
 
-	} else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)10e6)) {
+		_time_prev_gps_us = _gps_sample_delayed.time_us;
+
+	} else if (_control_status.flags.gps && isTimedOut(_time_prev_gps_us, (uint64_t)10e6)) {
 		stopGpsFusion();
 		_warning_events.flags.gps_data_stopped = true;
 		ECL_WARN("GPS data stopped");
 
-	}  else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)1e6)
+	}  else if (_control_status.flags.gps && isTimedOut(_time_prev_gps_us, 2 * GPS_MAX_INTERVAL)
 		    && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 
 		// Handle the case where we are fusing another position source along GPS,

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -203,23 +203,36 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
 
 bool Ekf::resetYawToGps(const float gnss_yaw)
 {
-	// define the predicted antenna array vector and rotate into earth frame
-	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
-
-	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+	// 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 false;
 	}
 
-	// GPS yaw measurement is alreday compensated for antenna offset in the driver
-	const float measured_yaw = gnss_yaw;
+	if (PX4_ISFINITE(gnss_yaw) && !_control_status.flags.gps_yaw_fault) {
+		// define the predicted antenna array vector and rotate into earth frame
+		const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+		const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
 
-	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
-	resetQuatStateYaw(measured_yaw, yaw_variance);
+		// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+		if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+			return false;
+		}
 
-	_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
-	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
+		// GPS yaw measurement is alreday compensated for antenna offset in the driver
+		const float measured_yaw = gnss_yaw;
 
-	return true;
+		const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
+		resetQuatStateYaw(measured_yaw, yaw_variance);
+
+		_control_status.flags.yaw_align = true;
+
+		_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
+		_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
+
+		resetMagStates();
+
+		return true;
+	}
+
+	return false;
 }

src/modules/ekf2/EKF/height_control.cpp

@@ -46,7 +46,6 @@ void Ekf::controlHeightFusion()
 	controlBaroHeightFusion();
 	controlGnssHeightFusion(_gps_sample_delayed);
 	controlRangeHeightFusion();
-	controlEvHeightFusion(_ev_sample_delayed);
 
 	checkHeightSensorRefFallback();
 }
@@ -62,6 +61,7 @@ void Ekf::checkHeightSensorRefFallback()
 
 	switch (_params.height_sensor_ref) {
 	default:
+
 	/* FALLTHROUGH */
 	case HeightSensor::UNKNOWN:
 		fallback_list[0] = HeightSensor::GNSS;
@@ -150,12 +150,18 @@ void Ekf::checkVerticalAccelerationHealth()
 
 	// declare a bad vertical acceleration measurement and make the declaration persist
 	// for a minimum of BADACC_PROBATION seconds
+	const bool bad_acc_vertical = _fault_status.flags.bad_acc_vertical;
+
 	if (_fault_status.flags.bad_acc_vertical) {
 		_fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
 
 	} else {
 		_fault_status.flags.bad_acc_vertical = bad_vert_accel;
 	}
+
+	if (!bad_acc_vertical && _fault_status.flags.bad_acc_vertical) {
+		ECL_WARN("bad vertical acceleration");
+	}
 }
 
 Likelihood Ekf::estimateInertialNavFallingLikelihood() const
@@ -172,6 +178,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 		bool failed_min;
 		bool failed_lim;
 	} checks[6] {};
+	static constexpr size_t NUM_CHECKS = sizeof(checks) / sizeof(checks[0]);
 
 	if (_control_status.flags.baro_hgt) {
 		checks[0] = {ReferenceType::PRESSURE, _aid_src_baro_hgt.innovation, _aid_src_baro_hgt.innovation_variance};
@@ -198,7 +205,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 	}
 
 	// Compute the check based on innovation ratio for all the sources
-	for (unsigned i = 0; i < 6; i++) {
+	for (unsigned i = 0; i < NUM_CHECKS; i++) {
 		if (checks[i].innov_var < FLT_EPSILON) {
 			continue;
 		}
@@ -209,13 +216,13 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 	}
 
 	// Check all the sources agains each other
-	for (unsigned i = 0; i < 6; i++) {
+	for (unsigned i = 0; i < NUM_CHECKS; i++) {
 		if (checks[i].failed_lim) {
 			// There is a chance that the inertial nav is falling if one source is failing the test
 			likelihood_medium = true;
 		}
 
-		for (unsigned j = 0; j < 6; j++) {
+		for (unsigned j = 0; j < NUM_CHECKS; j++) {
 
 			if ((checks[i].ref_type != checks[j].ref_type) && checks[i].failed_lim && checks[j].failed_min) {
 				// There is a high chance that the inertial nav is failing if two sources are failing the test

src/modules/ekf2/EKF/mag_control.cpp

@@ -75,7 +75,7 @@ void Ekf::controlMagFusion()
 
 			resetEstimatorAidStatus(_aid_src_mag_heading);
 			_aid_src_mag_heading.timestamp_sample = mag_sample.time_us;
-			_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();;
+			_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 			_aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation);
 
 			// compute magnetometer innovations (for estimator_aid_src_mag logging)
@@ -104,17 +104,13 @@ void Ekf::controlMagFusion()
 		return;
 	}
 
-	_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
-	_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
-
 	if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
 
-		const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D;
-
 		// Determine if we should use simple magnetic heading fusion which works better when
 		// there are large external disturbances or the more accurate 3-axis fusion
 		switch (_params.mag_fusion_type) {
 		default:
+
 		// FALLTHROUGH
 		case MagFuseType::AUTO:
 			selectMagAuto();
@@ -132,18 +128,10 @@ void Ekf::controlMagFusion()
 			break;
 		}
 
-		const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
-
-		if (!mag_enabled_previously && mag_enabled) {
-			_mag_yaw_reset_req = true;
-		}
-
-		if (_control_status.flags.in_air) {
-			checkHaglYawResetReq();
-			runInAirYawReset();
-
-		} else {
-			runOnGroundYawReset();
+		if ((_control_status.flags.mag_hdg || _control_status.flags.mag_3D)
+		    && (!_control_status.flags.yaw_align || _mag_yaw_reset_req || haglYawResetReq())
+		   ) {
+			runYawReset();
 		}
 
 		if (!_control_status.flags.yaw_align) {
@@ -152,43 +140,24 @@ void Ekf::controlMagFusion()
 		}
 
 		checkMagDeclRequired();
-		checkMagInhibition();
 
 		runMagAndMagDeclFusions(mag_sample.mag);
 	}
 }
 
-void Ekf::checkHaglYawResetReq()
+bool Ekf::haglYawResetReq() const
 {
 	// We need to reset the yaw angle after climbing away from the ground to enable
 	// recovery from ground level magnetic interference.
-	if (!_control_status.flags.mag_aligned_in_flight) {
+	if (_control_status.flags.in_air && !_control_status.flags.mag_aligned_in_flight && isTerrainEstimateValid()) {
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
 		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
-		_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
+		return above_mag_anomalies;
 	}
-}
 
-void Ekf::runOnGroundYawReset()
-{
-	if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
-		const bool has_realigned_yaw = canResetMagHeading() ? resetMagHeading() : false;
-
-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
-
-			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
-			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
-			if (_mag_inhibit_yaw_reset_req) {
-				_mag_inhibit_yaw_reset_req = false;
-				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
-				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
-			}
-		}
-	}
+	return false;
 }
 
 bool Ekf::canResetMagHeading() const
@@ -196,66 +165,48 @@ bool Ekf::canResetMagHeading() const
 	return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MagFuseType::NONE);
 }
 
-void Ekf::runInAirYawReset()
+void Ekf::runYawReset()
 {
-	if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
-		bool has_realigned_yaw = false;
+	// 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;
+	}
 
-		// use yaw estimator if available
-		if (_control_status.flags.gps && isYawEmergencyEstimateAvailable() &&
-		(_mag_counter != 0) && isNewestSampleRecent(_time_last_mag_buffer_push, 500'000) // mag LPF available
-		) {
+	bool has_realigned_yaw = false;
 
-			resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
-
-			_information_events.flags.yaw_aligned_to_imu_gps = true;
-			_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
-
-			// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-			if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
-				// use predicted earth field to reset states
-				const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
-				_state.mag_I = mag_earth_pred;
-
-				const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-				_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
-
-			} else {
-				// Use the last magnetometer measurements to reset the field states
-				// calculate initial earth magnetic field states
-				_state.mag_I = _R_to_earth * _mag_lpf.getState();
-				_state.mag_B.zero();
-			}
-
-			ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]",
-				(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
-				(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
-				);
-
-			resetMagCov();
+	// use yaw estimator if available
+	if (isYawEmergencyEstimateAvailable()) {
+		if (resetYawToEKFGSF()) {
+			has_realigned_yaw = true;
 		}
+	}
 
-		if (!has_realigned_yaw && canResetMagHeading()) {
-			has_realigned_yaw = resetMagHeading();
-		}
+	if (!has_realigned_yaw && canResetMagHeading()) {
+		has_realigned_yaw = resetMagHeading();
+	}
 
-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
+	if (has_realigned_yaw) {
+		_mag_yaw_reset_req = false;
+		_control_status.flags.yaw_align = true;
+
+
+		if (_control_status.flags.in_air) {
 			_control_status.flags.mag_aligned_in_flight = true;
-
 			// record the time for the magnetic field alignment event
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
-
-			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
-			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
-			if (_mag_inhibit_yaw_reset_req) {
-				_mag_inhibit_yaw_reset_req = false;
-				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
-				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
-			}
 		}
 
+		// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
+		// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
+		if (isTimedOut(_aid_src_mag_heading.time_last_fuse, (uint32_t)5e6)
+		    && isTimedOut(_aid_src_mag.time_last_fuse, (uint32_t)5e6)
+		    && isTimedOut(_aid_src_gnss_yaw.time_last_fuse, (uint32_t)5e6)
+		    && isTimedOut(_aid_src_ev_yaw.time_last_fuse, (uint32_t)5e6)
+		   ) {
+			// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
+			P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg));
+		}
 	}
 }
 
@@ -284,7 +235,7 @@ void Ekf::checkYawAngleObservability()
 				: _accel_lpf_NE.norm() > 2.0f * _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?
+				&& (_control_status.flags.gps || (_control_status.flags.ev_pos && _control_status.flags.yaw_align));
 }
 
 void Ekf::checkMagBiasObservability()
@@ -324,20 +275,6 @@ void Ekf::checkMagDeclRequired()
 	_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
 }
 
-void Ekf::checkMagInhibition()
-{
-	_is_yaw_fusion_inhibited = shouldInhibitMag();
-
-	if (!_is_yaw_fusion_inhibited) {
-		_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
-	}
-
-	// If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading
-	if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
-		_mag_inhibit_yaw_reset_req = true;
-	}
-}
-
 bool Ekf::shouldInhibitMag() const
 {
 	// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
@@ -347,9 +284,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;
 }
@@ -384,7 +319,7 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 	if (_control_status.flags.mag_3D) {
 		run3DMagAndDeclFusions(mag);
 
-	} else if (_control_status.flags.mag_hdg && !_is_yaw_fusion_inhibited) {
+	} else if (_control_status.flags.mag_hdg && !shouldInhibitMag()) {
 		// Rotate the measurements into earth frame using the zero yaw angle
 		Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
 
@@ -408,7 +343,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
 	// before they are used to constrain heading drift
 	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6)
-			&& !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed;
+				       && !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed;
 
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state
@@ -430,3 +365,69 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		}
 	}
 }
+
+bool Ekf::resetMagStates()
+{
+	bool reset = false;
+
+	// reinit mag states
+	const bool mag_available = (_mag_counter != 0) && isNewestSampleRecent(_time_last_mag_buffer_push, 500'000);
+
+	// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
+	if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
+		// use predicted earth field to reset states
+		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps,
+						     _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+		_state.mag_I = mag_earth_pred;
+
+		// TODO: ECL_DEBUG
+		ECL_INFO("resetting mag I to [%.3f, %.3f, %.3f]",
+			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
+
+		if (mag_available) {
+			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+			_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
+
+			// TODO: ECL_DEBUG
+			ECL_INFO("resetting mag B to [%.3f, %.3f, %.3f]",
+				 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2));
+
+		} else {
+			_state.mag_B.zero();
+		}
+
+		reset = true;
+
+	} else if (mag_available) {
+		// Use the last magnetometer measurements to reset the field states
+
+		// calculate initial earth magnetic field states
+		_state.mag_I = _R_to_earth * _mag_lpf.getState();
+		_state.mag_B.zero();
+
+		// TODO: ECL_DEBUG
+		ECL_INFO("resetting mag I to [%.3f, %.3f, %.3f]",
+			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
+
+		reset = true;
+	}
+
+	if (reset) {
+		resetMagCov();
+
+		if (mag_available) {
+			if (_control_status.flags.in_air) {
+				// record the start time for the magnetic field alignment
+				_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+				_control_status.flags.mag_aligned_in_flight = true;
+			}
+
+			// clear any pending resets
+			_mag_yaw_reset_req = false;
+		}
+
+		return true;
+	}
+
+	return false;
+}

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -50,6 +50,7 @@ void Ekf::controlOpticalFlowFusion()
 
 	// Accumulate autopilot gyro data across the same time interval as the flow sensor
 	const Vector3f delta_angle(_imu_sample_delayed.delta_ang - (getGyroBias() * _imu_sample_delayed.delta_ang_dt));
+
 	if (_delta_time_of < 0.1f) {
 		_imu_del_ang_of += delta_angle;
 		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
@@ -142,10 +143,14 @@ void Ekf::controlOpticalFlowFusion()
 		const bool preflight_motion_not_ok = !_control_status.flags.in_air
 						     && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
 								     || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
+
 		const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
 
+		const bool starting_conditions_passing = (_flow_sample_delayed.quality >= _params.flow_qual_min)
+				&& isTerrainEstimateValid();
+
 		const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
-				    || !_control_status.flags.tilt_align;
+					      || !_control_status.flags.tilt_align;
 
 		// Handle cases where we are using optical flow but we should not use it anymore
 		if (_control_status.flags.opt_flow) {
@@ -160,7 +165,8 @@ void Ekf::controlOpticalFlowFusion()
 		// optical flow fusion mode selection logic
 		if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
 		    && !_control_status.flags.opt_flow // we are not yet using flow data
-		    && !inhibit_flow_use) {
+		    && !inhibit_flow_use
+		    && starting_conditions_passing) {
 
 			// set the flag and reset the fusion timeout
 			ECL_INFO("starting optical flow fusion");
@@ -195,7 +201,7 @@ void Ekf::controlOpticalFlowFusion()
 			if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
 				// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
-				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
+				if (isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
 					fuseOptFlow();
 					_last_known_pos.xy() = _state.pos.xy();
 				}
@@ -204,8 +210,10 @@ void Ekf::controlOpticalFlowFusion()
 			}
 
 			// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
-			if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
-			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
+			if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)
+			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)
+			    && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)
+			   ) {
 
 				ECL_INFO("reset velocity to flow");
 				_information_events.flags.reset_vel_to_flow = true;

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

@@ -163,7 +163,7 @@ bool Ekf::isConditionalRangeAidSuitable()
 		float range_hagl_max = _params.max_hagl_for_range_aid;
 		float max_vel_xy = _params.max_vel_for_range_aid;
 
-		const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var));
+		const float hagl_test_ratio = (sq(_hagl_innov) / (sq(_params.range_aid_innov_gate) * _hagl_innov_var));
 
 		bool is_hagl_stable = (hagl_test_ratio < 1.f);
 
@@ -174,7 +174,6 @@ bool Ekf::isConditionalRangeAidSuitable()
 		}
 
 		const float range_hagl = _terrain_vpos - _state.pos(2);
-
 		const bool is_in_range = (range_hagl < range_hagl_max);
 
 		bool is_below_max_speed = true;

src/modules/ekf2/EKF2.cpp

@@ -123,6 +123,11 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
 	_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
 	_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
+	_param_ekf2_ev_ctrl(_params->ev_ctrl),
+	_param_ekf2_ev_qmin(_params->ev_quality_minimum),
+	_param_ekf2_evp_noise(_params->ev_pos_noise),
+	_param_ekf2_evv_noise(_params->ev_vel_noise),
+	_param_ekf2_eva_noise(_params->ev_att_noise),
 	_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
 	_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
 	_param_ekf2_of_n_min(_params->flow_noise),
@@ -145,6 +150,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
 	_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
 	_param_ekf2_arsp_thr(_params->arsp_thr),
+	_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
 	_param_ekf2_tau_vel(_params->vel_Tau),
 	_param_ekf2_tau_pos(_params->pos_Tau),
 	_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
@@ -201,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();
@@ -532,9 +582,6 @@ void EKF2::Run()
 			if (_status_sub.copy(&vehicle_status)) {
 				const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
 
-				// only fuse synthetic sideslip measurements if conditions are met
-				_ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
-
 				// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
 				_ekf.set_is_fixed_wing(is_fixed_wing);
 
@@ -589,14 +636,12 @@ 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);
 
-		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();
 
@@ -612,7 +657,7 @@ void EKF2::Run()
 			PublishBaroBias(now);
 			PublishGnssHgtBias(now);
 			PublishRngHgtBias(now);
-			PublishEvHgtBias(now);
+			PublishEvPosBias(now);
 			PublishEventFlags(now);
 			PublishGpsStatus(now);
 			PublishInnovations(now);
@@ -636,11 +681,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);
 	}
@@ -707,6 +747,43 @@ void EKF2::VerifyParams()
 		events::send<float>(events::ID("ekf2_hgt_ref_gps"), events::Log::Warning,
 				    "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get());
 	}
+
+	// EV EKF2_AID_MASK -> EKF2_EV_CTRL
+	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)
+	   ) {
+
+		// 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");
+		/* EVENT
+		 * @description <param>EKF2_AID_MASK</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_aid_mask_ev"), events::Log::Warning,
+				    "Use EKF2_EV_CTRL instead", _param_ekf2_aid_mask.get());
+	}
 }
 
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
@@ -813,15 +890,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);
 		}
 	}
 }
@@ -1229,43 +1326,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
@@ -1719,12 +1779,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");
@@ -1738,45 +1800,44 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		ev_data.vel.setNaN();
 		ev_data.quat.setNaN();
 
-		// if error estimates are unavailable, use parameter defined defaults
-
 		// check for valid velocity data
-		if (Vector3f(ev_odom.velocity).isAllFinite()) {
-			bool velocity_valid = true;
+		const Vector3f ev_odom_vel(ev_odom.velocity);
+		const Vector3f ev_odom_vel_var(ev_odom.velocity_variance);
+
+		if (ev_odom_vel.isAllFinite()) {
+			bool velocity_frame_valid = false;
 
 			switch (ev_odom.velocity_frame) {
-			// case vehicle_odometry_s::VELOCITY_FRAME_NED:
-			// 	ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
-			// 	break;
+			case vehicle_odometry_s::VELOCITY_FRAME_NED:
+				ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
+				velocity_frame_valid = true;
+				break;
 
 			case vehicle_odometry_s::VELOCITY_FRAME_FRD:
 				ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
+				velocity_frame_valid = true;
 				break;
 
 			case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD:
 				ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
-				break;
-
-			default:
-				velocity_valid = false;
+				velocity_frame_valid = true;
 				break;
 			}
 
-			if (velocity_valid) {
-				ev_data.vel(0) = ev_odom.velocity[0];
-				ev_data.vel(1) = ev_odom.velocity[1];
-				ev_data.vel(2) = ev_odom.velocity[2];
+			if (velocity_frame_valid) {
+				ev_data.vel = ev_odom_vel;
 
 				const float evv_noise_var = sq(_param_ekf2_evv_noise.get());
 
 				// velocity measurement error from ev_data or parameters
-				if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.velocity_variance).isAllFinite()) {
-					ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]);
-					ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]);
-					ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]);
+				if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_vel_var.isAllFinite()) {
+
+					ev_data.velocity_var(0) = fmaxf(evv_noise_var, ev_odom_vel_var(0));
+					ev_data.velocity_var(1) = fmaxf(evv_noise_var, ev_odom_vel_var(1));
+					ev_data.velocity_var(2) = fmaxf(evv_noise_var, ev_odom_vel_var(2));
 
 				} else {
-					ev_data.velVar.setAll(evv_noise_var);
+					ev_data.velocity_var.setAll(evv_noise_var);
 				}
 
 				new_ev_odom = true;
@@ -1784,38 +1845,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;
@@ -1823,23 +1884,34 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		}
 
 		// check for valid orientation data
-		if ((Quatf(ev_odom.q).isAllFinite())
-		    && ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f)
-			|| (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f))
-		   ) {
-			ev_data.quat = Quatf(ev_odom.q);
+		const Quatf ev_odom_q(ev_odom.q);
+		const Vector3f ev_odom_q_var(ev_odom.orientation_variance);
+		const bool non_zero = (fabsf(ev_odom_q(0)) > 0.f) || (fabsf(ev_odom_q(1)) > 0.f)
+				      || (fabsf(ev_odom_q(2)) > 0.f) || (fabsf(ev_odom_q(3)) > 0.f);
+		const float eps = 1e-5f;
+		const bool no_element_larger_than_one = (fabsf(ev_odom_q(0)) <= 1.f + eps)
+							&& (fabsf(ev_odom_q(1)) <= 1.f + eps)
+							&& (fabsf(ev_odom_q(2)) <= 1.f + eps)
+							&& (fabsf(ev_odom_q(3)) <= 1.f + eps);
+		const bool norm_in_tolerance = fabsf(1.f - ev_odom_q.norm()) <= eps;
+
+		const bool orientation_valid = ev_odom_q.isAllFinite() && non_zero && no_element_larger_than_one && norm_in_tolerance;
+
+		if (orientation_valid) {
+			ev_data.quat = ev_odom_q;
 			ev_data.quat.normalize();
 
 			// orientation measurement error from ev_data or parameters
 			const float eva_noise_var = sq(_param_ekf2_eva_noise.get());
 
-			if (!_param_ekf2_ev_noise_md.get() &&
-			    PX4_ISFINITE(ev_odom.orientation_variance[2])
-			   ) {
-				ev_data.angVar = fmaxf(eva_noise_var, ev_odom.orientation_variance[2]);
+			if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_q_var.isAllFinite()) {
+
+				ev_data.orientation_var(0) = fmaxf(eva_noise_var, ev_odom_q_var(0));
+				ev_data.orientation_var(1) = fmaxf(eva_noise_var, ev_odom_q_var(1));
+				ev_data.orientation_var(2) = fmaxf(eva_noise_var, ev_odom_q_var(2));
 
 			} else {
-				ev_data.angVar = eva_noise_var;
+				ev_data.orientation_var.setAll(eva_noise_var);
 			}
 
 			new_ev_odom = true;
@@ -1848,7 +1920,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		// use timestamp from external computer, clocks are synchronized when using MAVROS
 		ev_data.time_us = ev_odom.timestamp_sample;
 		ev_data.reset_counter = ev_odom.reset_counter;
-		//ev_data.quality = ev_odom.quality;
+		ev_data.quality = ev_odom.quality;
 
 		if (new_ev_odom)  {
 			_ekf.setExtVisionData(ev_data);

src/modules/ekf2/EKF2.hpp

@@ -69,6 +69,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 +143,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 +166,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);
@@ -290,7 +291,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};
@@ -339,7 +340,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)};
@@ -349,7 +350,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)};
 
@@ -520,13 +520,15 @@ private:
 		_param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD)
 
 		// vision estimate fusion
+		(ParamExtInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl,	 ///< external vision (EV) control selection
 		(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
 		_param_ekf2_ev_noise_md,	///< determine source of vision observation noise
-		(ParamFloat<px4::params::EKF2_EVP_NOISE>)
+		(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
+		(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)
 		_param_ekf2_evp_noise,	///< default position observation noise for exernal vision measurements (m)
-		(ParamFloat<px4::params::EKF2_EVV_NOISE>)
+		(ParamExtFloat<px4::params::EKF2_EVV_NOISE>)
 		_param_ekf2_evv_noise,	///< default velocity observation noise for exernal vision measurements (m/s)
-		(ParamFloat<px4::params::EKF2_EVA_NOISE>)
+		(ParamExtFloat<px4::params::EKF2_EVA_NOISE>)
 		_param_ekf2_eva_noise,	///< default angular observation noise for exernal vision measurements (rad)
 		(ParamExtFloat<px4::params::EKF2_EVV_GATE>)
 		_param_ekf2_evv_gate,	///< external vision velocity innovation consistency gate size (STD)
@@ -569,7 +571,7 @@ private:
 		// control of airspeed and sideslip fusion
 		(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
 		_param_ekf2_arsp_thr, 	///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
-		(ParamInt<px4::params::EKF2_FUSE_BETA>)
+		(ParamExtInt<px4::params::EKF2_FUSE_BETA>)
 		_param_ekf2_fuse_beta,		///< Controls synthetic sideslip fusion, 0 disables, 1 enables
 
 		// output predictor filter time constants

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -73,20 +73,35 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
 {
 	bool has_failed = false;
 
-	if (_is_using_gps_aiding || _is_using_ev_vel_aiding) {
-		const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])),
-						       fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1])));
-		Vector2f vel_ne_innov_lpf;
-		vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
-		vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
+	if (_is_using_gps_aiding) {
+		const Vector2f vel_ne_innov{fabsf(innov.gps_hvel[0]), fabsf(innov.gps_hvel[1])};
+
+		const Vector2f vel_ne_innov_lpf {
+			_filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim),
+			_filter_vel_e_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim)
+		};
+
+		has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
+	}
+
+	if (_is_using_ev_vel_aiding) {
+		const Vector2f vel_ne_innov{fabsf(innov.ev_hvel[0]), fabsf(innov.ev_hvel[1])};
+
+		const Vector2f vel_ne_innov_lpf {
+			_filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim),
+			_filter_vel_e_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim)
+		};
+
 		has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
 	}
 
 	if (_is_using_flow_aiding) {
-		const Vector2f flow_innov = Vector2f(innov.flow);
-		Vector2f flow_innov_lpf;
-		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
-		flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
+		const Vector2f flow_innov(innov.flow);
+		const Vector2f flow_innov_lpf{
+			_filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim),
+			_filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim)
+		};
+
 		has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim);
 	}
 
@@ -95,9 +110,21 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
 
 bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha)
 {
-	const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel));     // only temporary solution
-	const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
-	return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
+	bool has_failed = false;
+
+	if (_is_using_gps_aiding) {
+		const float vel_d_innov = fabsf(innov.gps_vvel);
+		const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
+		has_failed |= checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
+	}
+
+	if (_is_using_ev_vel_aiding) {
+		const float vel_d_innov = fabsf(innov.ev_vvel);
+		const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
+		has_failed |= checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
+	}
+
+	return has_failed;
 }
 
 bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)

src/modules/ekf2/ekf2_params.c

@@ -603,12 +603,12 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * 0 : Deprecated, use EKF2_GPS_CTRL instead
  * 1 : Set to true to use optical flow data if available
  * 2 : Set to true to inhibit IMU delta velocity bias estimation
- * 3 : Set to true to enable vision position fusion
- * 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
+ * 3 : Deprecated, use EKF2_EV_CTRL instead
+ * 4 : Deprecated, use EKF2_EV_CTRL instead
  * 5 : Set to true to enable multi-rotor drag specific force fusion
- * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
+ * 6 : Deprecated, use EKF2_EV_CTRL instead
  * 7 : Deprecated, use EKF2_GPS_CTRL instead
- * 8 : Set to true to enable vision velocity fusion
+ * 3 : Deprecated, use EKF2_EV_CTRL instead
  *
  * @group EKF2
  * @min 0
@@ -616,12 +616,12 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * @bit 0 unused
  * @bit 1 use optical flow
  * @bit 2 inhibit IMU bias estimation
- * @bit 3 vision position fusion
- * @bit 4 vision yaw fusion
+ * @bit 3 unused
+ * @bit 4 unused
  * @bit 5 multi-rotor drag fusion
- * @bit 6 rotate external vision
+ * @bit 6 unused
  * @bit 7 unused
- * @bit 8 vision velocity fusion
+ * @bit 8 unused
  * @reboot_required true
  */
 PARAM_DEFINE_INT32(EKF2_AID_MASK, 0);
@@ -654,6 +654,25 @@ PARAM_DEFINE_INT32(EKF2_HGT_REF, 1);
  */
 PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1);
 
+/**
+ * External vision (EV) sensor aiding
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Horizontal position fusion
+ * 1 : Vertical position fusion
+ * 2 : 3D velocity fusion
+ * 3 : Yaw
+ *
+ * @group EKF2
+ * @min 0
+ * @max 15
+ * @bit 0 Horizontal position
+ * @bit 1 Vertical position
+ * @bit 2 3D velocity
+ * @bit 3 Yaw
+ */
+PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15);
+
 /**
  * GNSS sensor aiding
  *
@@ -770,15 +789,30 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
 PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
 
 /**
- * Whether to set the external vision observation noise from the parameter or from vision message
+ * External vision (EV) noise mode
  *
- * If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound.
+ * If set to 0 (default) the measurement noise is taken from the vision message and the EV noise parameters are used as a lower bound.
+ * If set to 1 the observation noise is set from the parameters directly,
  *
- * @boolean
+ * @value 0 EV reported variance (parameter lower bound)
+ * @value 1 EV noise parameters
  * @group EKF2
  */
 PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0);
 
+/**
+ * External vision (EV) minimum quality (optional)
+ *
+ * External vision will only be started and fused if the quality metric is above this threshold.
+ * The quality metric is a completely optional field provided by some VIO systems.
+ *
+ * @group EKF2
+ * @min 0
+ * @max 100
+ * @decimal 1
+ */
+PARAM_DEFINE_INT32(EKF2_EV_QMIN, 0);
+
 /**
  * Measurement noise for vision position observations used to lower bound or replace the uncertainty included in the message
  *
@@ -807,7 +841,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
  * @unit rad
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f);
+PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f);
 
 /**
  * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
@@ -1112,10 +1146,10 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);
  *
  * @group EKF2
  * @min 0.1
- * @max 2
+ * @max 3
  * @unit m/s
  */
-PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f);
+PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 2.0f);
 
 /**
  * Maximum absolute altitude (height above ground level) allowed for conditional range aid mode.