ORB queue depth completely static

We had a special handling for RTL triggered in vtol_takeoff state.
The idea is to wait until the VTOL Takeoff is completed and only
then switch to RTL. On a second thought this special handling isn't
really necessary and for the sake of simplicity should be removed.
This also removes the side effect of the indicated flight mode
after RTL being set to VTOL_Takeoff again.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

.ci/Jenkinsfile-compile

@@ -120,7 +120,7 @@ pipeline {
                      "px4_fmu-v6xrt_rover",
                      "px4_io-v2_default",
                      "raspberrypi_pico_default",
-                     "siyi_n7_default"
+                     "siyi_n7_default",
                      "sky-drones_smartap-airlink_default",
                      "spracing_h7extreme_default",
                      "thepeach_k1_default",

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -0,0 +1,80 @@
+#!/bin/sh
+# @name Gazebo lawnmower
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_differential_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+# We can arm and drive in manual mode when it slides and GPS check fails:
+param set-default COM_ARM_WO_GPS 1
+
+# Set Differential Drive Kinematics Library parameters:
+param set RDD_WHEEL_BASE 0.9
+param set RDD_WHEEL_RADIUS 0.22
+param set RDD_WHEEL_SPEED 10.0	# Maximum wheel speed rad/s, approx 8 km/h
+
+# Actuator mapping - set SITL motors/servos output parameters:
+
+# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+#param set-default SIM_GZ_WH_MIN1 0
+#param set-default SIM_GZ_WH_MAX1 200
+#param set-default SIM_GZ_WH_DIS1 100
+#param set-default SIM_GZ_WH_FAIL1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+#param set-default SIM_GZ_WH_MIN2 0
+#param set-default SIM_GZ_WH_MAX2 200
+#aram set-default SIM_GZ_WH_DIS2 100
+#param set-default SIM_GZ_WH_FAIL2 100
+
+param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
+
+# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. 
+# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate 
+# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator 
+# controls in practical scenarios.
+
+# Cutter deck blades clutch, PCA9685 servo channel 3,  "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
+param set-default SIM_GZ_SV_FUNC3 203
+param set-default SIM_GZ_SV_MIN3 0
+param set-default SIM_GZ_SV_MAX3 1000
+param set-default SIM_GZ_SV_DIS3 500
+param set-default SIM_GZ_SV_FAIL3 500
+
+# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
+#    - on minimum when disarmed or failed:
+param set-default SIM_GZ_SV_FUNC4 204
+param set-default SIM_GZ_SV_MIN4 0
+param set-default SIM_GZ_SV_MAX4 1000
+param set-default SIM_GZ_SV_DIS4 500
+param set-default SIM_GZ_SV_FAIL4 500
+
+# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
+
+# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode:
+#param set-default SIM_GZ_SV_FUNC5 205
+#param set-default SIM_GZ_SV_MIN5 1000
+#param set-default SIM_GZ_SV_MAX5 2000
+#param set-default SIM_GZ_SV_DIS5 1000
+#param set-default SIM_GZ_SV_FAIL5 1000
+
+# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure:
+#param set-default SIM_GZ_SV_FUNC6 206
+
+# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207):
+#param set-default SIM_GZ_SV_FUNC7 207
+
+# Spare PCA9685 servo channel 8 - "Servo 8" (208):
+#param set-default SIM_GZ_SV_FUNC8 208

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

@@ -82,6 +82,7 @@ px4_add_romfs_files(
 	4008_gz_advanced_plane
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
+	4011_gz_lawnmower
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -253,7 +253,7 @@ fi
 tone_alarm start
 rc_update start
 manual_control start
-#sensors start
+sensors start
 commander start
 
 if ! pwm_out_sim start -m sim

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -34,28 +34,79 @@
 add_subdirectory(airframes)
 
 px4_add_romfs_files(
-	rc.airship_apps
-	rc.airship_defaults
+
 	rc.autostart_ext
-	rc.balloon_apps
-	rc.balloon_defaults
-	rc.boat_defaults
-	rc.fw_apps
-	rc.fw_defaults
-	rc.heli_defaults
-	rc.logging
-	rc.mc_apps
-	rc.mc_defaults
+
 	rcS
 	rc.sensors
-	rc.thermal_cal
-	rc.rover_apps
-	rc.rover_defaults
-	rc.rover_differential_apps
-	rc.rover_differential_defaults
-	rc.uuv_apps
-	rc.uuv_defaults
 	rc.vehicle_setup
-	rc.vtol_apps
-	rc.vtol_defaults
+
+	# TODO
+	rc.balloon_apps
+	rc.balloon_defaults
 )
+
+if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
+	px4_add_romfs_files(
+		rc.airship_apps
+		rc.airship_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_FW_RATE_CONTROL)
+	px4_add_romfs_files(
+		rc.fw_apps
+		rc.fw_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_MC_RATE_CONTROL)
+	px4_add_romfs_files(
+		rc.heli_defaults
+		rc.mc_apps
+		rc.mc_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_ROVER_POS_CONTROL)
+	px4_add_romfs_files(
+		rc.rover_apps
+		rc.rover_defaults
+
+		rc.boat_defaults # hack
+	)
+endif()
+
+if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
+	px4_add_romfs_files(
+		rc.rover_differential_apps
+		rc.rover_differential_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_UUV_ATT_CONTROL)
+	px4_add_romfs_files(
+		rc.uuv_apps
+		rc.uuv_defaults
+	)
+endif()
+
+if(CONFIG_MODULES_VTOL_ATT_CONTROL)
+	px4_add_romfs_files(
+		rc.vtol_apps
+		rc.vtol_defaults
+	)
+endif()
+
+
+if(CONFIG_MODULES_LOGGER)
+	px4_add_romfs_files(
+		rc.logging
+	)
+endif()
+
+if(CONFIG_MODULES_TEMPERATURE_COMPENSATION)
+	px4_add_romfs_files(
+		rc.thermal_cal
+	)
+endif()

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

@@ -32,94 +32,127 @@
 ############################################################################
 
 px4_add_romfs_files(
-	# [0-999] Reserved (historical)"
-
-	# [1000, 1999] Simulation setups"
-	1001_rc_quad_x.hil
-	1002_standard_vtol.hil
-	1100_rc_quad_x_sih.hil
-	1101_rc_plane_sih.hil
-	1102_tailsitter_duo_sih.hil
-
-	# [2000, 2999] Standard planes"
-	2100_standard_plane
-	2106_albatross
-
-	2507_cloudship
-
-	# [3000, 3999] Flying wing"
-	3000_generic_wing
-
-	# [4000, 4999] Quadrotor x"
-	4001_quad_x
-	4014_s500
-	4015_holybro_s500
-	4016_holybro_px4vision
-	4017_nxp_hovergames
-	4019_x500_v2
-	4020_holybro_px4vision_v1_5
-	4041_beta75x
-	4050_generic_250
-	4052_holybro_qav250
-	4053_holybro_kopis2
-	4061_atl_mantis_edu
-	4071_ifo
-	4073_ifo-s
-	4500_clover4
-	4901_crazyflie21
-
-	# [5000, 5999] Quadrotor +"
-	5001_quad_+
-
-	# [6000, 6999] Hexarotor x"
-	6001_hexa_x
-	6002_draco_r
-
-	# [7000, 7999] Hexarotor +"
-	7001_hexa_+
-
-	# [8000, 8999] Octorotor +"
-	8001_octo_x
-
-	# [9000, 9999] Octorotor +"
-	9001_octo_+
-
-	# [11000, 11999] Hexa Cox
-	11001_hexa_cox
-
-	# [12000, 12999] Octo Cox
-	12001_octo_cox
-
-	# [13000, 13999] VTOL
-	13000_generic_vtol_standard
-	13100_generic_vtol_tiltrotor
-	13013_deltaquad
-	13014_vtol_babyshark
-	13030_generic_vtol_quad_tiltrotor
-	13200_generic_vtol_tailsitter
-
-	# [14000, 14999] MC with tilt
-	14001_generic_mc_with_tilt
-
-	16001_helicopter
-
-	# [17000, 17999] Autogyro
-	17002_TF-AutoG2
-	17003_TF-G2
+	# [0-999] Reserved (historical)
 
 	# [18000, 18999] High-altitude balloons
 	18001_TF-B1
 
 	# [22000, 22999] Reserve for custom models
-
-	24001_dodeca_cox
-
-	50000_generic_ground_vehicle
-	50004_nxpcup_car_dfrobot_gpx
-	50003_aion_robotics_r1_rover
-
-	# [60000, 61000] (Unmanned) Underwater Robots
-	60000_uuv_generic
-	60001_uuv_hippocampus
-	60002_uuv_bluerov2_heavy
 )
+
+if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
+	px4_add_romfs_files(
+		# [1000, 1999] Simulation setups
+		1001_rc_quad_x.hil
+		1002_standard_vtol.hil
+		1100_rc_quad_x_sih.hil
+		1101_rc_plane_sih.hil
+		1102_tailsitter_duo_sih.hil
+	)
+endif()
+
+if(CONFIG_MODULES_MC_RATE_CONTROL)
+	px4_add_romfs_files(
+		# [4000, 4999] Quadrotor x
+		4001_quad_x
+		4014_s500
+		4015_holybro_s500
+		4016_holybro_px4vision
+		4017_nxp_hovergames
+		4019_x500_v2
+		4020_holybro_px4vision_v1_5
+		4041_beta75x
+		4050_generic_250
+		4052_holybro_qav250
+		4053_holybro_kopis2
+		4061_atl_mantis_edu
+		4071_ifo
+		4073_ifo-s
+		4500_clover4
+		4901_crazyflie21
+
+		# [5000, 5999] Quadrotor +
+		5001_quad_+
+
+		# [6000, 6999] Hexarotor x
+		6001_hexa_x
+		6002_draco_r
+
+		# [7000, 7999] Hexarotor +
+		7001_hexa_+
+
+		# [8000, 8999] Octorotor +
+		8001_octo_x
+
+		# [9000, 9999] Octorotor +
+		9001_octo_+
+
+		# [11000, 11999] Hexa Cox
+		11001_hexa_cox
+
+		# [12000, 12999] Octo Cox
+		12001_octo_cox
+
+		# [14000, 14999] MC with tilt
+		14001_generic_mc_with_tilt
+
+		16001_helicopter
+
+		24001_dodeca_cox
+	)
+endif()
+
+if(CONFIG_MODULES_FW_RATE_CONTROL)
+	px4_add_romfs_files(
+		# [2000, 2999] Standard planes
+		2100_standard_plane
+		2106_albatross
+
+		# [3000, 3999] Flying wing
+		3000_generic_wing
+
+		# [17000, 17999] Autogyro
+		17002_TF-AutoG2
+		17003_TF-G2
+	)
+endif()
+
+if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
+	px4_add_romfs_files(
+		2507_cloudship
+	)
+endif()
+
+if(CONFIG_MODULES_VTOL_ATT_CONTROL)
+	px4_add_romfs_files(
+		# [13000, 13999] VTOL
+		13000_generic_vtol_standard
+		13100_generic_vtol_tiltrotor
+		13013_deltaquad
+		13014_vtol_babyshark
+		13030_generic_vtol_quad_tiltrotor
+		13200_generic_vtol_tailsitter
+	)
+endif()
+
+if(CONFIG_MODULES_ROVER_POS_CONTROL)
+	px4_add_romfs_files(
+		50000_generic_ground_vehicle
+		50004_nxpcup_car_dfrobot_gpx
+	)
+endif()
+
+if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
+	px4_add_romfs_files(
+		50003_aion_robotics_r1_rover
+	)
+endif()
+
+if(CONFIG_MODULES_UUV_ATT_CONTROL)
+	px4_add_romfs_files(
+		# [60000, 61000] (Unmanned) Underwater Robots
+		60000_uuv_generic
+		60001_uuv_hippocampus
+		60002_uuv_bluerov2_heavy
+	)
+endif()

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -28,7 +28,6 @@ param set-default EKF2_ARSP_THR 8
 param set-default EKF2_FUSE_BETA 1
 param set-default EKF2_GPS_CHECK 21
 param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
 param set-default EKF2_REQ_EPH 10
 param set-default EKF2_REQ_EPV 10
 param set-default EKF2_REQ_HDRIFT 0.5

ROMFS/px4fmu_common/init.d/rcS

@@ -439,7 +439,12 @@ else
 	#
 	# Start a thermal calibration if required.
 	#
-	. ${R}etc/init.d/rc.thermal_cal
+	set RC_THERMAL_CAL ${R}etc/init.d/rc.thermal_cal
+	if [ -f ${RC_THERMAL_CAL} ]
+	then
+		. ${RC_THERMAL_CAL}
+	fi
+	unset RC_THERMAL_CAL
 
 	#
 	# Start gimbal to control mounts such as gimbals, disabled by default.
@@ -500,7 +505,12 @@ else
 	#
 	# Start the logger.
 	#
-	. ${R}etc/init.d/rc.logging
+	set RC_LOGGING ${R}etc/init.d/rc.logging
+	if [ -f ${RC_LOGGING} ]
+	then
+		. ${RC_LOGGING}
+	fi
+	unset RC_LOGGING
 
 	#
 	# Set additional parameters and env variables for selected AUTOSTART.

Tools/astyle/files_to_check_code_style.sh

@@ -30,4 +30,5 @@ exec find boards msg src platforms test \
     -path src/lib/cdrstream/cyclonedds -prune -o \
     -path src/lib/cdrstream/rosidl -prune -o \
     -path src/modules/zenoh/zenoh-pico -prune -o \
+    -path src/modules/muorb/apps/libfc-sensor-api -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

Tools/msg/templates/uorb/msg.cpp.em

@@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>
 
+@{
+queue_length = 1
+for constant in spec.constants:
+	if constant.name == 'ORB_QUEUE_LENGTH':
+		queue_length = constant.val
+}@
+
 @[for topic in topics]@
 static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
-ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic), @queue_length);
 @[end for]
 
 void print_message(const orb_metadata *meta, const @uorb_struct& message)

boards/atl/mantis-edu/src/board_config.h

@@ -64,9 +64,6 @@
 // Hacks for MAVLink RC button input
 #define ATL_MANTIS_RC_INPUT_HACKS
 
-// Hacks for MAVLink RC button input
-#define ATL_MANTIS_RC_INPUT_HACKS
-
 /*
  * ADC channels
  *

boards/modalai/voxl2-slpi/default.px4board

@@ -4,13 +4,13 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
-CONFIG_DRIVERS_VOXL2_IO=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_EKF2=y
@@ -28,4 +28,5 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
 CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_ORB_COMMUNICATOR=y

boards/modalai/voxl2-slpi/src/CMakeLists.txt

@@ -44,11 +44,10 @@ add_library(drivers_board
     )
 
 # Add custom drivers for SLPI
-# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
-# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
 # add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)

boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp

@@ -989,10 +989,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
 
 	gps.device_id = device_id.devid;
 
-	gps.lat = hil_gps.lat;
-	gps.lon = hil_gps.lon;
-	gps.alt = hil_gps.alt;
-	gps.alt_ellipsoid = hil_gps.alt;
+	gps.latitude_deg = hil_gps.lat;
+	gps.longitude_deg = hil_gps.lon;
+	gps.altitude_msl_m = hil_gps.alt;
+	gps.altitude_ellipsoid_m = hil_gps.alt;
 
 	gps.s_variance_m_s = 0.25f;
 	gps.c_variance_rad = 0.5f;

boards/modalai/voxl2-slpi/src/drivers/icm42688p/CMakeLists.txt

@@ -1,48 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-px4_add_module(
-	MODULE drivers__imu__invensense__icm42688p
-	MAIN icm42688p
-	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
-	SRCS
-		icm42688p_main.cpp
-		ICM42688P.cpp
-		ICM42688P.hpp
-		InvenSense_ICM42688P_registers.hpp
-	DEPENDS
-		px4_work_queue
-		drivers_accelerometer
-		drivers_gyroscope
-		drivers__device
-	)

boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp

@@ -1,991 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 "ICM42688P.hpp"
-
-bool hitl_mode = false;
-
-using namespace time_literals;
-
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
-}
-
-ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
-	// SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency),
-	// I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	// _drdy_gpio(drdy_gpio)
-	SPI(config),
-	I2CSPIDriver(config),
-	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
-{
-	if (config.drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
-	}
-
-	if (!hitl_mode) {
-		// _px4_accel = std::make_shared<PX4Accelerometer>(get_device_id(), rotation);
-		// _px4_gyro = std::make_shared<PX4Gyroscope>(get_device_id(), rotation);
-		ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
-		// _imu_server_pub.advertise();
-
-	} else {
-		ConfigureSampleRate(0);
-	}
-}
-
-ICM42688P::~ICM42688P()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
-
-	// if (!hitl_mode){
-	//  		_imu_server_pub.unadvertise();
-	// }
-}
-
-int ICM42688P::init()
-{
-	int ret = SPI::init();
-
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
-
-	return Reset() ? 0 : -1;
-}
-
-bool ICM42688P::Reset()
-{
-	_state = STATE::RESET;
-	DataReadyInterruptDisable();
-	ScheduleClear();
-	ScheduleNow();
-	return true;
-}
-
-void ICM42688P::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
-}
-
-void ICM42688P::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_missed_perf);
-}
-
-int ICM42688P::probe()
-{
-	for (int i = 0; i < 3; i++) {
-		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
-
-		if (whoami == WHOAMI) {
-			PX4_INFO("ICM42688P::probe successful!");
-			return PX4_OK;
-
-		} else {
-			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
-
-			uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
-			int bank = reg_bank_sel >> 4;
-
-			if (bank >= 1 && bank <= 3) {
-				DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
-				// force bank selection and retry
-				SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
-			}
-		}
-	}
-
-	return PX4_ERROR;
-}
-
-void ICM42688P::RunImpl()
-{
-	PX4_INFO(">>> ICM42688P this: %p", this);
-
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// DEVICE_CONFIG: Software reset configuration
-		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(2_ms); // to be safe wait 2 ms for soft reset to be effective
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
-		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
-		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
-
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(10_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-
-		if (Configure()) {
-
-			// Wakeup accel and gyro after configuring registers
-			ScheduleDelayed(1_ms); // add a delay here to be safe
-			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
-			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
-
-			// if configure succeeded then start reading from FIFO
-			_state = STATE::FIFO_READ;
-
-			if (DataReadyInterruptConfigure()) {
-				_data_ready_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_ERR("ICM42688P::RunImpl interrupt configuration failed");
-
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
-
-		} else {
-
-			PX4_ERR("ICM42688P::RunImpl configuration failed");
-
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_READ: {
-#ifndef __PX4_QURT
-			uint32_t samples = 0;
-
-			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
-
-				} else {
-					samples = _fifo_gyro_samples;
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			if (samples == 0) {
-				// check current FIFO count
-				const uint16_t fifo_count = FIFOReadCount();
-
-				if (fifo_count >= FIFO::SIZE) {
-					FIFOReset();
-					perf_count(_fifo_overflow_perf);
-
-				} else if (fifo_count == 0) {
-					perf_count(_fifo_empty_perf);
-
-				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
-
-					if (samples > FIFO_MAX_SAMPLES) {
-						// not technically an overflow, but more samples than we expected or can publish
-						FIFOReset();
-						perf_count(_fifo_overflow_perf);
-						samples = 0;
-					}
-				}
-			}
-
-			bool success = false;
-
-			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
-					success = true;
-
-					if (_failure_count > 0) {
-						_failure_count--;
-					}
-				}
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			// check configuration registers periodically or immediately following any failure
-			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
-			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
-			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
-			   ) {
-				_last_config_check_timestamp = now;
-				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
-				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
-
-			} else {
-				// register check failed, force reset
-				perf_count(_bad_register_perf);
-				Reset();
-			}
-
-#endif
-		}
-
-		break;
-	}
-}
-
-void ICM42688P::ConfigureSampleRate(int sample_rate)
-{
-	if (sample_rate == 0) {
-		sample_rate = 800; // default to 800 Hz
-	}
-
-	// round down to nearest FIFO sample dt
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
-
-	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
-
-	// recompute FIFO empty interval (us) with actual gyro sample limit
-	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
-
-	ConfigureFIFOWatermark(_fifo_gyro_samples);
-}
-
-void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO watermark threshold in number of bytes
-	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
-
-	for (auto &r : _register_bank0_cfg) {
-		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
-			// FIFO_WM[7:0]  FIFO_CONFIG2
-			r.set_bits = fifo_watermark_threshold & 0xFF;
-
-		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
-			// FIFO_WM[11:8] FIFO_CONFIG3
-			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
-		}
-	}
-}
-
-void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
-{
-	if (bank != _last_register_bank || force) {
-		// select BANK_0
-		uint8_t cmd_bank_sel[2] {};
-		cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
-		cmd_bank_sel[1] = bank;
-		transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
-
-		_last_register_bank = bank;
-	}
-}
-
-bool ICM42688P::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	for (const auto &reg_cfg : _register_bank1_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	for (const auto &reg_cfg : _register_bank2_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	// now check that all are configured
-	bool success = true;
-
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	for (const auto &reg_cfg : _register_bank1_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	for (const auto &reg_cfg : _register_bank2_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	// // 20-bits data format used
-	// //  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
-	if (!hitl_mode) {
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_gyro.set_range(math::radians(2000.f));
-		_px4_gyro.set_scale(math::radians(1.f / 131.f));
-	}
-
-	return success;
-}
-
-static bool interrupt_debug = false;
-static uint32_t interrupt_debug_count = 0;
-static const uint32_t interrupt_debug_trigger = 800;
-static hrt_abstime last_interrupt_time = 0;
-static hrt_abstime avg_interrupt_delta = 0;
-static hrt_abstime max_interrupt_delta = 0;
-static hrt_abstime min_interrupt_delta = 60 * 1000 * 1000;
-static hrt_abstime cumulative_interrupt_delta = 0;
-
-int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	hrt_abstime current_interrupt_time = hrt_absolute_time();
-
-	if (interrupt_debug) {
-		if (last_interrupt_time) {
-			hrt_abstime interrupt_delta_time = current_interrupt_time - last_interrupt_time;
-
-			if (interrupt_delta_time > max_interrupt_delta) { max_interrupt_delta = interrupt_delta_time; }
-
-			if (interrupt_delta_time < min_interrupt_delta) { min_interrupt_delta = interrupt_delta_time; }
-
-			cumulative_interrupt_delta += interrupt_delta_time;
-		}
-
-		last_interrupt_time = current_interrupt_time;
-
-		interrupt_debug_count++;
-
-		if (interrupt_debug_count == interrupt_debug_trigger) {
-			avg_interrupt_delta = cumulative_interrupt_delta / interrupt_debug_trigger;
-			PX4_INFO(">>> Max: %llu, Min: %llu, Avg: %llu", max_interrupt_delta,
-				 min_interrupt_delta, avg_interrupt_delta);
-			interrupt_debug_count = 0;
-			cumulative_interrupt_delta = 0;
-		}
-	}
-
-	static_cast<ICM42688P *>(arg)->DataReady();
-
-	return 0;
-}
-
-void ICM42688P::DataReady()
-{
-#ifndef __PX4_QURT
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
-
-#else
-	uint16_t fifo_byte_count = FIFOReadCount();
-
-	FIFORead(hrt_absolute_time(), fifo_byte_count / sizeof(FIFO::DATA));
-#endif
-}
-
-bool ICM42688P::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
-
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
-}
-
-bool ICM42688P::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
-
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
-}
-
-template <typename T>
-bool ICM42688P::RegisterCheck(const T &reg_cfg)
-{
-	bool success = true;
-
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
-
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
-
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
-
-	return success;
-}
-
-template <typename T>
-uint8_t ICM42688P::RegisterRead(T reg)
-{
-	uint8_t cmd[2] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	SelectRegisterBank(reg);
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[1];
-}
-
-template <typename T>
-void ICM42688P::RegisterWrite(T reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	SelectRegisterBank(reg);
-	transfer(cmd, cmd, sizeof(cmd));
-}
-
-template <typename T>
-void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
-
-	uint8_t val = (orig_val & ~clearbits) | setbits;
-
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
-}
-
-uint16_t ICM42688P::FIFOReadCount()
-{
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
-
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return 0;
-	}
-
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
-}
-
-// static uint32_t debug_decimator = 0;
-// static hrt_abstime last_sample_time = 0;
-// static bool imu_debug = true;
-
-bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t max_transfer_size = 10 * sizeof(FIFO::DATA) + 4;
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, max_transfer_size);
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
-
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
-
-	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
-	}
-
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
-
-	if (fifo_count_bytes >= FIFO::SIZE) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
-	}
-
-	const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
-		perf_count(_fifo_empty_perf);
-		return false;
-	}
-
-	// check FIFO header in every sample
-	uint16_t valid_samples = 0;
-
-	// for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
-	for (int i = 0; i < math::min(samples, (uint16_t) 10); i++) {
-		bool valid = true;
-
-		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
-
-		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
-			// FIFO sample empty if HEADER_MSG set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
-			// accel bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
-			// gyro bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
-			// Packet does not contain a new and valid extended 20-bit data
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
-			// accel ODR changed
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
-			// gyro ODR changed
-			valid = false;
-		}
-
-		if (valid) {
-			valid_samples++;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-			break;
-		}
-	}
-
-	// if (imu_debug) {
-	// 	debug_decimator++;
-	// 	if (debug_decimator == 801) {
-	// 		debug_decimator = 0;
-	// 		PX4_INFO("Initial: %u Next: %u Valid: %u Delta: %llu", samples, fifo_count_samples, valid_samples, timestamp_sample - last_sample_time);
-	// 	}
-	// 	last_sample_time = timestamp_sample;
-	// }
-
-	if (valid_samples > 0) {
-		if (ProcessTemperature(buffer.f, valid_samples)) {
-			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-			ProcessIMU(timestamp_sample, buffer.f, valid_samples);
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void ICM42688P::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
-
-	// SIGNAL_PATH_RESET: FIFO flush
-	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
-
-	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
-}
-
-static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
-{
-	// 0xXXXAABBC
-	uint32_t high   = ((a << 12) & 0x000FF000);
-	uint32_t low    = ((b << 4)  & 0x00000FF0);
-	uint32_t lowest = (c         & 0x0000000F);
-
-	uint32_t x = high | low | lowest;
-
-	if (a & Bit7) {
-		// sign extend
-		x |= 0xFFF00000u;
-	}
-
-	return static_cast<int32_t>(x);
-}
-
-void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
-	float gyro_x = 0.0,  gyro_y = 0.0,  gyro_z = 0.0;
-
-	for (int i = 0; i < samples; i++) {
-		_imu_server_decimator++;
-
-		if (_imu_server_decimator == 8) {
-			_imu_server_decimator = 0;
-			// 20 bit hires mode
-
-			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-			// Accel data is 18 bit
-			int32_t temp_accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
-								fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
-			int32_t temp_accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
-								fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
-			int32_t temp_accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
-								fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
-
-			// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-			int32_t temp_gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0,
-							       fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
-			int32_t temp_gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0,
-							       fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
-			int32_t temp_gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0,
-							       fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
-
-			// accel samples invalid if -524288
-			if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
-				// shift accel by 2 (2 least significant bits are always 0)
-				accel_x = (float) temp_accel_x / 4.f;
-				accel_y = (float) temp_accel_y / 4.f;
-				accel_z = (float) temp_accel_z / 4.f;
-
-				// shift gyro by 1 (least significant bit is always 0)
-				gyro_x = (float) temp_gyro_x / 2.f;
-				gyro_y = (float) temp_gyro_y / 2.f;
-				gyro_z = (float) temp_gyro_z / 2.f;
-
-				// correct frame for publication
-				// sensor's frame is +x forward, +y left, +z up
-				// flip y & z to publish right handed with z down (x forward, y right, z down)
-				accel_y = -accel_y;
-				accel_z = -accel_z;
-				gyro_y  = -gyro_y;
-				gyro_z  = -gyro_z;
-
-				// Scale everything appropriately
-				float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
-				accel_x *= accel_scale_factor;
-				accel_y *= accel_scale_factor;
-				accel_z *= accel_scale_factor;
-
-				float gyro_scale_factor = math::radians(1.f / 131.f);
-				gyro_x *= gyro_scale_factor;
-				gyro_y *= gyro_scale_factor;
-				gyro_z *= gyro_scale_factor;
-
-				// Store the data in our array
-				_imu_server_data.accel_x[_imu_server_index] = accel_x;
-				_imu_server_data.accel_y[_imu_server_index] = accel_y;
-				_imu_server_data.accel_z[_imu_server_index] = accel_z;
-				_imu_server_data.gyro_x[_imu_server_index]  = gyro_x;
-				_imu_server_data.gyro_y[_imu_server_index]  = gyro_y;
-				_imu_server_data.gyro_z[_imu_server_index]  = gyro_z;
-				_imu_server_data.ts[_imu_server_index]      = timestamp_sample - (125 * (samples - 1 - i));
-				_imu_server_index++;
-
-				// If array is full, publish the data
-				if (_imu_server_index == 10) {
-					_imu_server_index = 0;
-					_imu_server_data.timestamp = hrt_absolute_time();
-					_imu_server_data.temperature = 0; // Not used right now
-					_imu_server_pub.publish(_imu_server_data);
-				}
-			}
-		}
-	}
-}
-
-void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
-
-	// 18-bits of accelerometer data
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-		// 20 bit hires mode
-		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-		// Accel data is 18 bit ()
-		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
-						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
-		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
-						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
-		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
-						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
-
-		// sample invalid if -524288
-		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
-			// check if any values are going to exceed int16 limits
-			static constexpr int16_t max_accel = INT16_MAX;
-			static constexpr int16_t min_accel = INT16_MIN;
-
-			if (accel_x >= max_accel || accel_x <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_y >= max_accel || accel_y <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_z >= max_accel || accel_z <= min_accel) {
-				scale_20bit = true;
-			}
-
-			// shift by 2 (2 least significant bits are always 0)
-			accel.x[accel.samples] = accel_x / 4;
-			accel.y[accel.samples] = accel_y / 4;
-			accel.z[accel.samples] = accel_z / 4;
-			accel.samples++;
-		}
-	}
-
-	if (!scale_20bit) {
-		// if highres enabled accel data is always 8192 LSB/g
-		if (!hitl_mode) {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		}
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			// 20 bit hires mode
-			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-			// Accel data is 18 bit ()
-			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
-			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
-			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
-
-			accel.x[i] = accel_x;
-			accel.y[i] = accel_y;
-			accel.z[i] = accel_z;
-		}
-
-		if (!hitl_mode) {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		}
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < accel.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[i] = accel.x[i];
-		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
-		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
-	}
-
-	if (!hitl_mode) {
-		_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-	}
-
-	if (accel.samples > 0) {
-		if (!hitl_mode) {
-			_px4_accel.updateFIFO(accel);
-		}
-	}
-}
-
-void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = 0;
-	gyro.dt = FIFO_SAMPLE_DT;
-
-	// 20-bits of gyroscope data
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-		// 20 bit hires mode
-		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
-		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
-		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
-
-		// check if any values are going to exceed int16 limits
-		static constexpr int16_t max_gyro = INT16_MAX;
-		static constexpr int16_t min_gyro = INT16_MIN;
-
-		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		gyro.x[gyro.samples] = gyro_x / 2;
-		gyro.y[gyro.samples] = gyro_y / 2;
-		gyro.z[gyro.samples] = gyro_z / 2;
-		gyro.samples++;
-	}
-
-	if (!scale_20bit) {
-		// if highres enabled gyro data is always 131 LSB/dps
-		if (!hitl_mode) {
-			_px4_gyro.set_scale(math::radians(1.f / 131.f));
-		}
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
-			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
-			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
-		}
-
-		if (!hitl_mode) {
-			_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
-		}
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < gyro.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro.x[i];
-		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
-		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
-	}
-
-	if (!hitl_mode) {
-		_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-					  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-	}
-
-	if (gyro.samples > 0) {
-		if (!hitl_mode) {
-			_px4_gyro.updateFIFO(gyro);
-		}
-	}
-}
-
-bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
-{
-	int16_t temperature[FIFO_MAX_SAMPLES];
-	float temperature_sum{0};
-
-	int valid_samples = 0;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
-
-		// sample invalid if -32768
-		if (t != -32768) {
-			temperature_sum += t;
-			temperature[valid_samples] = t;
-			valid_samples++;
-		}
-	}
-
-	if (valid_samples > 0) {
-		const float temperature_avg = temperature_sum / valid_samples;
-
-		for (int i = 0; i < valid_samples; i++) {
-			// temperature changing wildly is an indication of a transfer error
-			if (fabsf(temperature[i] - temperature_avg) > 1000) {
-				perf_count(_bad_transfer_perf);
-				return false;
-			}
-		}
-
-		// use average temperature reading
-		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-		if (PX4_ISFINITE(TEMP_degC)) {
-			if (!hitl_mode) {
-				_px4_accel.set_temperature(TEMP_degC);
-				_px4_gyro.set_temperature(TEMP_degC);
-				return true;
-			}
-
-		} else {
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	return false;
-}

boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp

@@ -1,235 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ICM42688P.hpp
- *
- * Driver for the Invensense ICM42688P connected via SPI.
- *
- */
-
-#pragma once
-
-#include "InvenSense_ICM42688P_registers.hpp"
-
-#include <drivers/drv_hrt.h>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/device/spi.h>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-#include <lib/geo/geo.h>
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/atomic.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-#include <uORB/topics/imu_server.h>
-#include <uORB/topics/sensor_accel_fifo.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
-#include <memory>
-
-using namespace InvenSense_ICM42688P;
-
-extern bool hitl_mode;
-
-class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
-{
-public:
-	// ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
-	// 	  spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
-	ICM42688P(const I2CSPIDriverConfig &config);
-	~ICM42688P() override;
-
-	// static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-	// 				     int runtime_instance);
-	static void print_usage();
-
-	void RunImpl();
-
-	int init() override;
-	void print_status() override;
-
-private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured
-	static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR};
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
-
-	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	// static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
-	static constexpr uint32_t FIFO_MAX_SAMPLES{10};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
-		uint8_t INT_STATUS{0};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)),
-		      "Invalid FIFOTransferBuffer size");
-
-	struct register_bank0_config_t {
-		Register::BANK_0 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	struct register_bank1_config_t {
-		Register::BANK_1 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	struct register_bank2_config_t {
-		Register::BANK_2 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Reset();
-
-	bool Configure();
-	void ConfigureSampleRate(int sample_rate);
-	void ConfigureFIFOWatermark(uint8_t samples);
-
-	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
-	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
-	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
-	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
-
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
-
-	template <typename T> bool RegisterCheck(const T &reg_cfg);
-	template <typename T> uint8_t RegisterRead(T reg);
-	template <typename T> void RegisterWrite(T reg, uint8_t value);
-	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
-	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
-	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
-
-	uint16_t FIFOReadCount();
-	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
-	void FIFOReset();
-
-	void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
-
-	const spi_drdy_gpio_t _drdy_gpio;
-
-	// std::shared_ptr<PX4Accelerometer> _px4_accel;
-	// std::shared_ptr<PX4Gyroscope> _px4_gyro;
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
-	perf_counter_t _drdy_missed_perf{nullptr};
-
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	hrt_abstime _temperature_update_timestamp{0};
-	int _failure_count{0};
-
-	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
-
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	bool _data_ready_interrupt_enabled{false};
-
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		FIFO_READ,
-	} _state{STATE::RESET};
-
-	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
-
-	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{12};
-	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
-		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
-		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
-		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
-		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
-		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
-		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
-		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
-		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
-		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
-	};
-
-	uint8_t _checked_register_bank1{0};
-	static constexpr uint8_t size_register_bank1_cfg{4};
-	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
-		{ Register::BANK_1::GYRO_CONFIG_STATIC3,  GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC4,  GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC5,  GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_CLEAR},
-	};
-
-	uint8_t _checked_register_bank2{0};
-	static constexpr uint8_t size_register_bank2_cfg{3};
-	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_CLEAR },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_CLEAR },
-	};
-
-	uint32_t _temperature_samples{0};
-
-	// Support for the IMU server
-	uint32_t _imu_server_index{0};
-	uint32_t _imu_server_decimator{0};
-	imu_server_s _imu_server_data;
-	uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};
-
-};

boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp

@@ -1,430 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 InvenSense_ICM42688P_registers.hpp
- *
- * Invensense ICM-42688-P registers.
- *
- */
-
-#pragma once
-
-#include <cstdint>
-
-namespace InvenSense_ICM42688P
-{
-// TODO: move to a central header
-static constexpr uint8_t Bit0 = (1 << 0);
-static constexpr uint8_t Bit1 = (1 << 1);
-static constexpr uint8_t Bit2 = (1 << 2);
-static constexpr uint8_t Bit3 = (1 << 3);
-static constexpr uint8_t Bit4 = (1 << 4);
-static constexpr uint8_t Bit5 = (1 << 5);
-static constexpr uint8_t Bit6 = (1 << 6);
-static constexpr uint8_t Bit7 = (1 << 7);
-
-static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
-static constexpr uint8_t DIR_READ = 0x80;
-
-static constexpr uint8_t WHOAMI = 0x47;
-
-static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
-static constexpr float TEMPERATURE_OFFSET = 25.f; // C
-
-namespace Register
-{
-
-enum class BANK_0 : uint8_t {
-	DEVICE_CONFIG      = 0x11,
-
-	INT_CONFIG         = 0x14,
-
-	FIFO_CONFIG        = 0x16,
-
-	TEMP_DATA1         = 0x1D,
-	TEMP_DATA0         = 0x1E,
-
-	INT_STATUS         = 0x2D,
-	FIFO_COUNTH        = 0x2E,
-	FIFO_COUNTL        = 0x2F,
-	FIFO_DATA          = 0x30,
-
-	SIGNAL_PATH_RESET  = 0x4B,
-	INTF_CONFIG0       = 0x4C,
-	INTF_CONFIG1       = 0x4D,
-	PWR_MGMT0          = 0x4E,
-	GYRO_CONFIG0       = 0x4F,
-	ACCEL_CONFIG0      = 0x50,
-	GYRO_CONFIG1       = 0x51,
-	GYRO_ACCEL_CONFIG0 = 0x52,
-	ACCEL_CONFIG1      = 0x53,
-
-	FIFO_CONFIG1       = 0x5F,
-	FIFO_CONFIG2       = 0x60,
-	FIFO_CONFIG3       = 0x61,
-
-	INT_CONFIG0        = 0x63,
-
-	INT_SOURCE0        = 0x65,
-
-	SELF_TEST_CONFIG   = 0x70,
-
-	WHO_AM_I           = 0x75,
-	REG_BANK_SEL       = 0x76,
-};
-
-enum class BANK_1 : uint8_t {
-	GYRO_CONFIG_STATIC2 = 0x0B,
-	GYRO_CONFIG_STATIC3 = 0x0C,
-	GYRO_CONFIG_STATIC4 = 0x0D,
-	GYRO_CONFIG_STATIC5 = 0x0E,
-	INTF_CONFIG5        = 0x7B,
-};
-enum class BANK_2 : uint8_t {
-	ACCEL_CONFIG_STATIC2 = 0x03,
-	ACCEL_CONFIG_STATIC3 = 0x04,
-	ACCEL_CONFIG_STATIC4 = 0x05,
-};
-
-};
-
-//---------------- BANK0 Register bits
-
-// DEVICE_CONFIG
-enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0, //
-};
-
-// INT_CONFIG
-enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE           = Bit2,
-	INT1_DRIVE_CIRCUIT  = Bit1,
-	INT1_POLARITY       = Bit0,
-};
-
-// FIFO_CONFIG
-enum FIFO_CONFIG_BIT : uint8_t {
-	// 7:6 FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
-};
-
-// INT_STATUS
-enum INT_STATUS_BIT : uint8_t {
-	RESET_DONE_INT = Bit4,
-	DATA_RDY_INT   = Bit3,
-	FIFO_THS_INT   = Bit2,
-	FIFO_FULL_INT  = Bit1,
-};
-
-// SIGNAL_PATH_RESET
-enum SIGNAL_PATH_RESET_BIT : uint8_t {
-	ABORT_AND_RESET = Bit3,
-	FIFO_FLUSH      = Bit1,
-};
-
-// PWR_MGMT0
-enum PWR_MGMT0_BIT : uint8_t {
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
-};
-
-// GYRO_CONFIG0
-enum GYRO_CONFIG0_BIT : uint8_t {
-	// 7:5 GYRO_FS_SEL
-	GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
-	GYRO_FS_SEL_1000_DPS = Bit5,
-	GYRO_FS_SEL_500_DPS  = Bit6,
-	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
-	GYRO_FS_SEL_125_DPS  = Bit7,
-
-
-	// 3:0 GYRO_ODR
-	//  0001: 32kHz
-	GYRO_ODR_32KHZ_SET   = Bit0,
-	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0010: 16kHz
-	GYRO_ODR_16KHZ_SET   = Bit1,
-	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0011: 8kHz
-	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
-	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
-	//  0110: 1kHz (default)
-	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
-	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
-};
-
-// ACCEL_CONFIG0
-enum ACCEL_CONFIG0_BIT : uint8_t {
-	// 7:5 ACCEL_FS_SEL
-	ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
-	ACCEL_FS_SEL_8G  = Bit5,
-	ACCEL_FS_SEL_4G  = Bit6,
-	ACCEL_FS_SEL_2G  = Bit6 | Bit5,
-
-
-	// 3:0 ACCEL_ODR
-	//  0001: 32kHz
-	ACCEL_ODR_32KHZ_SET   = Bit0,
-	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0010: 16kHz
-	ACCEL_ODR_16KHZ_SET   = Bit1,
-	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0011: 8kHz
-	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
-	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
-	//  0110: 1kHz (default)
-	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
-	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
-};
-
-// GYRO_CONFIG1
-enum GYRO_CONFIG1_BIT : uint8_t {
-	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
-};
-
-// GYRO_ACCEL_CONFIG0
-enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
-	// 7:4 ACCEL_UI_FILT_BW
-	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
-
-	// 3:0 GYRO_UI_FILT_BW
-	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
-};
-
-// ACCEL_CONFIG1
-enum ACCEL_CONFIG1_BIT : uint8_t {
-	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
-};
-
-// FIFO_CONFIG1
-enum FIFO_CONFIG1_BIT : uint8_t {
-	FIFO_RESUME_PARTIAL_RD = Bit6,
-	FIFO_WM_GT_TH          = Bit5,
-	FIFO_HIRES_EN          = Bit4,
-	FIFO_TEMP_EN           = Bit2,
-	FIFO_GYRO_EN           = Bit1,
-	FIFO_ACCEL_EN          = Bit0,
-};
-
-// INT_CONFIG0
-enum INT_CONFIG0_BIT : uint8_t {
-	// 3:2 FIFO_THS_INT_CLEAR
-	CLEAR_ON_FIFO_READ = Bit3,
-};
-
-// INT_SOURCE0
-enum INT_SOURCE0_BIT : uint8_t {
-	UI_FSYNC_INT1_EN   = Bit6,
-	PLL_RDY_INT1_EN    = Bit5,
-	RESET_DONE_INT1_EN = Bit4,
-	UI_DRDY_INT1_EN    = Bit3,
-	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
-	FIFO_FULL_INT1_EN  = Bit1,
-	UI_AGC_RDY_INT1_EN = Bit0,
-};
-
-// REG_BANK_SEL
-enum REG_BANK_SEL_BIT : uint8_t {
-	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
-	USER_BANK_1 = Bit0,        // 1: Select USER BANK 1.
-	USER_BANK_2 = Bit1,        // 2: Select USER BANK 2.
-	USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3.
-};
-
-
-//---------------- BANK1 Register bits
-
-// GYRO_CONFIG_STATIC2
-enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
-	GYRO_AAF_DIS = Bit1,
-	GYRO_NF_DIS  = Bit0,
-};
-
-// GYRO_CONFIG_STATIC3
-enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
-
-	// 585 Hz
-	GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13
-	GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1,
-
-	// 213 Hz
-	// GYRO_AAF_DELT_SET = Bit2 | Bit0, //5
-	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1,
-
-	// 126 Hz
-	//GYRO_AAF_DELT_SET = Bit1 | Bit0, //3
-	//GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2,
-
-	// 42 Hz
-	// GYRO_AAF_DELT_SET = Bit0, //1
-	// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
-
-};
-
-// GYRO_CONFIG_STATIC4
-enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
-
-	// 585 Hz
-	GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170
-	GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
-
-	// 213 Hz
-	// GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
-	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
-
-	// 126 Hz
-	//GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9
-	//GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1,
-
-	// 42 Hz
-	// GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1
-	// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
-};
-
-// GYRO_CONFIG_STATIC5
-enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
-
-	// 585 Hz
-	GYRO_AAF_DELTSQR_HIGH_SET = 0,
-	GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
-	GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4
-	GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4,
-
-	// 213 Hz
-	// GYRO_AAF_DELTSQR_HIGH_SET = 0,
-	// GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
-	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
-	// GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
-
-	// 126 Hz
-	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12
-	// GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4,
-
-	// 42 Hz
-	// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
-	// GYRO_AAF_BITSHIFT_CLEAR = 0,
-
-
-};
-
-
-//---------------- BANK2 Register bits
-
-// ACCEL_CONFIG_STATIC2
-enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
-	ACCEL_AAF_DIS  = Bit0,
-	ACCEL_AAF_DELT = Bit3 | Bit1,
-
-	// 213 Hz
-	ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5
-	ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2,
-
-	// 42 Hz
-	// ACCEL_AAF_DELT_SET = Bit1, //1
-	// ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2,
-};
-
-// ACCEL_CONFIG_STATIC3
-enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
-	ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0,
-	// 213 Hz
-	ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
-	ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
-
-	// 42 Hz
-	// ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1
-	// ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
-
-};
-
-// ACCEL_CONFIG_STATIC4
-enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
-	ACCEL_AAF_BITSHIFT     = Bit7 | Bit5,
-	ACCEL_AAF_DELTSQR_HIGH = 0,
-	// 213 Hz
-	ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
-	ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
-
-	// 42 Hz
-	// ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
-	// ACCEL_AAF_BITSHIFT_CLEAR = 0,
-
-	ACCEL_AAF_DELTSQR_HIGH_SET = 0,
-	ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
-};
-
-
-namespace FIFO
-{
-static constexpr size_t SIZE = 2048;
-
-// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
-
-// Packet 4
-struct DATA {
-	uint8_t FIFO_Header;
-	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
-	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
-	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
-	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
-	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
-	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
-	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
-	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
-	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
-	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
-	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
-	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
-	uint8_t TEMP_DATA1;    // Temperature[15:8]
-	uint8_t TEMP_DATA0;    // Temperature[7:0]
-	uint8_t TimeStamp_h;   // TimeStamp[15:8]
-	uint8_t TimeStamp_l;   // TimeStamp[7:0]
-	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
-	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
-	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
-};
-
-// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-enum FIFO_HEADER_BIT : uint8_t {
-	HEADER_MSG             = Bit7, // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
-	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
-	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
-	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
-	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
-	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
-};
-
-}
-} // namespace InvenSense_ICM42688P

boards/modalai/voxl2-slpi/src/drivers/icm42688p/icm42688p_main.cpp

@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 "ICM42688P.hpp"
-
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-#include <string>
-
-void ICM42688P::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-// 		int runtime_instance)
-// {
-// 	ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
-// 					    cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
-//
-// 	if (!instance) {
-// 		PX4_ERR("alloc failed");
-// 		return nullptr;
-// 	}
-//
-// 	if (OK != instance->init()) {
-// 		delete instance;
-// 		return nullptr;
-// 	}
-//
-// 	return instance;
-// }
-
-extern "C" int icm42688p_main(int argc, char *argv[])
-{
-
-	for (int i = 0; i <= argc - 1; i++) {
-		if (std::string(argv[i]) == "-h") {
-			argv[i] = 0;
-			hitl_mode = true;
-			break;
-		}
-	}
-
-	int ch;
-	using ThisDriver = ICM42688P;
-	BusCLIArguments cli{false, true};
-	cli.default_spi_frequency = SPI_SPEED;
-
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
-
-	const char *verb = cli.optArg();
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}

boards/px4/fmu-v5x/init/rc.board_defaults

@@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2

boards/px4/fmu-v5x/init/rc.board_mavlink

@@ -3,7 +3,7 @@
 # board specific MAVLink startup script.
 #------------------------------------------------------------------------------
 
-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	# Start MAVLink on the UART connected to the mission computer
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

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

@@ -49,7 +49,7 @@ then
 	fi
 fi
 
-if ver hwbasecmp 008 009 00a 010
+if ver hwbasecmp 008 009 00a 010 011
 then
 	#SKYNODE base fmu board orientation
 

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

@@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 0
 
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
 	param set-default UXRCE_DDS_PTCFG 2

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

@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------
 
 # if skynode base board is detected start Mavlink on Telem2
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
 

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

@@ -56,7 +56,7 @@ then
 fi
 
 #Start Auterion Power Module selector for Skynode boards
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	pm_selector_auterion start
 else
@@ -93,7 +93,7 @@ else
 		icm20649 -s -R 6 start
 	else
 		# Internal SPI BMI088
-		if ver hwbasecmp 009 010
+		if ver hwbasecmp 009 010 011
 		then
 			bmi088 -A -R 6 -s start
 			bmi088 -G -R 6 -s start
@@ -110,7 +110,7 @@ else
 	fi
 
 	# Internal SPI bus ICM42688p
-	if ver hwbasecmp 009 010
+	if ver hwbasecmp 009 010 011
 	then
 		icm42688p -R 12 -s start
 	else
@@ -127,7 +127,7 @@ else
 		# Internal SPI bus ICM-42670-P (hard-mounted)
 		icm42670p -R 10 -s start
 	else
-		if ver hwbasecmp 009 010
+		if ver hwbasecmp 009 010 011
 		then
 			icm20602 -R 6 -s start
 		else

boards/px4/fmu-v6xrt/default.px4board

@@ -6,21 +6,30 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
+CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
+CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
-CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
-CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
-CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
-CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
@@ -32,14 +41,20 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_COMMON_UWB=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -57,11 +72,15 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_UXRCE_DDS_CLIENT=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y

boards/px4/fmu-v6xrt/init/rc.board_mavlink

@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------
 
 # if skynode base board is detected start Mavlink on Telem2
-if ver hwbasecmp 009 010
+if ver hwbasecmp 009 010 011
 then
 	mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
 

boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig

@@ -257,7 +257,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_LPWORKSTACKSIZE=2032
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32

boards/px4/fmu-v6xrt/rover.px4board

@@ -1,4 +1,9 @@
+CONFIG_MODULES_AIRSPEED_SELECTOR=n
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -6,6 +11,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
+CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

boards/px4/sitl/simplesim.px4board

@@ -1,5 +0,0 @@
-CONFIG_EKF2_AUX_GLOBAL_POSITION=n
-CONFIG_EKF2_VERBOSE_STATUS=n
-CONFIG_MODULES_EKF2=n
-CONFIG_MODULES_SENSORS=y
-CONFIG_BOARD_NOLOCKSTEP=y

msg/CMakeLists.txt

@@ -173,6 +173,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	Rpm.msg
+	RtlStatus.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
 	SensorAccel.msg

msg/EstimatorStatusFlags.msg

@@ -55,15 +55,9 @@ bool fs_bad_airspeed          #  5 - true if fusion of the airspeed has encounte
 bool fs_bad_sideslip          #  6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 bool fs_bad_optflow_x         #  7 - true if fusion of the optical flow X axis has encountered a numerical error
 bool fs_bad_optflow_y         #  8 - true if fusion of the optical flow Y axis has encountered a numerical error
-bool fs_bad_vel_n             #  9 - true if fusion of the North velocity has encountered a numerical error
-bool fs_bad_vel_e             # 10 - true if fusion of the East velocity has encountered a numerical error
-bool fs_bad_vel_d             # 11 - true if fusion of the Down velocity has encountered a numerical error
-bool fs_bad_pos_n             # 12 - true if fusion of the North position has encountered a numerical error
-bool fs_bad_pos_e             # 13 - true if fusion of the East position has encountered a numerical error
-bool fs_bad_pos_d             # 14 - true if fusion of the Down position has encountered a numerical error
-bool fs_bad_acc_bias          # 15 - true if bad delta velocity bias estimates have been detected
-bool fs_bad_acc_vertical      # 16 - true if bad vertical accelerometer data has been detected
-bool fs_bad_acc_clipping      # 17 - true if delta velocity data contains clipping (asymmetric railing)
+bool fs_bad_acc_bias          #  9 - true if bad delta velocity bias estimates have been detected
+bool fs_bad_acc_vertical      # 10 - true if bad vertical accelerometer data has been detected
+bool fs_bad_acc_clipping      # 11 - true if delta velocity data contains clipping (asymmetric railing)
 
 
 # innovation test failures

msg/OrbTestMedium.msg

@@ -4,4 +4,6 @@ int32 val
 
 uint8[64] junk
 
+uint8 ORB_QUEUE_LENGTH = 16
+
 # TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll

msg/RtlStatus.msg

@@ -0,0 +1,15 @@
+uint64 timestamp                      # time since system start (microseconds)
+
+uint32 safe_points_id 		      # unique ID of active set of safe_point_items
+bool is_evaluation_pending 	      # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading).
+
+bool has_vtol_approach 		      # flag if approaches are defined for current RTL_TYPE parameter setting
+
+uint8 rtl_type	      		      # Type of RTL chosen
+uint8 safe_point_index 		      # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
+
+uint8 RTL_STATUS_TYPE_NONE=0       # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
+uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
+uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 	# RTL type is following the mission from closest point to mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position

platforms/common/pab_manifest.c

@@ -48,6 +48,8 @@
 
 #include <board_config.h>
 
+#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
+
 #include <inttypes.h>
 #include <stdbool.h>
 #include <syslog.h>
@@ -57,7 +59,6 @@
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
 
 typedef struct {
 	hw_base_id_t             hw_base_id; /* The ID of the Base */
@@ -292,12 +293,6 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
 		.mandatory   = 1,
 		.connection  = px4_hw_con_onboard,
 	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
 	{
 		.id          = PX4_MFT_ETHERNET,
 		.present     = 1,
@@ -315,6 +310,52 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
 // BASE ID 10  Skynode QS no USB Alaised to ID 9
 // BASE ID 16  Auterion Skynode RC10, RC11, RC12, RC13 Alaised to ID 0
 
+// BASE ID 17  Auterion Skynode RC13 with many parts removed
+static const px4_hw_mft_item_t base_configuration_17[] = {
+	{
+		.id          = PX4_MFT_PX4IO,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_USB,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.id          = PX4_MFT_CAN2,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_CAN3,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.id          = PX4_MFT_PM2,
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		.id          = PX4_MFT_ETHERNET,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_connector,
+	},
+	{
+		.id          = PX4_MFT_T100_ETH,
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
 //  ver_rev
@@ -328,6 +369,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{HW_BASE_ID(9),  base_configuration_9, arraySize(base_configuration_9)},  // Auterion Skynode ver 9
 	{HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)},  // Auterion Skynode ver 10
 	{HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)},  // Auterion Skynode ver 16
+	{HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)},  // Auterion Skynode ver 17
 };
 
 /************************************************************************************

platforms/common/px4_log.cpp

@@ -74,7 +74,7 @@ void px4_log_initialize(void)
 	log_message.severity = 6; // info
 	strcpy((char *)log_message.text, "initialized uORB logging");
 	log_message.timestamp = hrt_absolute_time();
-	orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH);
+	orb_log_message_pub = orb_advertise(ORB_ID(log_message), &log_message);
 }
 
 __EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...)

platforms/common/uORB/Publication.hpp

@@ -48,24 +48,6 @@
 namespace uORB
 {
 
-template <typename U> class DefaultQueueSize
-{
-private:
-	template <typename T>
-	static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *)
-	{
-		return T::ORB_QUEUE_LENGTH;
-	}
-
-	template <typename T> static constexpr uint8_t get_queue_size(...)
-	{
-		return 1;
-	}
-
-public:
-	static constexpr unsigned value = get_queue_size<U>(nullptr);
-};
-
 class PublicationBase
 {
 public:
@@ -97,7 +79,7 @@ protected:
 /**
  * uORB publication wrapper class
  */
-template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class Publication : public PublicationBase
 {
 public:
@@ -113,7 +95,7 @@ public:
 	bool advertise()
 	{
 		if (!advertised()) {
-			_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
+			_handle = orb_advertise(get_topic(), nullptr);
 		}
 
 		return advertised();

platforms/common/uORB/PublicationMulti.hpp

@@ -51,7 +51,7 @@ namespace uORB
 /**
  * Base publication multi wrapper class
  */
-template<typename T, uint8_t QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class PublicationMulti : public PublicationBase
 {
 public:
@@ -73,7 +73,7 @@ public:
 	{
 		if (!advertised()) {
 			int instance = 0;
-			_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE);
+			_handle = orb_advertise_multi(get_topic(), nullptr, &instance);
 		}
 
 		return advertised();

platforms/common/uORB/uORB.cpp

@@ -118,22 +118,11 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);
 }
 
-orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
-{
-	return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
-}
-
 orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance);
 }
 
-orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-				       unsigned int queue_size)
-{
-	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size);
-}
-
 int orb_unadvertise(orb_advert_t handle)
 {
 	return uORB::Manager::get_instance()->orb_unadvertise(handle);
@@ -227,6 +216,14 @@ const char *orb_get_c_type(unsigned char short_type)
 	return nullptr;
 }
 
+uint8_t orb_get_queue_depth(const struct orb_metadata *meta)
+{
+	if (meta) {
+		return meta->o_queue;
+	}
+
+	return 0;
+}
 
 void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name)
 {

platforms/common/uORB/uORB.h

@@ -53,6 +53,8 @@ struct orb_metadata {
 	const uint16_t o_size_no_padding;   /**< object size w/o padding at the end (for logger) */
 	uint32_t message_hash;	/**< Hash over all fields for message compatibility checks */
 	orb_id_size_t  o_id;                /**< ORB_ID enum */
+	uint8_t o_queue;					/**< queue size */
+
 };
 
 typedef const struct orb_metadata *orb_id_t;
@@ -102,14 +104,16 @@ typedef const struct orb_metadata *orb_id_t;
  * @param _size_no_padding	Struct size w/o padding at the end
  * @param _message_hash	32 bit message hash over all fields
  * @param _orb_id_enum	ORB ID enum e.g.: ORB_ID::vehicle_status
+ * @param _queue_size Queue size from topic definition
  */
-#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum)		\
-	const struct orb_metadata __orb_##_name = {	\
-		#_name,					\
-		sizeof(_struct),		\
-		_size_no_padding,			\
-		_message_hash,				\
-		_orb_id_enum				\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum, _queue_size)               \
+	const struct orb_metadata __orb_##_name = {     \
+		#_name,                                 \
+		sizeof(_struct),                \
+		_size_no_padding,                       \
+		_message_hash,                          \
+		_orb_id_enum,                           \
+		_queue_size                             \
 	}; struct hack
 
 __BEGIN_DECLS
@@ -135,23 +139,11 @@ typedef void 	*orb_advert_t;
  */
 extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
 
-/**
- * @see uORB::Manager::orb_advertise()
- */
-extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data,
-					unsigned int queue_size) __EXPORT;
-
 /**
  * @see uORB::Manager::orb_advertise_multi()
  */
 extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT;
 
-/**
- * @see uORB::Manager::orb_advertise_multi()
- */
-extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size) __EXPORT;
-
 /**
  * @see uORB::Manager::orb_unadvertise()
  */
@@ -160,7 +152,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT;
 /**
  * @see uORB::Manager::orb_publish()
  */
-extern int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
+extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
 
 /**
  * Advertise as the publisher of a topic.
@@ -241,6 +233,12 @@ extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
  */
 const char *orb_get_c_type(unsigned char short_type);
 
+/**
+ * Returns the queue depth of a topic
+ * @param meta orb topic metadata
+ */
+extern uint8_t orb_get_queue_depth(const struct orb_metadata *meta);
+
 /**
  * Print a topic to console. Do not call this directly, use print_message() instead.
  * @param meta orb topic metadata

platforms/common/uORB/uORBDeviceNode.cpp

@@ -73,12 +73,10 @@ static inline uint8_t round_pow_of_two_8(uint8_t n)
 	return value + 1;
 }
 
-uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path,
-			     uint8_t queue_size) :
+uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) :
 	CDev(strdup(path)), // success is checked in CDev::init
 	_meta(meta),
-	_instance(instance),
-	_queue_size(round_pow_of_two_8(queue_size))
+	_instance(instance)
 {
 }
 
@@ -186,7 +184,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 
 			/* re-check size */
 			if (nullptr == _data) {
-				const size_t data_size = _meta->o_size * _queue_size;
+				const size_t data_size = _meta->o_size * _meta->o_queue;
 				_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
 
 				if (_data) {
@@ -217,7 +215,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
 	unsigned generation = _generation.fetch_add(1);
 
-	memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
+	memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size);
 
 	// callbacks
 	for (auto item : _callbacks) {
@@ -254,13 +252,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 		*(uintptr_t *)arg = (uintptr_t)this;
 		return PX4_OK;
 
-	case ORBIOCSETQUEUESIZE: {
-			lock();
-			int ret = update_queue_size(arg);
-			unlock();
-			return ret;
-		}
-
 	case ORBIOCGETINTERVAL:
 		*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
 		return PX4_OK;
@@ -389,12 +380,11 @@ uORB::DeviceNode::print_statistics(int max_topic_length)
 
 	const uint8_t instance = get_instance();
 	const int8_t sub_count = subscriber_count();
-	const uint8_t queue_size = get_queue_size();
 
 	unlock();
 
 	PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
-		     queue_size, get_meta()->o_size, get_devname());
+		     get_meta()->o_queue, get_meta()->o_size, get_devname());
 
 	return true;
 }
@@ -483,21 +473,6 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data
 }
 #endif /* CONFIG_ORB_COMMUNICATOR */
 
-int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
-{
-	if (_queue_size == queue_size) {
-		return PX4_OK;
-	}
-
-	//queue size is limited to 255 for the single reason that we use uint8 to store it
-	if (_data || _queue_size > queue_size || queue_size > 255) {
-		return PX4_ERROR;
-	}
-
-	_queue_size = round_pow_of_two_8(queue_size);
-	return PX4_OK;
-}
-
 unsigned uORB::DeviceNode::get_initial_generation()
 {
 	ATOMIC_ENTER;

platforms/common/uORB/uORBDeviceNode.hpp

@@ -62,7 +62,7 @@ class UnitTest;
 class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
 {
 public:
-	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1);
+	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path);
 	virtual ~DeviceNode();
 
 	// no copy, assignment, move, move assignment
@@ -179,15 +179,6 @@ public:
 
 	void mark_as_advertised() { _advertised = true; }
 
-	/**
-	 * Try to change the size of the queue. This can only be done as long as nobody published yet.
-	 * This is the case, for example when orb_subscribe was called before an orb_advertise.
-	 * The queue size can only be increased.
-	 * @param queue_size new size of the queue
-	 * @return PX4_OK if queue size successfully set
-	 */
-	int update_queue_size(unsigned int queue_size);
-
 	/**
 	 * Print statistics
 	 * @param max_topic_length max topic name length for printing
@@ -195,7 +186,7 @@ public:
 	 */
 	bool print_statistics(int max_topic_length);
 
-	uint8_t get_queue_size() const { return _queue_size; }
+	uint8_t get_queue_size() const { return _meta->o_queue; }
 
 	int8_t subscriber_count() const { return _subscriber_count; }
 
@@ -234,7 +225,7 @@ public:
 	bool copy(void *dst, unsigned &generation)
 	{
 		if ((dst != nullptr) && (_data != nullptr)) {
-			if (_queue_size == 1) {
+			if (_meta->o_queue == 1) {
 				ATOMIC_ENTER;
 				memcpy(dst, _data, _meta->o_size);
 				generation = _generation.load();
@@ -253,12 +244,12 @@ public:
 				}
 
 				// Compatible with normal and overflow conditions
-				if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
+				if (!is_in_range(current_generation - _meta->o_queue, generation, current_generation - 1)) {
 					// Reader is too far behind: some messages are lost
-					generation = current_generation - _queue_size;
+					generation = current_generation - _meta->o_queue;
 				}
 
-				memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
+				memcpy(dst, _data + (_meta->o_size * (generation % _meta->o_queue)), _meta->o_size);
 				ATOMIC_LEAVE;
 
 				++generation;
@@ -295,7 +286,7 @@ private:
 
 	const uint8_t _instance; /**< orb multi instance identifier */
 	bool _advertised{false};  /**< has ever been advertised (not necessarily published data yet) */
-	uint8_t _queue_size; /**< maximum number of elements in the queue */
+
 	int8_t _subscriber_count{0};
 
 

platforms/common/uORB/uORBManager.cpp

@@ -265,8 +265,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 	return ret;
 }
 
-orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size)
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 #ifdef ORB_USE_PUBLISHER_RULES
 
@@ -300,19 +299,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}
 
-	/* Set the queue size. This must be done before the first publication; thus it fails if
-	 * this is not the first advertiser.
-	 */
-	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
-
-	if (result < 0 && queue_size > 1) {
-		PX4_WARN("orb_advertise_multi: failed to set queue size");
-	}
-
 	/* get the advertiser handle and close the node */
 	orb_advert_t advertiser;
 
-	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);
 
 	if (result == PX4_ERROR) {
@@ -602,6 +592,22 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 {
 	PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);
 
+	// First make sure this is a valid topic
+	const struct orb_metadata *const *topic_list = orb_get_topics();
+	orb_id_t topic_ptr = nullptr;
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
+			topic_ptr = topic_list[i];
+			break;
+		}
+	}
+
+	if (! topic_ptr) {
+		PX4_ERR("process_remote_topic meta not found for %s\n", topic_name);
+		return -1;
+	}
+
 	// Look to see if we already have a node for this topic
 	char nodepath[orb_maxpath];
 	int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
@@ -613,7 +619,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 			uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
 
 			if (node) {
-				PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
+				PX4_DEBUG("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
 				node->mark_as_advertised();
 				_remote_topics.insert(topic_name);
 				return 0;
@@ -622,27 +628,9 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
 	}
 
 	// We didn't find a node so we need to create it via an advertisement
-	const struct orb_metadata *const *topic_list = orb_get_topics();
-	orb_id_t topic_ptr = nullptr;
-
-	for (size_t i = 0; i < orb_topics_count(); i++) {
-		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
-			topic_ptr = topic_list[i];
-			break;
-		}
-	}
-
-	if (topic_ptr) {
-		PX4_INFO("Advertising remote topic %s", topic_name);
-		_remote_topics.insert(topic_name);
-		// Add some queue depth when advertising remote topics. These
-		// topics may get aggregated and thus delivered in a batch that
-		// requires some buffering in a queue.
-		orb_advertise(topic_ptr, nullptr, 5);
-
-	} else {
-		PX4_INFO("process_remote_topic meta not found for %s\n", topic_name);
-	}
+	PX4_DEBUG("Advertising remote topic %s", topic_name);
+	_remote_topics.insert(topic_name);
+	orb_advertise(topic_ptr, nullptr, topic_ptr->o_queue);
 
 	return 0;
 }
@@ -663,8 +651,11 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName)
 			PX4_DEBUG("DeviceNode(%s) not created yet", messageName);
 
 		} else {
-			// node is present.
-			node->process_add_subscription();
+			// node is present. But don't send any data to it if it
+			// is a node advertised by the remote side
+			if (_remote_topics.find(messageName) == false) {
+				node->process_add_subscription();
+			}
 		}
 
 	} else {

platforms/common/uORB/uORBManager.hpp

@@ -215,17 +215,15 @@ public:
 	 * @param data    A pointer to the initial data to be published.
 	 *      For topics updated by interrupt handlers, the advertisement
 	 *      must be performed from non-interrupt context.
-	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
-	 *      used.
 	 * @return    nullptr on error, otherwise returns an object pointer
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
+	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data = nullptr)
 	{
-		return orb_advertise_multi(meta, data, nullptr, queue_size);
+		return orb_advertise_multi(meta, data, nullptr);
 	}
 
 	/**
@@ -250,16 +248,13 @@ public:
 	 * @param instance  Pointer to an integer which will yield the instance ID (0-based)
 	 *      of the publication. This is an output parameter and will be set to the newly
 	 *      created instance, ie. 0 for the first advertiser, 1 for the next and so on.
-	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
-	 *      used.
 	 * @return    nullptr on error, otherwise returns a handle
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-					 unsigned int queue_size = 1);
+	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance);
 
 	/**
 	 * Unadvertise a topic.

platforms/common/uORB/uORBManagerUsr.cpp

@@ -89,8 +89,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 	return data.ret;
 }
 
-orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		unsigned int queue_size)
+orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
 {
 	/* open the node as an advertiser */
 	int fd = node_open(meta, true, instance);
@@ -100,19 +99,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}
 
-	/* Set the queue size. This must be done before the first publication; thus it fails if
-	 * this is not the first advertiser.
-	 */
-	int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
-
-	if (result < 0 && queue_size > 1) {
-		PX4_WARN("orb_advertise_multi: failed to set queue size");
-	}
-
 	/* get the advertiser handle and close the node */
 	orb_advert_t advertiser;
 
-	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);
 
 	if (result == PX4_ERROR) {

platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp

@@ -574,8 +574,8 @@ int uORBTest::UnitTest::test_wrap_around()
 	bool updated{false};
 
 	// Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation
-	const int queue_size = 16;
-	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size);
+	const int queue_size = orb_test_medium_s::ORB_QUEUE_LENGTH;
+	ptopic = orb_advertise(ORB_ID(orb_test_medium_wrap_around), nullptr);
 
 	if (ptopic == nullptr) {
 		return test_fail("advertise failed: %d", errno);
@@ -828,9 +828,9 @@ int uORBTest::UnitTest::test_queue()
 		return test_fail("subscribe failed: %d", errno);
 	}
 
-	const int queue_size = 16;
+	const int queue_size = orb_test_medium_s::ORB_QUEUE_LENGTH;
 	orb_test_medium_s t{};
-	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
+	ptopic = orb_advertise(ORB_ID(orb_test_medium_queue), &t);
 
 	if (ptopic == nullptr) {
 		return test_fail("advertise failed: %d", errno);
@@ -935,9 +935,9 @@ int uORBTest::UnitTest::pub_test_queue_main()
 {
 	orb_test_medium_s t{};
 	orb_advert_t ptopic{nullptr};
-	const int queue_size = 50;
+	const int queue_size = orb_test_medium_s::ORB_QUEUE_LENGTH;
 
-	if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
+	if ((ptopic = orb_advertise(ORB_ID(orb_test_medium_queue_poll), &t)) == nullptr) {
 		_thread_should_exit = true;
 		return test_fail("advertise failed: %d", errno);
 	}

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

@@ -40,15 +40,34 @@
 #include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 #include <uORB/uORB.h>
 
+#if defined(CONFIG_MODULES_MUORB_APPS)
+extern "C" { int muorb_init(); }
+#endif
+
 int px4_platform_init(void)
 {
 	hrt_init();
 
 	px4::WorkQueueManagerStart();
 
+// MUORB has slightly different startup requirements
+#if defined(CONFIG_MODULES_MUORB_APPS)
+	//Put sleeper in here to allow wq to finish initializing before param_init is called
+	usleep(10000);
+
+	uorb_start();
+
+	muorb_init();
+
+	// Give muorb some time to setup the DSP
+	usleep(100000);
+
+	param_init();
+#else
 	param_init();
 
 	uorb_start();
+#endif
 
 	px4_log_initialize();
 

platforms/ros2/include/uORB/Publication.hpp

@@ -45,28 +45,10 @@
 namespace uORB
 {
 
-template <typename U> class DefaultQueueSize
-{
-private:
-	template <typename T>
-	static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *)
-	{
-		return T::ORB_QUEUE_LENGTH;
-	}
-
-	template <typename T> static constexpr uint8_t get_queue_size(...)
-	{
-		return 1;
-	}
-
-public:
-	static constexpr unsigned value = get_queue_size<U>(nullptr);
-};
-
 /**
  * uORB publication wrapper class
  */
-template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
+template<typename T>
 class Publication
 {
 public:

src/drivers/barometer/bmp388/bmp388.cpp

@@ -77,6 +77,7 @@ BMP388::init()
 
 	if (_chip_id == BMP390_CHIP_ID) {
 		_interface->set_device_type(DRV_BARO_DEVTYPE_BMP390);
+		this->_item_name = "bmp390";
 	}
 
 	_chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);

src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp

@@ -251,7 +251,7 @@ ICP201XX::RunImpl()
 	case STATE::CONFIG: {
 			if (configure()) {
 				_state = STATE::WAIT_READ;
-				ScheduleDelayed(30_ms);
+				ScheduleDelayed(50_ms);
 
 			} else {
 				if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {

src/drivers/camera_trigger/camera_trigger.cpp

@@ -314,7 +314,7 @@ CameraTrigger::CameraTrigger() :
 	// Advertise critical publishers here, because we cannot advertise in interrupt context
 	camera_trigger_s trigger{};
 
-	_trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH);
+	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
 }
 
 CameraTrigger::~CameraTrigger()

src/drivers/drv_orb_dev.h

@@ -64,13 +64,10 @@
 /** Get the priority for the topic */
 #define ORBIOCGPRIORITY		_ORBIOC(14)
 
-/** Set the queue size of the topic */
-#define ORBIOCSETQUEUESIZE	_ORBIOC(15)
-
 /** Get the minimum interval at which the topic can be seen to be updated for this subscription */
-#define ORBIOCGETINTERVAL	_ORBIOC(16)
+#define ORBIOCGETINTERVAL	_ORBIOC(15)
 
 /** Check whether the topic is advertised, sets *(unsigned long *)arg to 1 if advertised, 0 otherwise */
-#define ORBIOCISADVERTISED	_ORBIOC(17)
+#define ORBIOCISADVERTISED	_ORBIOC(16)
 
 #endif /* _DRV_UORB_H */

src/drivers/imu/invensense/icm42688p/ICM42688P.hpp

@@ -88,7 +88,7 @@ private:
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
 	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)), "Invalid transfer buffer size");
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;

src/drivers/tone_alarm/ToneAlarm.cpp

@@ -45,8 +45,6 @@ using namespace time_literals;
 ToneAlarm::ToneAlarm() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
-	// ensure ORB_ID(tune_control) is advertised with correct queue depth
-	orb_advertise_queue(ORB_ID(tune_control), nullptr, tune_control_s::ORB_QUEUE_LENGTH);
 }
 
 ToneAlarm::~ToneAlarm()

src/drivers/voxl2_io/CMakeLists.txt

@@ -45,6 +45,7 @@ px4_add_module(
 		voxl2_io.cpp
 		voxl2_io.hpp
 	DEPENDS
+		rc
 		px4_work_queue
 		mixer_module
 	MODULE_CONFIG

src/lib/adsb/AdsbConflict.h

@@ -151,7 +151,7 @@ private:
 
 	transponder_report_s tr{};
 
-	orb_advert_t fake_traffic_report_publisher = orb_advertise_queue(ORB_ID(transponder_report), &tr, (unsigned int)20);
+	orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr);
 
 	TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};
 

src/lib/events/events.cpp

@@ -61,7 +61,7 @@ void send(EventType &event)
 		orb_publish(ORB_ID(event), orb_event_pub, &event);
 
 	} else {
-		orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH);
+		orb_event_pub = orb_advertise(ORB_ID(event), &event);
 	}
 
 	pthread_mutex_unlock(&publish_event_mutex);

src/lib/geo/test_geo.cpp

@@ -42,7 +42,7 @@ class GeoTest : public ::testing::Test
 public:
 	void SetUp() override
 	{
-		proj.initReference(math::radians(473566094 / 1e7), math::radians(85190237 / 1e7), 0);
+		proj.initReference(473566094 / 1e7, 85190237 / 1e7, 0);
 	}
 
 protected:
@@ -73,7 +73,7 @@ TEST_F(GeoTest, reprojectProject)
 
 TEST_F(GeoTest, projectReproject)
 {
-	// GIVEN: x and y coordinates in the local cartesian frame
+	// GIVEN: lat and lon coordinates in the geographic coordinate system
 	double lat = 47.356616973876953;
 	double lon = 8.5190505981445313;
 	float x;

src/lib/matrix/matrix/Matrix.hpp

@@ -162,6 +162,24 @@ public:
 		return res;
 	}
 
+	// Using this function reduces the number of temporary variables needed to compute A * B.T
+	template<size_t P>
+	Matrix<Type, M, M> multiplyByTranspose(const Matrix<Type, P, N> &other) const
+	{
+		Matrix<Type, M, P> res;
+		const Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t k = 0; k < P; k++) {
+				for (size_t j = 0; j < N; j++) {
+					res(i, k) += self(i, j) * other(k, j);
+				}
+			}
+		}
+
+		return res;
+	}
+
 	// Element-wise multiplication
 	Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
 	{
@@ -391,12 +409,6 @@ public:
 			for (unsigned j = 0; j < N; j++) {
 				double d = static_cast<double>(self(i, j));
 
-				// Matrix diagonal elements
-				if (N > 1 && M > 1 && i == j) {
-					// make diagonal elements bold (ANSI CSI n 1)
-					printf("\033[1m");
-				}
-
 				// if symmetric don't print upper triangular elements
 				if ((M == N) && (j > i) && (i < N) && (j < M)
 				    && (fabs(d - static_cast<double>(self(j, i))) < (double)eps)
@@ -417,12 +429,6 @@ public:
 						printf("% 6.5f ", d);
 					}
 				}
-
-				// Matrix diagonal elements
-				if (N > 1 && M > 1 && i == j) {
-					// reset any formatting (ANSI CSI n 0)
-					printf("\033[0m");
-				}
 			}
 
 			printf("\n");

src/lib/parameters/px4params/injectxmlparams.py

@@ -14,7 +14,7 @@ class XMLInject():
     def __init__(self, injected_xml_filename):
         self.groups=[]
 
-        valid_parameter_attributes = set(["category", "default", "name", "type", "volatile"])
+        valid_parameter_attributes = set(["category", "default", "name", "type", "volatile", "boolean"])
         valid_field_tags = set(["board","short_desc", "long_desc", "min", "max", "unit", "decimal", "increment", "reboot_required"])
         valid_other_top_level_tags = set(["group","values"])
 
@@ -42,7 +42,8 @@ class XMLInject():
                         new_param.default = iparam.get('default')
                     elif param_attrib == 'volatile':
                         new_param.SetVolatile()
-
+                    elif param_attrib == "boolean":
+                        new_param.SetBoolean()
 
                 #get param info stored as child tags
                 for child in iparam:

src/lib/systemlib/mavlink_log.cpp

@@ -73,6 +73,6 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 		orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg);
 
 	} else {
-		*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH);
+		*mavlink_log_pub = orb_advertise(ORB_ID(mavlink_log), &log_msg);
 	}
 }

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -156,18 +156,18 @@ private:
 #ifndef CONSTRAINED_FLASH
 		&_external_checks,
 #endif
-		//&_accelerometer_checks,
-		//&_airspeed_checks,
+		&_accelerometer_checks,
+		&_airspeed_checks,
 		&_arm_permission_checks,
-		//&_baro_checks,
+		&_baro_checks,
 		&_cpu_resource_checks,
 		&_distance_sensor_checks,
 		&_esc_checks,
 		&_estimator_checks,
 		&_failure_detector_checks,
-		//&_gyro_checks,
-		//&_imu_consistency_checks,
-		//&_magnetometer_checks,
+		&_gyro_checks,
+		&_imu_consistency_checks,
+		&_magnetometer_checks,
 		&_manual_control_checks,
 		&_home_position_checks,
 		&_mission_checks,

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp

@@ -55,7 +55,7 @@ public:
 	void SetUp() override
 	{
 		// ensure topic exists, otherwise we might lose first queued events
-		orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH);
+		orb_advertise(ORB_ID(event), nullptr);
 	}
 
 };

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

@@ -99,7 +99,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}
 
-	if (missing_data && _param_sys_mc_est_group.get() == 2 && false) {
+	if (missing_data && _param_sys_mc_est_group.get() == 2) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,

src/modules/commander/commander_helper.cpp

@@ -153,7 +153,7 @@ int buzzer_init()
 	tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW] = 800000;
 	tune_durations[tune_control_s::TUNE_ID_SINGLE_BEEP] = 300000;
 
-	tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH);
+	tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control);
 
 	return PX4_OK;
 }
@@ -330,7 +330,7 @@ int led_init()
 	led_control.mode = led_control_s::MODE_OFF;
 	led_control.priority = 0;
 	led_control.timestamp = hrt_absolute_time();
-	led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
+	led_control_pub = orb_advertise(ORB_ID(led_control), &led_control);
 
 	/* first open normal LEDs */
 	fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR);

src/modules/commander/mag_calibration.cpp

@@ -1006,8 +1006,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 			return PX4_ERROR;
 		}
 
-		calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees",
-					 (double)math::radians(heading_radians));
+		calibration_log_info(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees",
+				     (double)math::degrees(heading_radians));
 
 		matrix::Eulerf euler{matrix::Quatf{attitude.q}};
 		euler(2) = heading_radians;

src/modules/ekf2/CMakeLists.txt

@@ -127,7 +127,8 @@ list(APPEND EKF_SRCS
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
 	EKF/output_predictor.cpp
-	EKF/vel_pos_fusion.cpp
+	EKF/velocity_fusion.cpp
+	EKF/position_fusion.cpp
 	EKF/yaw_fusion.cpp
 	EKF/zero_innovation_heading_update.cpp
 

src/modules/ekf2/EKF/CMakeLists.txt

@@ -44,7 +44,8 @@ list(APPEND EKF_SRCS
 	height_control.cpp
 	imu_down_sampler.cpp
 	output_predictor.cpp
-	vel_pos_fusion.cpp
+	velocity_fusion.cpp
+	position_fusion.cpp
 	yaw_fusion.cpp
 	zero_innovation_heading_update.cpp
 

src/modules/ekf2/EKF/EKFGSF_yaw.h

@@ -67,11 +67,11 @@ public:
 
 	void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }
 
-	void setGyroBias(const Vector3f &imu_gyro_bias)
+	void setGyroBias(const Vector3f &imu_gyro_bias, const bool force = false)
 	{
 		// Initialise to gyro bias estimate from main filter because there could be a large
 		// uncorrected rate gyro bias error about the gravity vector
-		if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started) {
+		if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started || force) {
 			// init gyro bias for each model
 			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
 				_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;

src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp

@@ -60,22 +60,13 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 		if (zero_gyro_update_data_ready) {
 
 			Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt;
-			Vector3f innovation = ekf.state().gyro_bias - gyro_bias;
 
 			const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f));
 
-			const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var;
-
-			for (int i = 0; i < 3; i++) {
-				Ekf::VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
-				const unsigned state_index = State::gyro_bias.idx + i;
-
-				// calculate kalman gain K = PHS, where S = 1/innovation variance
-				for (int row = 0; row < State::size; row++) {
-					K(row) = ekf.stateCovariance(row, state_index) / innov_var(i);
-				}
-
-				ekf.measurementUpdate(K, innov_var(i), innovation(i));
+			for (unsigned i = 0; i < 3; i++) {
+				const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i);
+				const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var;
+				ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i);
 			}
 
 			// Reset the integrators

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp

@@ -66,7 +66,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 
 			for (unsigned i = 0; i < 3; i++) {
 				const float innovation = ekf.state().vel(i) - vel_obs(i);
-				ekf.fuseVelPosHeight(innovation, innov_var(i), State::vel.idx + i);
+				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}
 
 			_time_last_zero_velocity_fuse = imu_delayed.time_us;

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -93,7 +93,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
 		const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
 		const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant
-		                                         && (is_airspeed_consistent || !_control_status.flags.wind); // if wind isn't already estimated, the states are reset when starting airspeed fusion
+		                                         && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning);
 
 		if (_control_status.flags.fuse_aspd) {
 			if (continuing_conditions_passing) {
@@ -118,17 +118,16 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		} else if (starting_conditions_passing) {
 			ECL_INFO("starting airspeed fusion");
 
-			// If starting wind state estimation, reset the wind states and covariances before fusing any data
-			// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
-			const Vector2f wind_var_xy = getWindVelocityVariance();
+			if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
+				resetVelUsingAirspeed(airspeed_sample);
 
-			if (!_control_status.flags.wind || (wind_var_xy(0) + wind_var_xy(1) > sq(_params.initial_wind_uncertainty))) {
-				// activate the wind states
-				_control_status.flags.wind = true;
-				// reset the wind speed states and corresponding covariances
+			} else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) {
+				// If starting wind state estimation, reset the wind states and covariances before fusing any data
+				// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
 				resetWindUsingAirspeed(airspeed_sample);
 			}
 
+			_control_status.flags.wind = true;
 			_control_status.flags.fuse_aspd = true;
 		}
 
@@ -208,7 +207,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation);
+	const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
 
 	aid_src.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;
@@ -247,3 +246,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 
 	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
 }
+
+void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample)
+{
+	const float euler_yaw = getEulerYaw(_R_to_earth);
+
+	// Estimate velocity using zero sideslip assumption and airspeed measurement
+	Vector2f horizontal_velocity;
+	horizontal_velocity(0) = _state.wind_vel(0) + airspeed_sample.true_airspeed * cosf(euler_yaw);
+	horizontal_velocity(1) = _state.wind_vel(1) + airspeed_sample.true_airspeed * sinf(euler_yaw);
+
+	float vel_var = NAN; // Do not reset the velocity variance as wind variance estimate is most likely not correct
+	resetHorizontalVelocityTo(horizontal_velocity, vel_var);
+
+	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
+}

src/modules/ekf2/EKF/auxvel_fusion.cpp

@@ -42,10 +42,10 @@ void Ekf::controlAuxVelFusion()
 
 			resetEstimatorAidStatus(_aid_src_aux_vel);
 
-			updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
+			updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
 
 			if (isHorizontalAidingActive()) {
-				fuseVelocity(_aid_src_aux_vel);
+				fuseHorizontalVelocity(_aid_src_aux_vel);
 			}
 		}
 	}

src/modules/ekf2/EKF/common.h

@@ -364,7 +364,6 @@ struct parameters {
 	int32_t mag_declination_source{7};      ///< bitmask used to control the handling of declination data
 	int32_t mag_fusion_type{0};             ///< integer used to specify the type of magnetometer fusion used
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
-	float mag_yaw_rate_gate{0.20f};         ///< yaw rate threshold used by mode select logic (rad/sec)
 
 	// compute synthetic magnetomter Z value if possible
 	int32_t synthesize_mag_z{0};
@@ -509,15 +508,9 @@ union fault_status_u {
 		bool bad_sideslip      : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 		bool bad_optflow_X     : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
 		bool bad_optflow_Y     : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
-		bool bad_vel_N         : 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error
-		bool bad_vel_E         : 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error
-		bool bad_vel_D         : 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error
-		bool bad_pos_N         : 1; ///< 12 - true if fusion of the North position has encountered a numerical error
-		bool bad_pos_E         : 1; ///< 13 - true if fusion of the East position has encountered a numerical error
-		bool bad_pos_D         : 1; ///< 14 - true if fusion of the Down position has encountered a numerical error
-		bool bad_acc_bias      : 1; ///< 15 - true if bad delta velocity bias estimates have been detected
-		bool bad_acc_vertical  : 1; ///< 16 - true if bad vertical accelerometer data has been detected
-		bool bad_acc_clipping  : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
+		bool bad_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
+		bool bad_acc_vertical  : 1; ///< 10 - true if bad vertical accelerometer data has been detected
+		bool bad_acc_clipping  : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/covariance.cpp

@@ -166,11 +166,21 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	if (_control_status.flags.mag) {
 		// mag_I: add process noise
 		float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
-		P.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) += sq(mag_I_sig);
+		float mag_I_process_noise = sq(mag_I_sig);
+
+		for (unsigned index = 0; index < State::mag_I.dof; index++) {
+			const unsigned i = State::mag_I.idx + index;
+			P(i, i) += mag_I_process_noise;
+		}
 
 		// mag_B: add process noise
 		float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
-		P.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) += sq(mag_B_sig);
+		float mag_B_process_noise = sq(mag_B_sig);
+
+		for (unsigned index = 0; index < State::mag_B.dof; index++) {
+			const unsigned i = State::mag_B.idx + index;
+			P(i, i) += mag_B_process_noise;
+		}
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -179,7 +189,12 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	if (_control_status.flags.wind) {
 		// wind vel: add process noise
 		float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
-		P.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) += sq(wind_vel_nsd_scaled) * dt;
+		float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
+
+		for (unsigned index = 0; index < State::wind_vel.dof; index++) {
+			const unsigned i = State::wind_vel.idx + index;
+			P(i, i) += wind_vel_process_noise;
+		}
 	}
 #endif // CONFIG_EKF2_WIND
 
@@ -222,14 +237,6 @@ void Ekf::constrainStateVariances()
 #endif // CONFIG_EKF2_WIND
 }
 
-void Ekf::forceCovarianceSymmetry()
-{
-	// DEBUG
-	// P.isBlockSymmetric(0, 1e-9f);
-
-	P.makeRowColSymmetric<State::size>(0);
-}
-
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
 {
 	for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
@@ -256,22 +263,6 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
 	}
 }
 
-// if the covariance correction will result in a negative variance, then
-// the covariance matrix is unhealthy and must be corrected
-bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
-{
-	bool healthy = true;
-
-	for (int i = 0; i < State::size; i++) {
-		if (P(i, i) < KHP(i, i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.f);
-			healthy = false;
-		}
-	}
-
-	return healthy;
-}
-
 void Ekf::resetQuatCov(const float yaw_noise)
 {
 	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -160,7 +160,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 			VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index];
 
-			if (measurementUpdate(K, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) {
+			if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) {
 				fused[axis_index] = true;
 			}
 		}

src/modules/ekf2/EKF/ekf.cpp

@@ -326,14 +326,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	const float alpha = 1.0f - imu_delayed.delta_vel_dt;
 	_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
 
-	// calculate a yaw change about the earth frame vertical
-	const float spin_del_ang_D = corrected_delta_ang.dot(Vector3f(_R_to_earth.row(2)));
-	_yaw_delta_ef += spin_del_ang_D;
-
-	// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
-	// Note fixed coefficients are used to save operations. The exact time constant is not important.
-	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
-
 	// Calculate low pass filtered height rate
 	float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;

src/modules/ekf2/EKF/ekf.h

@@ -327,8 +327,8 @@ public:
 #endif
 	}
 
-	// fuse single velocity and position measurement
-	bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
+	// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
+	bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);
 
 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
@@ -468,35 +468,49 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
-	bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
+	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation)
 	{
 		clearInhibitedStateKalmanGains(K);
 
-		const VectorState KS = K * innovation_variance;
-		SquareMatrixState KHP;
+#if false
+		// Matrix implementation of the Joseph stabilized covariance update
+		// This is extremely expensive to compute. Use for debugging purposes only.
+		auto A = matrix::eye<float, State::size>();
+		A -= K.multiplyByTranspose(H);
+		P = A * P;
+		P = P.multiplyByTranspose(A);
 
-		for (unsigned row = 0; row < State::size; row++) {
-			for (unsigned col = 0; col < State::size; col++) {
-				// Instad of literally computing KHP, use an equvalent
-				// equation involving less mathematical operations
-				KHP(row, col) = KS(row) * K(col);
+		const VectorState KR = K * R;
+		P += KR.multiplyByTranspose(K);
+#else
+		// Efficient implementation of the Joseph stabilized covariance update
+		// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
+
+		// Step 1: conventional update
+		VectorState PH = P * H;
+
+		for (unsigned i = 0; i < State::size; i++) {
+			for (unsigned j = 0; j < State::size; j++) {
+				P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed)
 			}
 		}
 
-		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
+		// Step 2: stabilized update
+		PH = P * H;
 
-		if (is_healthy) {
-			// apply the covariance corrections
-			P -= KHP;
-
-			constrainStateVariances();
-			forceCovarianceSymmetry();
-
-			// apply the state corrections
-			fuse(K, innovation);
+		for (unsigned i = 0; i < State::size; i++) {
+			for (unsigned j = 0; j <= i; j++) {
+				P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
+				P(j, i) = P(i, j);
+			}
 		}
+#endif
 
-		return is_healthy;
+		constrainStateVariances();
+
+		// apply the state corrections
+		fuse(K, innovation);
+		return true;
 	}
 
 	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
@@ -572,8 +586,6 @@ private:
 
 	Vector2f _accel_lpf_NE{};			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 	float _height_rate_lpf{0.0f};
-	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
-	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)
 
 	SquareMatrixState P{};	///< state covariance matrix
 
@@ -702,9 +714,7 @@ private:
 	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)
 
 	// used by magnetometer fusion mode selection
-	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
 	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
 	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
 	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.
@@ -720,7 +730,6 @@ private:
 
 	// Variables used to control activation of post takeoff functionality
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
-	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	uint64_t _time_last_mag_check_failing{0};
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -784,6 +793,7 @@ private:
 
 	// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
 	void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
+	void resetVelUsingAirspeed(const airspeedSample &airspeed_sample);
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_SIDESLIP)
@@ -828,7 +838,7 @@ private:
 	void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
 
 	// 2d & 3d velocity aid source
-	void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
+	void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
 	void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;
 
 	// horizontal and vertical position fusion
@@ -836,7 +846,7 @@ private:
 	void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
 
 	// 2d & 3d velocity fusion
-	void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
+	void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
 	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
 
 #if defined(CONFIG_EKF2_TERRAIN)
@@ -942,15 +952,9 @@ private:
 #endif // CONFIG_EKF2_WIND
 	}
 
-	// if the covariance correction will result in a negative variance, then
-	// the covariance matrix is unhealthy and must be corrected
-	bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP);
-
 	// limit the diagonal of the covariance matrix
 	void constrainStateVariances();
 
-	void forceCovarianceSymmetry();
-
 	void constrainStateVar(const IdxDof &state, float min, float max);
 	void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f);
 
@@ -1049,7 +1053,6 @@ private:
 	bool haglYawResetReq();
 
 	void checkYawAngleObservability();
-	void checkMagBiasObservability();
 	void checkMagHeadingConsistency();
 
 	bool checkMagField(const Vector3f &mag);
@@ -1137,8 +1140,6 @@ private:
 	void resetFakePosFusion();
 	void stopFakePosFusion();
 
-	void setVelPosStatus(const int state_index, const bool healthy);
-
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)
 	// yaw_variance : yaw error variance (rad^2)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -45,121 +45,6 @@
 #include <lib/world_magnetic_model/geo_mag_declination.h>
 #include <cstdlib>
 
-void Ekf::resetHorizontalVelocityToZero()
-{
-	_information_events.flags.reset_vel_to_zero = true;
-	ECL_INFO("reset velocity to zero");
-	// Used when falling back to non-aiding mode of operation
-	resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
-}
-
-void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
-{
-	resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
-	resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
-}
-
-void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
-{
-	const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
-	_state.vel.xy() = new_horz_vel;
-
-	if (PX4_ISFINITE(new_horz_vel_var(0))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
-	}
-
-	if (PX4_ISFINITE(new_horz_vel_var(1))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
-	}
-
-	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
-
-	// record the state change
-	if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
-		_state_reset_status.velNE_change = delta_horz_vel;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.velNE_change += delta_horz_vel;
-	}
-
-	_state_reset_status.reset_count.velNE++;
-
-	// Reset the timout timer
-	_time_last_hor_vel_fuse = _time_delayed_us;
-}
-
-void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
-{
-	const float delta_vert_vel = new_vert_vel - _state.vel(2);
-	_state.vel(2) = new_vert_vel;
-
-	if (PX4_ISFINITE(new_vert_vel_var)) {
-		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
-	}
-
-	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
-
-	// record the state change
-	if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
-		_state_reset_status.velD_change = delta_vert_vel;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.velD_change += delta_vert_vel;
-	}
-
-	_state_reset_status.reset_count.velD++;
-
-	// Reset the timout timer
-	_time_last_ver_vel_fuse = _time_delayed_us;
-}
-
-void Ekf::resetHorizontalPositionToLastKnown()
-{
-	ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
-
-	_information_events.flags.reset_pos_to_last_known = true;
-
-	// Used when falling back to non-aiding mode of operation
-	resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
-}
-
-void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
-{
-	const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
-	_state.pos.xy() = new_horz_pos;
-
-	if (PX4_ISFINITE(new_horz_pos_var(0))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
-	}
-
-	if (PX4_ISFINITE(new_horz_pos_var(1))) {
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
-	}
-
-	_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
-
-	// record the state change
-	if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
-		_state_reset_status.posNE_change = delta_horz_pos;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.posNE_change += delta_horz_pos;
-	}
-
-	_state_reset_status.reset_count.posNE++;
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
-
-	// Reset the timout timer
-	_time_last_hor_pos_fuse = _time_delayed_us;
-}
-
 bool Ekf::isHeightResetRequired() const
 {
 	// check if height is continuously failing because of accel errors
@@ -171,68 +56,6 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }
 
-void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) {
-    _information_events.flags.reset_pos_to_ext_obs = true;
-    ECL_INFO("reset position to external observation");
-    resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
-}
-
-void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
-{
-	const float old_vert_pos = _state.pos(2);
-	_state.pos(2) = new_vert_pos;
-
-	if (PX4_ISFINITE(new_vert_pos_var)) {
-		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
-	}
-
-	const float delta_z = new_vert_pos - old_vert_pos;
-
-	// apply the change in height / height rate to our newest height / height rate estimate
-	// which have already been taken out from the output buffer
-	_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
-
-	// record the state change
-	if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
-		_state_reset_status.posD_change = delta_z;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.posD_change += delta_z;
-	}
-
-	_state_reset_status.reset_count.posD++;
-
-#if defined(CONFIG_EKF2_BAROMETER)
-	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_BAROMETER
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_GNSS)
-	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_GNSS
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_TERRAIN)
-	terrainHandleVerticalPositionReset(delta_z);
-#endif
-
-	// Reset the timout timer
-	_time_last_hgt_fuse = _time_delayed_us;
-}
-
-void Ekf::resetVerticalVelocityToZero()
-{
-	// we don't know what the vertical velocity is, so set it to zero
-	// Set the variance to a value large enough to allow the state to converge quickly
-	// that does not destabilise the filter
-	resetVerticalVelocityTo(0.0f, 10.f);
-}
-
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
 float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
 {
@@ -684,7 +507,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
 	ekf_solution_status_u soln_status{};
 	// TODO: Is this accurate enough?
-	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
+	soln_status.flags.attitude = attitude_valid();
 	soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
 	soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
@@ -1015,3 +838,57 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 		_accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
 	}
 }
+
+bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
+{
+	VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
+
+	// calculate kalman gain K = PHS, where S = 1/innovation variance
+	for (int row = 0; row < State::size; row++) {
+		K(row) = P(row, state_index) / innov_var;
+	}
+
+	clearInhibitedStateKalmanGains(K);
+
+#if false
+	// Matrix implementation of the Joseph stabilized covariance update
+	// This is extremely expensive to compute. Use for debugging purposes only.
+	auto A = matrix::eye<float, State::size>();
+	VectorState H;
+	H(state_index) = 1.f;
+	A -= K.multiplyByTranspose(H);
+	P = A * P;
+	P = P.multiplyByTranspose(A);
+
+	const VectorState KR = K * R;
+	P += KR.multiplyByTranspose(K);
+#else
+	// Efficient implementation of the Joseph stabilized covariance update
+	// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
+
+	// Step 1: conventional update
+	VectorState PH = P.row(state_index);
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j < State::size; j++) {
+			P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed)
+		}
+	}
+
+	// Step 2: stabilized update
+	PH = P.row(state_index);
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j <= i; j++) {
+			P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
+			P(j, i) = P(i, j);
+		}
+	}
+#endif
+
+	constrainStateVariances();
+
+	// apply the state corrections
+	fuse(K, innov);
+	return true;
+}

src/modules/ekf2/EKF/ev_control.cpp

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

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -45,7 +45,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 
 	HeightBiasEstimator &bias_est = _ev_hgt_b_est;
 
-	bias_est.predict(_dt_ekf_avg);
+	// bias_est.predict(_dt_ekf_avg) called by controlExternalVisionFusion()
 
 	// correct position for offset relative to IMU
 	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;

src/modules/ekf2/EKF/gps_control.cpp

@@ -47,7 +47,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	}
 
 	if (!gyro_bias_inhibited()) {
-		_yawEstimator.setGyroBias(getGyroBias());
+		_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);
 	}
 
 	// run EKF-GSF yaw estimator once per imu_delayed update

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -134,7 +134,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 	// only calculate gains for states we are using
 	VectorState Kfusion = P * H / gnss_yaw.innovation_variance;
 
-	const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation);
+	const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation);
 	_fault_status.flags.bad_hdg = !is_fused;
 	gnss_yaw.fused = is_fused;
 

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -111,7 +111,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 		const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
 
 		if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
-			fused[index] = measurementUpdate(K, _aid_src_gravity.innovation_variance[index], _aid_src_gravity.innovation[index]);
+			fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]);
 		}
 	}
 

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -45,13 +45,12 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 
 	resetEstimatorAidStatus(aid_src);
 
-	const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse);
+	const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion
 
 	// determine if we should use mag fusion
 	bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
 					     && _control_status.flags.tilt_align
 					     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-					     && (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6))
 					     && mag_sample.mag.longerThan(0.f)
 					     && mag_sample.mag.isAllFinite();
 
@@ -65,8 +64,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 				       && _control_status.flags.mag_aligned_in_flight
 				       && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
 				       && !_control_status.flags.mag_fault
-				       && isRecent(aid_src.time_last_fuse, 500'000)
-				       && getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f))
 				       && !_control_status.flags.ev_yaw
 				       && !_control_status.flags.gps_yaw;
 
@@ -92,7 +89,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 
 		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
 
-			if (mag_sample.reset || checkHaglYawResetReq()) {
+			if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) {
 				ECL_INFO("reset to %s", AID_SRC_NAME);
 				resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
 				aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/mag_control.cpp

@@ -47,15 +47,9 @@ void Ekf::controlMagFusion()
 		_control_status.flags.mag_aligned_in_flight = false;
 	}
 
-	// check mag state observability
 	checkYawAngleObservability();
-	checkMagBiasObservability();
 	checkMagHeadingConsistency();
 
-	if (_mag_bias_observable || _yaw_angle_observable) {
-		_time_last_mov_3d_mag_suitable = _time_delayed_us;
-	}
-
 	if (_params.mag_fusion_type == MagFuseType::NONE) {
 		stopMagFusion();
 		stopMagHdgFusion();
@@ -245,24 +239,6 @@ void Ekf::checkYawAngleObservability()
 	}
 }
 
-void Ekf::checkMagBiasObservability()
-{
-	// check if there is enough yaw rotation to make the mag bias states observable
-	if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
-		// initial yaw motion is detected
-		_mag_bias_observable = true;
-
-	} else if (_mag_bias_observable) {
-		// require sustained yaw motion of 50% the initial yaw rate threshold
-		const float yaw_dt = 1e-6f * (float)(_time_delayed_us - _time_yaw_started);
-		const float min_yaw_change_req =  0.5f * _params.mag_yaw_rate_gate * yaw_dt;
-		_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
-	}
-
-	_yaw_delta_ef = 0.0f;
-	_time_yaw_started = _time_delayed_us;
-}
-
 void Ekf::checkMagHeadingConsistency()
 {
 	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -191,7 +191,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 			Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 		}
 
-		if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
 			fused[index] = true;
 			limitDeclination();
 
@@ -227,35 +227,52 @@ bool Ekf::fuseDeclination(float decl_sigma)
 		return false;
 	}
 
-	// observation variance (rad**2)
-	const float R_DECL = sq(decl_sigma);
+	float decl_measurement = NAN;
 
-	VectorState H;
-	float decl_pred;
-	float innovation_variance;
+	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+	    && PX4_ISFINITE(_mag_declination_gps)
+	   ) {
+		decl_measurement = _mag_declination_gps;
 
-	// TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter?
-	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
-
-	const float innovation = wrap_pi(decl_pred - getMagDeclination());
-
-	if (innovation_variance < R_DECL) {
-		// variance calculation is badly conditioned
-		return false;
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+		  ) {
+		decl_measurement = math::radians(_params.mag_declination_deg);
 	}
 
-	// Calculate the Kalman gains
-	VectorState Kfusion = P * H / innovation_variance;
+	if (PX4_ISFINITE(decl_measurement)) {
 
-	const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
+		// observation variance (rad**2)
+		const float R_DECL = sq(decl_sigma);
 
-	_fault_status.flags.bad_mag_decl = !is_fused;
+		VectorState H;
+		float decl_pred;
+		float innovation_variance;
 
-	if (is_fused) {
-		limitDeclination();
+		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
+
+		const float innovation = wrap_pi(decl_pred - decl_measurement);
+
+		if (innovation_variance < R_DECL) {
+			// variance calculation is badly conditioned
+			return false;
+		}
+
+		// Calculate the Kalman gains
+		VectorState Kfusion = P * H / innovation_variance;
+
+		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
+
+		_fault_status.flags.bad_mag_decl = !is_fused;
+
+		if (is_fused) {
+			limitDeclination();
+		}
+
+		return is_fused;
 	}
 
-	return is_fused;
+	return false;
 }
 
 void Ekf::limitDeclination()
@@ -274,7 +291,9 @@ void Ekf::limitDeclination()
 		// set to 50% of the horizontal strength from geo tables if location is known
 		h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
 
-	} else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) {
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+		  ) {
 		// use parameter value if GPS isn't available
 		decl_reference = math::radians(_params.mag_declination_deg);
 	}

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file vel_pos_fusion.cpp
+ * @file optflow_fusion.cpp
  * Function for fusing gps and baro measurements/
  * equations generated using EKF/python/ekf_derivation/main.py
  *
@@ -146,7 +146,7 @@ void Ekf::fuseOptFlow()
 
 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
 
-		if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
 	}

src/modules/ekf2/EKF/position_fusion.cpp

@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+
+void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
+		const float innov_gate, estimator_aid_source2d_s &aid_src) const
+{
+	resetEstimatorAidStatus(aid_src);
+
+	for (int i = 0; i < 2; i++) {
+		aid_src.observation[i] = obs(i);
+		aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
+
+		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
+		const int state_index = State::pos.idx + i;
+		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
+	}
+
+	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
+
+	aid_src.timestamp_sample = time_us;
+}
+
+void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
+		const float innov_gate, estimator_aid_source1d_s &aid_src) const
+{
+	resetEstimatorAidStatus(aid_src);
+
+	aid_src.observation = obs;
+	aid_src.innovation = _state.pos(2) - aid_src.observation;
+
+	aid_src.observation_variance = math::max(sq(0.01f), obs_var);
+	aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
+
+	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
+
+	aid_src.timestamp_sample = time_us;
+}
+
+void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
+{
+	// x & y
+	if (!aid_src.innovation_rejected
+	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0)
+	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1)
+	   ) {
+		aid_src.fused = true;
+		aid_src.time_last_fuse = _time_delayed_us;
+
+		_time_last_hor_pos_fuse = _time_delayed_us;
+
+	} else {
+		aid_src.fused = false;
+	}
+}
+
+void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
+{
+	// z
+	if (!aid_src.innovation_rejected
+	    && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2)
+	   ) {
+		aid_src.fused = true;
+		aid_src.time_last_fuse = _time_delayed_us;
+
+		_time_last_hgt_fuse = _time_delayed_us;
+
+	} else {
+		aid_src.fused = false;
+	}
+}
+
+void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
+{
+	const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
+	_state.pos.xy() = new_horz_pos;
+
+	if (PX4_ISFINITE(new_horz_pos_var(0))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
+	}
+
+	if (PX4_ISFINITE(new_horz_pos_var(1))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
+	}
+
+	_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
+
+	// record the state change
+	if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
+		_state_reset_status.posNE_change = delta_horz_pos;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.posNE_change += delta_horz_pos;
+	}
+
+	_state_reset_status.reset_count.posNE++;
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
+
+	// Reset the timout timer
+	_time_last_hor_pos_fuse = _time_delayed_us;
+}
+
+void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
+{
+	const float old_vert_pos = _state.pos(2);
+	_state.pos(2) = new_vert_pos;
+
+	if (PX4_ISFINITE(new_vert_pos_var)) {
+		// the state variance is the same as the observation
+		P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
+	}
+
+	const float delta_z = new_vert_pos - old_vert_pos;
+
+	// apply the change in height / height rate to our newest height / height rate estimate
+	// which have already been taken out from the output buffer
+	_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
+
+	// record the state change
+	if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
+		_state_reset_status.posD_change = delta_z;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.posD_change += delta_z;
+	}
+
+	_state_reset_status.reset_count.posD++;
+
+#if defined(CONFIG_EKF2_BAROMETER)
+	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
+#endif // CONFIG_EKF2_BAROMETER
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+#if defined(CONFIG_EKF2_GNSS)
+	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
+#endif // CONFIG_EKF2_GNSS
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	terrainHandleVerticalPositionReset(delta_z);
+#endif
+
+	// Reset the timout timer
+	_time_last_hgt_fuse = _time_delayed_us;
+}
+
+void Ekf::resetHorizontalPositionToLastKnown()
+{
+	ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
+	_information_events.flags.reset_pos_to_last_known = true;
+
+	// Used when falling back to non-aiding mode of operation
+	resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
+}
+
+void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy)
+{
+	ECL_INFO("reset position to external observation");
+	_information_events.flags.reset_pos_to_ext_obs = true;
+
+	resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
+}

src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py

@@ -44,7 +44,7 @@ from pyulog.px4 import PX4ULog
 import numpy as np
 import quaternion
 from scipy import optimize
-from scipy.signal import detrend
+from scipy.signal import sosfilt, butter
 
 def getAllData(logfile):
     log = ULog(logfile)
@@ -59,9 +59,6 @@ def getAllData(logfile):
     baro = getData(log, 'vehicle_air_data', 'baro_alt_meter')
     t_baro = ms2s(getData(log, 'vehicle_air_data', 'timestamp'))
 
-    baro_bias = getData(log, 'estimator_baro_bias', 'bias')
-    t_baro_bias = ms2s(getData(log, 'estimator_baro_bias', 'timestamp'))
-
     q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'),
               getData(log, 'vehicle_attitude', 'q[1]'),
               getData(log, 'vehicle_attitude', 'q[2]'),
@@ -71,18 +68,17 @@ def getAllData(logfile):
     gnss_h = getData(log, 'vehicle_gps_position', 'altitude_msl_m')
     t_gnss = ms2s(getData(log, 'vehicle_gps_position', 'timestamp'))
 
-    (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias)
+    (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h)
 
     t_aligned -= t_aligned[0]
 
-    return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned)
+    return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned)
 
-def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias):
+def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h):
     #TODO: use resample?
     len_q = len(t_q)
     len_l = len(t_local)
     len_g = len(t_gnss)
-    len_bb = len(t_baro_bias)
     i_q = 0
     i_l = 0
     i_g = 0
@@ -91,7 +87,6 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_
     baro_aligned = []
     gnss_h_aligned = []
     v_local_z_aligned = []
-    baro_bias_aligned = []
     t_aligned = []
 
     for i_b in range(len(t_baro)):
@@ -102,8 +97,6 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_
             i_q += 1
         while t_gnss[i_g] < t and i_g < len_g-1:
             i_g += 1
-        while t_baro_bias[i_bb] < t and i_bb < len_bb-1:
-            i_bb += 1
 
         # Only use in air data
         if dist_bottom[i_l] < 1.0:
@@ -118,10 +111,9 @@ def alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_
         baro_aligned = np.append(baro_aligned, baro[i_b])
         v_local_z_aligned = np.append(v_local_z_aligned, v_local[2, i_l])
         gnss_h_aligned = np.append(gnss_h_aligned, gnss_h[i_g])
-        baro_bias_aligned = np.append(baro_bias_aligned, baro_bias[i_bb])
         t_aligned.append(t)
 
-    return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned)
+    return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned)
 
 def getData(log, topic_name, variable_name, instance=0):
     variable_data = np.array([])
@@ -159,15 +151,19 @@ def baroCorrection(x, v_body):
     return correction
 
 def run(logfile):
-    (t, v_body, baro, v_local_z, gnss_h, baro_bias) = getAllData(logfile)
+    (t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile)
 
     # x[0]: pcoef_xn / g
     # x[1]: pcoef_xp / g
     # x[2]: pcoef_yn / g
     # x[3]: pcoef_yp / g
     # x[4]: pcoef_z / g
-    baro -= baro_bias
-    baro_error = detrend(gnss_h - baro)
+    baro_error = (gnss_h - baro)
+
+    # Remove low ferquency part of the signal as we're only interested in the short-term errors
+    baro_error -= baro_error[0]
+    sos = butter(4, 0.01, 'hp', fs=1/(t[1]-t[0]), output='sos')
+    baro_error = sosfilt(sos, baro_error)
 
     J = lambda x: np.sum(np.power(baro_error - baroCorrection(x, v_body), 2.0)) # cost function
 

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -142,7 +142,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation);
+	const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation);
 
 	sideslip.fused = is_fused;
 	_fault_status.flags.bad_sideslip = !is_fused;

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -1,303 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015-2023 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 vel_pos_fusion.cpp
- *
- * @author Roman Bast <bapstroman@gmail.com>
- * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
- * @author Paul Riseborough <p_riseborough@live.com.au>
- *
- */
-
-#include <mathlib/mathlib.h>
-#include "ekf.h"
-
-void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
-				     const float innov_gate, estimator_aid_source2d_s &aid_src) const
-{
-	resetEstimatorAidStatus(aid_src);
-
-	for (int i = 0; i < 2; i++) {
-		aid_src.observation[i] = obs(i);
-		aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
-
-		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
-		const int state_index = State::vel.idx + i;
-		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
-	}
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	aid_src.timestamp_sample = time_us;
-}
-
-void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
-				     const float innov_gate, estimator_aid_source3d_s &aid_src) const
-{
-	resetEstimatorAidStatus(aid_src);
-
-	for (int i = 0; i < 3; i++) {
-		aid_src.observation[i] = obs(i);
-		aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
-
-		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
-		const int state_index = State::vel.idx + i;
-		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
-	}
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
-	// but limit innovation to prevent spikes that could destabilise the filter
-	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
-		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]);
-		aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
-		aid_src.innovation_rejected = false;
-	}
-
-	aid_src.timestamp_sample = time_us;
-}
-
-void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
-		const float innov_gate, estimator_aid_source1d_s &aid_src) const
-{
-	resetEstimatorAidStatus(aid_src);
-
-	aid_src.observation = obs;
-	aid_src.innovation = _state.pos(2) - aid_src.observation;
-
-	aid_src.observation_variance = math::max(sq(0.01f), obs_var);
-	aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	// z special case if there is bad vertical acceleration data, then don't reject measurement,
-	// but limit innovation to prevent spikes that could destabilise the filter
-	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
-		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
-		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
-		aid_src.innovation_rejected = false;
-	}
-
-	aid_src.timestamp_sample = time_us;
-}
-
-void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
-		const float innov_gate, estimator_aid_source2d_s &aid_src) const
-{
-	resetEstimatorAidStatus(aid_src);
-
-	for (int i = 0; i < 2; i++) {
-		aid_src.observation[i] = obs(i);
-		aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
-
-		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
-		const int state_index = State::pos.idx + i;
-		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
-	}
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	aid_src.timestamp_sample = time_us;
-}
-
-void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
-{
-	if (!aid_src.innovation_rejected) {
-		// vx, vy
-		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
-		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
-		   ) {
-			aid_src.fused = true;
-			aid_src.time_last_fuse = _time_delayed_us;
-
-		} else {
-			aid_src.fused = false;
-		}
-	}
-}
-
-void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
-{
-	if (!aid_src.innovation_rejected) {
-		// vx, vy, vz
-		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
-		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
-		    && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
-		   ) {
-			aid_src.fused = true;
-			aid_src.time_last_fuse = _time_delayed_us;
-
-		} else {
-			aid_src.fused = false;
-		}
-	}
-}
-
-void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
-{
-	// x & y
-	if (!aid_src.innovation_rejected) {
-		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
-		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
-		   ) {
-			aid_src.fused = true;
-			aid_src.time_last_fuse = _time_delayed_us;
-
-		} else {
-			aid_src.fused = false;
-		}
-	}
-}
-
-void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
-{
-	// z
-	if (!aid_src.innovation_rejected) {
-		if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
-			aid_src.fused = true;
-			aid_src.time_last_fuse = _time_delayed_us;
-		}
-	}
-}
-
-// Helper function that fuses a single velocity or position measurement
-bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index)
-{
-	VectorState Kfusion;  // Kalman gain vector for any single observation - sequential fusion is used.
-
-	// calculate kalman gain K = PHS, where S = 1/innovation variance
-	for (int row = 0; row < State::size; row++) {
-		Kfusion(row) = P(row, state_index) / innov_var;
-	}
-
-	clearInhibitedStateKalmanGains(Kfusion);
-
-	SquareMatrixState KHP;
-
-	for (unsigned row = 0; row < State::size; row++) {
-		for (unsigned column = 0; column < State::size; column++) {
-			KHP(row, column) = Kfusion(row) * P(state_index, column);
-		}
-	}
-
-	const bool healthy = checkAndFixCovarianceUpdate(KHP);
-
-	setVelPosStatus(state_index, healthy);
-
-	if (healthy) {
-		// apply the covariance corrections
-		P -= KHP;
-
-		constrainStateVariances();
-
-		// apply the state corrections
-		fuse(Kfusion, innov);
-
-		return true;
-	}
-
-	return false;
-}
-
-void Ekf::setVelPosStatus(const int state_index, const bool healthy)
-{
-	switch (state_index) {
-	case State::vel.idx:
-		if (healthy) {
-			_fault_status.flags.bad_vel_N = false;
-			_time_last_hor_vel_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_vel_N = true;
-		}
-
-		break;
-
-	case State::vel.idx + 1:
-		if (healthy) {
-			_fault_status.flags.bad_vel_E = false;
-			_time_last_hor_vel_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_vel_E = true;
-		}
-
-		break;
-
-	case State::vel.idx + 2:
-		if (healthy) {
-			_fault_status.flags.bad_vel_D = false;
-			_time_last_ver_vel_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_vel_D = true;
-		}
-
-		break;
-
-	case State::pos.idx:
-		if (healthy) {
-			_fault_status.flags.bad_pos_N = false;
-			_time_last_hor_pos_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_pos_N = true;
-		}
-
-		break;
-
-	case State::pos.idx + 1:
-		if (healthy) {
-			_fault_status.flags.bad_pos_E = false;
-			_time_last_hor_pos_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_pos_E = true;
-		}
-
-		break;
-
-	case State::pos.idx + 2:
-		if (healthy) {
-			_fault_status.flags.bad_pos_D = false;
-			_time_last_hgt_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_pos_D = true;
-		}
-
-		break;
-	}
-}

src/modules/ekf2/EKF/velocity_fusion.cpp

@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+
+void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
+		const float innov_gate, estimator_aid_source2d_s &aid_src) const
+{
+	resetEstimatorAidStatus(aid_src);
+
+	for (int i = 0; i < 2; i++) {
+		aid_src.observation[i] = obs(i);
+		aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
+
+		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
+		const int state_index = State::vel.idx + i;
+		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
+	}
+
+	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
+
+	aid_src.timestamp_sample = time_us;
+}
+
+void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
+				     const float innov_gate, estimator_aid_source3d_s &aid_src) const
+{
+	resetEstimatorAidStatus(aid_src);
+
+	for (int i = 0; i < 3; i++) {
+		aid_src.observation[i] = obs(i);
+		aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
+
+		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
+		const int state_index = State::vel.idx + i;
+		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
+	}
+
+	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
+
+	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]);
+		aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
+
+	aid_src.timestamp_sample = time_us;
+}
+
+void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
+{
+	// vx, vy
+	if (!aid_src.innovation_rejected
+	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
+	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
+	   ) {
+		aid_src.fused = true;
+		aid_src.time_last_fuse = _time_delayed_us;
+
+		_time_last_hor_vel_fuse = _time_delayed_us;
+
+	} else {
+		aid_src.fused = false;
+	}
+}
+
+void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
+{
+	// vx, vy, vz
+	if (!aid_src.innovation_rejected
+	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
+	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
+	    && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2)
+	   ) {
+		aid_src.fused = true;
+		aid_src.time_last_fuse = _time_delayed_us;
+
+		_time_last_hor_vel_fuse = _time_delayed_us;
+		_time_last_ver_vel_fuse = _time_delayed_us;
+
+	} else {
+		aid_src.fused = false;
+	}
+}
+
+void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
+{
+	const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
+	_state.vel.xy() = new_horz_vel;
+
+	if (PX4_ISFINITE(new_horz_vel_var(0))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
+	}
+
+	if (PX4_ISFINITE(new_horz_vel_var(1))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
+	}
+
+	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
+
+	// record the state change
+	if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
+		_state_reset_status.velNE_change = delta_horz_vel;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.velNE_change += delta_horz_vel;
+	}
+
+	_state_reset_status.reset_count.velNE++;
+
+	// Reset the timout timer
+	_time_last_hor_vel_fuse = _time_delayed_us;
+}
+
+void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
+{
+	const float delta_vert_vel = new_vert_vel - _state.vel(2);
+	_state.vel(2) = new_vert_vel;
+
+	if (PX4_ISFINITE(new_vert_vel_var)) {
+		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
+	}
+
+	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
+
+	// record the state change
+	if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
+		_state_reset_status.velD_change = delta_vert_vel;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.velD_change += delta_vert_vel;
+	}
+
+	_state_reset_status.reset_count.velD++;
+
+	// Reset the timout timer
+	_time_last_ver_vel_fuse = _time_delayed_us;
+}
+
+void Ekf::resetHorizontalVelocityToZero()
+{
+	ECL_INFO("reset velocity to zero");
+	_information_events.flags.reset_vel_to_zero = true;
+
+	// Used when falling back to non-aiding mode of operation
+	resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
+}
+
+void Ekf::resetVerticalVelocityToZero()
+{
+	// we don't know what the vertical velocity is, so set it to zero
+	// Set the variance to a value large enough to allow the state to converge quickly
+	// that does not destabilise the filter
+	resetVerticalVelocityTo(0.0f, 10.f);
+}
+
+void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
+{
+	resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
+	resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
+}

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -113,7 +113,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
-	if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
+	if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) {
 
 		_time_last_heading_fuse = _time_delayed_us;
 

src/modules/ekf2/EKF2.cpp

@@ -135,7 +135,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_decl_type(_params->mag_declination_source),
 	_param_ekf2_mag_type(_params->mag_fusion_type),
 	_param_ekf2_mag_acclim(_params->mag_acc_gate),
-	_param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate),
 	_param_ekf2_mag_check(_params->mag_check),
 	_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
 	_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
@@ -1896,12 +1895,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.fs_bad_sideslip          = _ekf.fault_status_flags().bad_sideslip;
 		status_flags.fs_bad_optflow_x         = _ekf.fault_status_flags().bad_optflow_X;
 		status_flags.fs_bad_optflow_y         = _ekf.fault_status_flags().bad_optflow_Y;
-		status_flags.fs_bad_vel_n             = _ekf.fault_status_flags().bad_vel_N;
-		status_flags.fs_bad_vel_e             = _ekf.fault_status_flags().bad_vel_E;
-		status_flags.fs_bad_vel_d             = _ekf.fault_status_flags().bad_vel_D;
-		status_flags.fs_bad_pos_n             = _ekf.fault_status_flags().bad_pos_N;
-		status_flags.fs_bad_pos_e             = _ekf.fault_status_flags().bad_pos_E;
-		status_flags.fs_bad_pos_d             = _ekf.fault_status_flags().bad_pos_D;
 		status_flags.fs_bad_acc_bias          = _ekf.fault_status_flags().bad_acc_bias;
 		status_flags.fs_bad_acc_vertical      = _ekf.fault_status_flags().bad_acc_vertical;
 		status_flags.fs_bad_acc_clipping      = _ekf.fault_status_flags().bad_acc_clipping;

src/modules/ekf2/EKF2.hpp

@@ -605,7 +605,6 @@ private:
 		(ParamExtInt<px4::params::EKF2_DECL_TYPE>) _param_ekf2_decl_type,
 		(ParamExtInt<px4::params::EKF2_MAG_TYPE>) _param_ekf2_mag_type,
 		(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>) _param_ekf2_mag_acclim,
-		(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>) _param_ekf2_mag_yawlim,
 		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
 		(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
 		(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,

src/modules/ekf2/ekf2_params.c

@@ -503,9 +503,9 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
 PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
 
 /**
- * Horizontal acceleration threshold used by automatic selection of magnetometer fusion method.
+ * Horizontal acceleration threshold used for heading observability check
  *
- * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered horizontal acceleration is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion.
+ * The heading is assumed to be observable when the body acceleration is greater than this parameter when a global position/velocity aiding source is active.
  *
  * @group EKF2
  * @min 0.0
@@ -515,19 +515,6 @@ PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
  */
 PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f);
 
-/**
- * Yaw rate threshold used by automatic selection of magnetometer fusion method.
- *
- * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered yaw rate is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion.
- *
- * @group EKF2
- * @min 0.0
- * @max 1.0
- * @unit rad/s
- * @decimal 2
- */
-PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.20f);
-
 /**
  * Gate size for barometric and GPS height fusion
  *