mavlink: early return in parser

Signed-off-by: Julian Oes <julian@oes.ch>
mavlink: add MAV_SIK_RADIO_ID param back in
2026-05-29 23:30:05 +08:00 · 2023-02-09 21:06:16 +13:00 · 2023-02-09 21:05:44 +13:00 · 2023-02-08 13:15:33 +13:00 · 2023-02-07 13:57:10 -05:00 · 2023-02-07 13:28:58 -05:00
126 changed files with 9042 additions and 6888 deletions
@@ -62,6 +62,8 @@ pipeline {
                     "holybro_durandal-v1_default",
                     "holybro_kakutef7_default",
                     "holybro_kakuteh7_default",
+                     "holybro_kakuteh7v2_default",
+                     "holybro_kakuteh7mini_default",
                     "holybro_pix32v5_default",
                     "matek_gnss-m9n-f4_canbootloader",
                     "matek_gnss-m9n-f4_default",
@@ -180,7 +180,7 @@ CONFIG:
      short: cubepilot_cubeorange
      buildType: MinSizeRel
      settings:
-        CONFIG: cubepilot_orange_test
+        CONFIG: cubepilot_cubeorange_test
    emlid_navio2_default:
      short: emlid_navio2
      buildType: MinSizeRel
@@ -2,7 +2,6 @@
        "astyle.astylerc": "${workspaceFolder}/Tools/astyle/astylerc",
        "astyle.c.enable": true,
        "astyle.cpp.enable": true,
-        "breadcrumbs.enabled": true,
        "C_Cpp.autoAddFileAssociations": false,
        "C_Cpp.clang_format_fallbackStyle": "none",
        "C_Cpp.default.browse.limitSymbolsToIncludedHeaders": true,
@@ -20,7 +19,6 @@
        "cmakeExplorer.buildDir": "${workspaceFolder}/build/px4_sitl_test",
        "cmakeExplorer.parallelJobs": 1,
        "cmakeExplorer.suiteDelimiter": "-",
-        "cortex-debug.enableTelemetry": false,
        "cSpell.allowCompoundWords": true,
        "cSpell.diagnosticLevel": "Hint",
        "cSpell.showStatus": false,
@@ -31,7 +29,6 @@
        ],
        "debug.toolBarLocation": "docked",
        "editor.defaultFormatter": "chiehyu.vscode-astyle",
-        "editor.dragAndDrop": false,
        "editor.insertSpaces": false,
        "editor.minimap.maxColumn": 120,
        "editor.minimap.renderCharacters": false,
@@ -127,12 +124,7 @@
                "${workspaceFolder}/build": true
        },
        "search.showLineNumbers": true,
-        "telemetry.enableTelemetry": false,
-        "terminal.integrated.scrollback": 5000,
-        "window.title": "${dirty} ${activeEditorMedium}${separator}${rootName}",
-        "workbench.editor.highlightModifiedTabs": true,
-        "workbench.enableExperiments": false,
-        "workbench.settings.enableNaturalLanguageSearch": false,
+        "terminal.integrated.scrollback": 15000,
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
        }
@@ -73,6 +73,13 @@ menu "Toolchain"
        help
            relative path to the ROMFS root directory

+    config BOARD_ROOTFSDIR
+        string "Root directory"
+        depends on PLATFORM_POSIX
+        default "."
+        help
+            Configure the root directory in the file system for PX4 files
+
    config BOARD_IO
        string "IO board name"
        default "px4_io-v2_default"
@@ -49,7 +49,7 @@ param set-default CA_AIRFRAME 4

 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_PX 0
-param set-default CA_ROTOR0_PY 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX 0
 param set-default CA_ROTOR1_PY -1
@@ -13,7 +13,7 @@ param set-default CA_AIRFRAME 4

 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 1
-param set-default CA_ROTOR0_PY 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX -1
 param set-default CA_ROTOR1_PY -1
@@ -54,6 +54,9 @@ param set-default RWTO_TKOFF 1

 param set-default CA_AIRFRAME 1

+param set-default COM_PREARM_MODE 2
+param set-default CBRK_AIRSPD_CHK 162128
+
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3

@@ -14,9 +14,14 @@ param set-default MAV_TYPE 1
 # Default parameters for fixed wing UAVs.
 #
 param set-default COM_POS_FS_DELAY 5
-param set-default COM_POS_FS_EPH 15
+
+# there is a 2.5 factor applied on the _FS thresholds if for invalidation
+param set-default COM_POS_FS_EPH 50
 param set-default COM_POS_FS_EPV 30
 param set-default COM_VEL_FS_EVH 5
+
+param set-default COM_POS_LOW_EPH 50
+
 # Disable preflight disarm to not interfere with external launching
 param set-default COM_DISARM_PRFLT -1

@@ -10,6 +10,11 @@ set VEHICLE_TYPE vtol
 # MAV_TYPE_VTOL_FIXEDROTOR 22
 param set-default MAV_TYPE 22

+# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
+param set-default COM_POS_FS_EPH 50
+
+param set-default COM_POS_LOW_EPH 50
+
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_YAW_TMT 10

@@ -217,6 +217,17 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	# Set Java 11 as default
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")

+	# Install Gazebo
+	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+		wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+		# Update list, since new gazebo-stable.list has been added
+		sudo apt-get update -y --quiet
+		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
+			ignition-fortress \
+			;
+	fi
+
 	# Install Gazebo classic
 	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 		gazebo_version=9
@@ -255,16 +266,6 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		echo "export SVGA_VGPU10=0" >> ~/.profile
 	fi

-	# Install Gazebo
-	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-		wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-		# Update list, since new gazebo-stable.list has been added
-		sudo apt-get update -y --quiet
-		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-			ignition-fortress \
-			;
-	fi
 fi

 if [[ $INSTALL_NUTTX == "true" ]]; then
@@ -685,6 +685,7 @@
      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
      <joint_name>servo_0</joint_name>
      <sub_topic>servo_0</sub_topic>
+      <p_gain>10.0</p_gain>
    </plugin>
    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
      <a0>0.05984281113</a0>
@@ -708,6 +709,7 @@
      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
      <joint_name>servo_1</joint_name>
      <sub_topic>servo_1</sub_topic>
+      <p_gain>10.0</p_gain>
    </plugin>
    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
      <a0>0.05984281113</a0>
@@ -767,6 +769,7 @@
      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
      <joint_name>servo_2</joint_name>
      <sub_topic>servo_2</sub_topic>
+      <p_gain>10.0</p_gain>
    </plugin>
    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
      <a0>0.0</a0>
@@ -789,6 +792,8 @@
    <plugin
      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
      <joint_name>rudder_joint</joint_name>
+      <sub_topic>servo_3</sub_topic>
+      <p_gain>10.0</p_gain>
    </plugin>
    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_puller_joint</jointName>
@@ -1,9 +1,9 @@
 CONFIG_PLATFORM_QURT=y
 CONFIG_BOARD_TOOLCHAIN="qurt"
 CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
+CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
-CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -414,7 +414,7 @@ bool ICM42688P::Configure()
 	return success;
 }

-static bool interrupt_debug = true;
+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;
@@ -17,3 +17,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
+CONFIG_BOARD_ROOTFSDIR="/data/px4"
@@ -19,5 +19,3 @@ fi
 muorb start

 qshell icm42688p start -s
-
-qshell modal_io start
@@ -18,6 +18,4 @@ bool fusion_enabled          # true when measurements are being fused
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused

-# TOPICS estimator_aid_src_ev_vel
-# TOPICS estimator_aid_src_gnss_vel
-# TOPICS estimator_aid_src_mag
+# TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag
@@ -31,7 +31,7 @@ bool height_sensor_timeout              #  4 - true when the height sensor has n
 bool stopping_navigation                #  5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
 bool invalid_accel_bias_cov_reset       #  6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
 bool bad_yaw_using_gps_course           #  7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
-bool stopping_mag_use                   #  8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
+bool stopping_mag_use                   #  8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
 bool vision_data_stopped                #  9 - true when the vision system data has stopped for a significant time period
-bool emergency_yaw_reset_mag_stopped    # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
+bool emergency_yaw_reset_mag_stopped    # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
 bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
@@ -28,6 +28,7 @@ float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance compute
 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)
 float32[3] mag_field	# earth magnetic field innovation (Gauss) and innovation variance (Gauss**2)
+float32[3] gravity	# gravity innovation from accelerometerr vector (m/s**2)
 float32[2] drag		# drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2)
 float32 airspeed	# airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
 float32 beta		# synthetic sideslip innovation (rad) and innovation variance (rad**2)
@@ -35,7 +35,7 @@ uint8 CS_EV_HGT = 14		# 14 - true when height data from external vision measurem
 uint8 CS_BETA = 15		# 15 - true when synthetic sideslip measurements are being fused
 uint8 CS_MAG_FIELD = 16		# 16 - true when only the magnetic field states are updated by the magnetometer
 uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed wing vehicle with constrained sideslip
-uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetomer has been declared faulty and is no longer being used
+uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetometer has been declared faulty and is no longer being used
 uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
 uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
 uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
@@ -38,6 +38,7 @@ bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on win
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
 bool cs_fake_pos                # 32 - true when fake position measurements are being fused
 bool cs_fake_hgt                # 33 - true when fake height measurements are being fused
+bool cs_gravity_vector          # 34 - true when gravity vector measurements are being fused

 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
@@ -15,6 +15,7 @@ uint32 mode_req_global_position
 uint32 mode_req_mission
 uint32 mode_req_offboard_signal
 uint32 mode_req_home_position
+uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
 uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
 uint32 mode_req_other             # other requirements, not covered above (for external modes)

@@ -47,6 +48,7 @@ bool mission_failure                  # Mission failure
 bool vtol_fixed_wing_system_failure   # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
 bool wind_limit_exceeded              # Wind limit exceeded
 bool flight_time_limit_exceeded       # Maximum flight time exceeded
+bool local_position_accuracy_low      # Local position estimate has dropped below threshold, but is currently still declared valid

 # Failure detector
 bool fd_critical_failure              # Critical failure (attitude/altitude limit exceeded, or external ATS)
@@ -84,6 +84,7 @@ uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    		# Request to send a single instanc
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530		# Set camera capture mode (photo, video, etc.)
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531		# Set camera zoom
 uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
+uint16 VEHICLE_CMD_SET_AT_PARAM = 550			# Set AT command for SiK radio
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 	# Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 	# Gimbal configuration to set which sysid/compid is in primary and secondary control
 uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000   	# Start image capture sequence.
@@ -40,6 +40,8 @@
 #pragma once

 #include <sys/ioctl.h>
+#include <px4_boardconfig.h>
+

 /****************************************************************************
 * Defines for all platforms.
@@ -95,7 +97,7 @@ __BEGIN_DECLS
 extern long PX4_TICKS_PER_SEC;
 __END_DECLS

-#define PX4_ROOTFSDIR "."
+#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR

 #define PX4_STORAGEDIR PX4_ROOTFSDIR

@@ -470,7 +470,7 @@ if(NOT NUTTX_DIR MATCHES "external")
 		)

 		# JLINK_RTOS_PATH used by launch.json.in
-		set(JLINK_RTOS_PATH ${NUTTX_DIR}/tools/jlink-nuttx)
+		set(JLINK_RTOS_PATH ${NUTTX_DIR}/tools/jlink-nuttx.so)
 	else()
 		set(JLINK_RTOS_PATH "")
 	endif()
@@ -3,6 +3,7 @@
    "configurations": [
        {
            "name": "jlink (@PX4_BOARD@)",
+            "gdbPath": "@CMAKE_GDB@",
            "device": "@DEBUG_DEVICE@",
            "svdFile": "@DEBUG_SVD_FILE_PATH@",
            "rtos": "@JLINK_RTOS_PATH@",
@@ -24,6 +25,7 @@
        },
        {
            "name": "stlink (@PX4_BOARD@)",
+            "gdbPath": "@CMAKE_GDB@",
            "device": "@DEBUG_DEVICE@",
            "svdFile": "@DEBUG_SVD_FILE_PATH@",
            "executable": "${command:cmake.launchTargetPath}",
@@ -42,6 +44,7 @@
        },
        {
            "name": "blackmagic (@PX4_BOARD@)",
+            "gdbPath": "@CMAKE_GDB@",
            "device": "@DEBUG_DEVICE@",
            "svdFile": "@DEBUG_SVD_FILE_PATH@",
            "executable": "${command:cmake.launchTargetPath}",
@@ -308,7 +308,7 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd)

 		instances[i] = new mtd_instance_s;

-		if (instances == nullptr) {
+		if (instances[i] == nullptr) {
 memoryout:
 			PX4_ERR("failed to allocate memory!");
 			return rv;
@@ -444,41 +444,39 @@ __EXPORT int px4_mtd_query(const char *sub, const char *val, const char **get)
 {
 	int rv = -ENODEV;

-	if (instances != nullptr) {
+	static const char *keys[] = PX4_MFT_MTD_STR_TYPES;
+	static const px4_mtd_types_t types[] = PX4_MFT_MTD_TYPES;
+	int key = 0;

-		static const char *keys[] = PX4_MFT_MTD_STR_TYPES;
-		static const px4_mtd_types_t types[] = PX4_MFT_MTD_TYPES;
-		int key = 0;
-
-		for (unsigned int k = 0; k < arraySize(keys); k++) {
-			if (!strcmp(keys[k], sub)) {
-				key = types[k];
-				break;
-			}
+	for (unsigned int k = 0; k < arraySize(keys); k++) {
+		if (!strcmp(keys[k], sub)) {
+			key = types[k];
+			break;
 		}
+	}


-		rv = -EINVAL;
+	rv = -EINVAL;

-		if (key != 0) {
-			rv = -ENOENT;
+	if (key != 0) {
+		rv = -ENOENT;

-			for (int i = 0; i < num_instances; i++) {
-				for (unsigned n = 0; n < instances[i]->n_partitions_current; n++) {
-					if (instances[i]->partition_types[n] == key) {
-						if (get != nullptr && val == nullptr) {
-							*get =  instances[i]->partition_names[n];
-							return 0;
-						}
+		for (int i = 0; i < num_instances; i++) {
+			for (unsigned n = 0; n < instances[i]->n_partitions_current; n++) {
+				if (instances[i]->partition_types[n] == key) {
+					if (get != nullptr && val == nullptr) {
+						*get =  instances[i]->partition_names[n];
+						return 0;
+					}

-						if (val != nullptr && strcmp(instances[i]->partition_names[n], val) == 0) {
-							return 0;
-						}
+					if (val != nullptr && strcmp(instances[i]->partition_names[n], val) == 0) {
+						return 0;
 					}
 				}
 			}
 		}
 	}

+
 	return rv;
 }
@@ -32,6 +32,7 @@
 ############################################################################

 add_subdirectory(../stm32_common/io_pins io_pins)
+add_subdirectory(../stm32_common/spi spi)
 add_subdirectory(../stm32_common/hrt hrt)
 add_subdirectory(../stm32_common/board_critmon board_critmon)
 add_subdirectory(../stm32_common/board_reset board_reset)
@@ -53,7 +53,7 @@

 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
-static LockstepScheduler lockstep_scheduler {};
+static LockstepScheduler lockstep_scheduler {true};
 #endif

 // Intervals in usec
@@ -46,7 +46,7 @@
 class LockstepComponents
 {
 public:
-	LockstepComponents();
+	LockstepComponents(bool no_cleanup_on_destroy = false);
 	~LockstepComponents();

 	/**
@@ -69,6 +69,7 @@ public:
 	void wait_for_components();

 private:
+	const bool _no_cleanup_on_destroy;

 	px4_sem_t _components_sem;

@@ -46,6 +46,7 @@
 class LockstepScheduler
 {
 public:
+	LockstepScheduler(bool no_cleanup_on_destroy = false) : _components(no_cleanup_on_destroy) {}
 	~LockstepScheduler();

 	void set_absolute_time(uint64_t time_us);
@@ -42,14 +42,19 @@
 #include <px4_platform_common/tasks.h>
 #include <limits.h>

-LockstepComponents::LockstepComponents()
+LockstepComponents::LockstepComponents(bool no_cleanup_on_destroy)
+	: _no_cleanup_on_destroy(no_cleanup_on_destroy)
 {
 	px4_sem_init(&_components_sem, 0, 0);
 }

 LockstepComponents::~LockstepComponents()
 {
-	px4_sem_destroy(&_components_sem);
+	// Trying to destroy a condition variable with threads currently blocked on it results in undefined behavior.
+	// Therefore we allow the caller not to cleanup and let the OS take care of that.
+	if (!_no_cleanup_on_destroy) {
+		px4_sem_destroy(&_components_sem);
+	}
 }

 int LockstepComponents::register_component()
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
+ * Copyright (C) 2022-2023 ModalAI, Inc. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -39,7 +39,9 @@

 __BEGIN_DECLS

-//Defining hap_debug
+extern void qurt_log_to_apps(int level, const char *message);
+
+// Defining hap_debug
 void HAP_debug(const char *msg, int level, const char *filename, int line);

 static __inline void qurt_log(int level, const char *file, int line,
@@ -51,6 +53,8 @@ static __inline void qurt_log(int level, const char *file, int line,
 	vsnprintf(buf, sizeof(buf), format, args);
 	va_end(args);
 	HAP_debug(buf, level, file, line);
+
+	qurt_log_to_apps(level, buf);
 }

 __END_DECLS
@@ -37,6 +37,7 @@ set(QURT_LAYER_SRCS
 	tasks.cpp
 	px4_qurt_impl.cpp
 	main.cpp
+	qurt_log.cpp
 	)

 add_library(px4_layer
@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 ModalAI, Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#include <px4_platform_common/log.h>
+#include <uORB/uORBManager.hpp>
+
+// This function will send a debug or error message up to the apps proc
+// so that it can be displayed and logged. Otherwise the messages are only
+// available with the mini-dm tool that requires adb (i.e. USB cable attached)
+extern "C" void qurt_log_to_apps(int level, const char *message)
+{
+	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
+
+	if (ch != nullptr) {
+		if (level >= _PX4_LOG_LEVEL_ERROR) { ch->send_message("slpi_error", strlen(message) + 1, (uint8_t *) message); }
+
+		else { ch->send_message("slpi_debug", strlen(message) + 1, (uint8_t *) message); }
+	}
+}
@@ -80,7 +80,7 @@
 #define DRV_IMU_DEVTYPE_ICM42605 0x29
 #define DRV_IMU_DEVTYPE_ICM42670P 0x2A
 #define DRV_IMU_DEVTYPE_IIM42652 0x2B
-
+#define DRV_IMU_DEVTYPE_IAM20680HP 0x2C
 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32
 #define DRV_ACC_DEVTYPE_MPU6050  0x33
@@ -111,11 +111,11 @@
 #define DRV_DIFF_PRESS_DEVTYPE_SDP32     0x4B
 #define DRV_DIFF_PRESS_DEVTYPE_SDP33     0x4C

-#define DRV_BARO_DEVTYPE_LPS33HW        0x4C
 #define DRV_BARO_DEVTYPE_TCBP001TA      0x4D
 #define DRV_BARO_DEVTYPE_MS5837         0x4E
 #define DRV_BARO_DEVTYPE_SPL06          0x4F

+#define DRV_BARO_DEVTYPE_LPS33HW        0x50
 #define DRV_BARO_DEVTYPE_MPL3115A2	0x51
 #define DRV_ACC_DEVTYPE_FXOS8701C	0x52

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__imu__invensense__iam20680hp
+	MAIN iam20680hp
+	COMPILE_FLAGS
+	SRCS
+		IAM20680HP.cpp
+		IAM20680HP.hpp
+		iam20680hp_main.cpp
+		InvenSense_IAM20680HP_registers.hpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)
@@ -0,0 +1,641 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+
+#include "IAM20680HP.hpp"
+
+using namespace time_literals;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+IAM20680HP::IAM20680HP(const I2CSPIDriverConfig &config) :
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio),
+	_px4_accel(get_device_id(), config.rotation),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (_drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+}
+
+IAM20680HP::~IAM20680HP()
+{
+	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);
+}
+
+int IAM20680HP::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool IAM20680HP::Reset()
+{
+	_state = STATE::RESET;
+	DataReadyInterruptDisable();
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void IAM20680HP::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void IAM20680HP::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);
+}
+
+bool IAM20680HP::StoreCheckedRegisterValue(Register reg)
+{
+	// 3 retries
+	for (int i = 0; i < 3; i++) {
+		uint8_t read1 = RegisterRead(reg);
+		uint8_t read2 = RegisterRead(reg);
+
+		if (read1 == read2) {
+			for (auto &r : _register_cfg) {
+				if (r.reg == reg) {
+					r.set_bits = read1;
+					r.clear_bits = ~read1;
+					return true;
+				}
+			}
+
+		} else {
+			PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast<uint8_t>(reg), read1, read2);
+		}
+	}
+
+	return false;
+}
+
+int IAM20680HP::probe()
+{
+	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+
+	if (whoami != WHOAMI) {
+		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+void IAM20680HP::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// PWR_MGMT_1: Device Reset
+		RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(100_ms);
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+
+		// The reset value is 0x00 for all registers other than the registers below
+		//  Document Number: DS-000114 Page Page 35 of 53
+		if (RegisterRead(Register::WHO_AM_I) == WHOAMI) {
+
+			// offset registers (factory calibration) should not change during normal operation
+			StoreCheckedRegisterValue(Register::XA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::XA_OFFSET_L);
+			StoreCheckedRegisterValue(Register::YA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::YA_OFFSET_L);
+			StoreCheckedRegisterValue(Register::ZA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::ZA_OFFSET_L);
+
+			// Wakeup and reset digital signal path
+			RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
+			RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST);
+			RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS);
+
+			// if reset succeeded then configure
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(35_ms); // max 35 ms start-up time from sleep
+
+		} 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()) {
+			// 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 {
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();
+
+		} else {
+			// 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: {
+			hrt_abstime timestamp_sample = now;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			// always check current FIFO count
+			bool success = false;
+			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) should be a multiple of the FIFO::DATA structure
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+					samples--;
+				}
+
+				if (samples > FIFO_MAX_SAMPLES) {
+					// not technically an overflow, but more samples than we expected or can publish
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, 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;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_cfg[_checked_register])) {
+					_last_config_check_timestamp = now;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+
+			} else {
+				// periodically update temperature (~1 Hz)
+				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+					UpdateTemperature();
+					_temperature_update_timestamp = now;
+				}
+			}
+		}
+
+		break;
+	}
+}
+
+void IAM20680HP::ConfigureAccel()
+{
+	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
+
+	switch (ACCEL_FS_SEL) {
+	case ACCEL_FS_SEL_2G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
+		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_4G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_8G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
+		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_16G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		break;
+	}
+}
+
+void IAM20680HP::ConfigureGyro()
+{
+	const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
+
+	float range_dps = 0.f;
+
+	switch (FS_SEL) {
+	case FS_SEL_250_DPS:
+		range_dps = 250.f;
+		break;
+
+	case FS_SEL_500_DPS:
+		range_dps = 500.f;
+		break;
+
+	case FS_SEL_1000_DPS:
+		range_dps = 1000.f;
+		break;
+
+	case FS_SEL_2000_DPS:
+		range_dps = 2000.f;
+		break;
+	}
+
+	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));	//灵敏度为16.4 LSB/dps
+	_px4_gyro.set_range(math::radians(range_dps));	//陀螺仪量程
+}
+
+void IAM20680HP::ConfigureSampleRate(int sample_rate)
+{
+	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	_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);
+}
+
+bool IAM20680HP::Configure()
+{
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_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_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	ConfigureAccel();
+	ConfigureGyro();
+
+	return success;
+}
+
+int IAM20680HP::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<IAM20680HP *>(arg)->DataReady();
+	return 0;
+}
+
+void IAM20680HP::DataReady()
+{
+	// at least the required number of samples in the FIFO
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
+		ScheduleNow();
+	}
+}
+
+bool IAM20680HP::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 IAM20680HP::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+bool IAM20680HP::RegisterCheck(const register_config_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;
+}
+
+uint8_t IAM20680HP::RegisterRead(Register reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+void IAM20680HP::RegisterWrite(Register reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+void IAM20680HP::RegisterSetAndClearBits(Register 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 IAM20680HP::FIFOReadCount()
+{
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
+
+	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]);
+}
+
+bool IAM20680HP::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+
+	ProcessGyro(timestamp_sample, buffer.f, samples);
+	return ProcessAccel(timestamp_sample, buffer.f, samples);
+}
+
+void IAM20680HP::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// FIFO_EN: disable FIFO
+	RegisterWrite(Register::FIFO_EN, 0);
+
+	// USER_CTRL: reset FIFO
+	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
+
+	// reset while FIFO is disabled
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
+
+	// FIFO_EN: enable both gyro and accel
+	// USER_CTRL: re-enable FIFO
+	for (const auto &r : _register_cfg) {
+		if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
+			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+		}
+	}
+}
+
+static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+{
+	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
+}
+
+bool IAM20680HP::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 * SAMPLES_PER_TRANSFER;
+
+	bool bad_data = false;
+
+	// accel data is doubled in FIFO, but might be shifted
+	int accel_first_sample = 1;
+
+	if (samples >= 4) {
+		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
+			// [A0, A1, A2, A3]
+			//  A0==A1, A2==A3
+			accel_first_sample = 1;
+
+		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
+			// [A0, A1, A2, A3]
+			//  A0, A1==A2, A3
+			accel_first_sample = 0;
+
+		} else {
+			// no matching accel samples is an error
+			bad_data = true;
+			perf_count(_bad_transfer_perf);
+		}
+	}
+
+	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
+		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
+		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
+		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
+
+		// 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[accel.samples] = accel_x;
+		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+		accel.samples++;
+	}
+
+	_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) {
+		_px4_accel.updateFIFO(accel);
+	}
+
+	return !bad_data;
+}
+
+void IAM20680HP::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 = samples;
+	gyro.dt = FIFO_SAMPLE_DT;
+
+	for (int i = 0; i < samples; i++) {
+		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
+		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
+		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
+
+		// 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;
+		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+	}
+
+	_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));
+
+	_px4_gyro.updateFIFO(gyro);
+}
+
+void IAM20680HP::UpdateTemperature()
+{
+	// read current temperature
+	uint8_t temperature_buf[3] {};
+	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
+
+	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
+
+	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+	if (PX4_ISFINITE(TEMP_degC)) {
+		_px4_accel.set_temperature(TEMP_degC);
+		_px4_gyro.set_temperature(TEMP_degC);
+	}
+}
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+
+
+#pragma once
+
+#include "InvenSense_IAM20680HP_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 <lib/drivers/device/Device.hpp>
+
+using namespace InvenSense_IAM20680HP;
+
+class IAM20680HP : public device::SPI, public I2CSPIDriver<IAM20680HP>
+{
+public:
+	IAM20680HP(const I2CSPIDriverConfig &config);
+	~IAM20680HP() override;
+
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+private:
+	void exit_and_cleanup() override;
+
+	// Sensor Configuration
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                     // ensure at least 1 new accel sample per transfer
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};              // 8000 Hz gyro
+	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER};  // 4000 Hz accel
+
+	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
+	static constexpr int32_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))};
+
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_config_t {
+		Register reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Reset();
+
+	bool Configure();
+	void ConfigureAccel();
+	void ConfigureGyro();
+	void ConfigureSampleRate(int sample_rate);
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	bool RegisterCheck(const register_config_t &reg_cfg);
+	bool StoreCheckedRegisterValue(Register reg);
+
+	uint8_t RegisterRead(Register reg);
+	void RegisterWrite(Register reg, uint8_t value);
+	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+	void FIFOReset();
+
+	bool 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);
+	void UpdateTemperature();
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	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};
+
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{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
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+
+	uint8_t _checked_register{0};
+	static constexpr uint8_t size_register_cfg{16};
+	register_config_t _register_cfg[size_register_cfg] {
+		// Register                | Set bits, Clear bits
+		{ Register::CONFIG,         CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
+		{ Register::SMPLRT_DIV,     SMPLRT_DIV_BIT::SPEED, 0 },//输出速率100hz
+		{ Register::GYRO_CONFIG,    GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },//陀螺仪±2000dps量程
+		{ Register::ACCEL_CONFIG,   ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },//加速度计±16G量程
+		{ Register::ACCEL_CONFIG2,  ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B, ACCEL_CONFIG2_BIT::FIFO_SIZE },//加速度计低通滤波BW=21.2Hz
+		{ Register::FIFO_EN,        FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN },
+		{ Register::INT_PIN_CFG,    INT_PIN_CFG_BIT::INT_LEVEL, 0 },
+		{ Register::INT_ENABLE,     INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 },
+		{ Register::USER_CTRL,      USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
+		{ Register::PWR_MGMT_1,     PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
+		{ Register::XA_OFFSET_H,    0, 0 },
+		{ Register::XA_OFFSET_L,    0, 0 },
+		{ Register::YA_OFFSET_H,    0, 0 },
+		{ Register::YA_OFFSET_L,    0, 0 },
+		{ Register::ZA_OFFSET_H,    0, 0 },
+		{ Register::ZA_OFFSET_L,    0, 0 },
+	};
+};
@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+
+#pragma once
+
+#include <cstdint>
+
+// 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);
+
+namespace InvenSense_IAM20680HP
+{
+static constexpr uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8MHz SPI serial interface
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0xF8; //whoami默认值
+
+static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C 灵敏度
+static constexpr float TEMPERATURE_OFFSET = 25.f; // C 温度偏移
+//寄存器地址
+enum class Register : uint8_t {
+	SMPLRT_DIV        = 0X19,
+	CONFIG            = 0x1A,
+	GYRO_CONFIG       = 0x1B,
+	ACCEL_CONFIG      = 0x1C,
+	ACCEL_CONFIG2     = 0x1D,
+
+	FIFO_EN           = 0x23,
+
+	INT_PIN_CFG       = 0x37,
+	INT_ENABLE        = 0x38,
+
+	TEMP_OUT_H        = 0x41,
+	TEMP_OUT_L        = 0x42,
+
+	SIGNAL_PATH_RESET = 0x68,
+
+	USER_CTRL         = 0x6A,
+	PWR_MGMT_1        = 0x6B,
+
+	FIFO_COUNTH       = 0x72,
+	FIFO_COUNTL       = 0x73,
+	FIFO_R_W          = 0x74,
+	WHO_AM_I          = 0x75,
+
+	XA_OFFSET_H       = 0x77,
+	XA_OFFSET_L       = 0x78,
+
+	YA_OFFSET_H       = 0x7A,
+	YA_OFFSET_L       = 0x7B,
+
+	ZA_OFFSET_H       = 0x7D,
+	ZA_OFFSET_L       = 0x7E,
+};
+
+// CONFIG
+enum CONFIG_BIT : uint8_t {
+	FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO
+
+	DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0]
+};
+
+// GYRO_CONFIG
+enum GYRO_CONFIG_BIT : uint8_t {
+	// FS_SEL [4:3]
+	FS_SEL_250_DPS	= 0,           // 0b00000
+	FS_SEL_500_DPS	= Bit3,        // 0b01000
+	FS_SEL_1000_DPS	= Bit4,        // 0b10000
+	FS_SEL_2000_DPS	= Bit4 | Bit3, // 0b11000
+
+	// FCHOICE_B [1:0]
+	FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0   8 kHz
+};
+
+// ACCEL_CONFIG
+enum ACCEL_CONFIG_BIT : uint8_t {
+	// ACCEL_FS_SEL [4:3]
+	ACCEL_FS_SEL_2G  = 0,           // 0b00000
+	ACCEL_FS_SEL_4G  = Bit3,        // 0b01000
+	ACCEL_FS_SEL_8G  = Bit4,        // 0b10000
+	ACCEL_FS_SEL_16G = Bit4 | Bit3, // 0b11000
+};
+
+// ACCEL_CONFIG2
+enum ACCEL_CONFIG2_BIT : uint8_t {
+	FIFO_SIZE       = Bit7 | Bit6, // 0=512bytes,
+	ACCEL_FCHOICE_B = Bit3,        // Used to bypass DLPF (DS-000114 Page 40 of 53)
+};
+
+// FIFO_EN
+enum FIFO_EN_BIT : uint8_t {
+	TEMP_FIFO_EN  = Bit7,
+	XG_FIFO_EN    = Bit6,
+	YG_FIFO_EN    = Bit5,
+	ZG_FIFO_EN    = Bit4,
+	ACCEL_FIFO_EN = Bit3,
+};
+
+// INT_PIN_CFG
+enum INT_PIN_CFG_BIT : uint8_t {
+	INT_LEVEL = Bit7,
+};
+
+// INT_ENABLE
+enum INT_ENABLE_BIT : uint8_t {
+	DATA_RDY_INT_EN = Bit0,
+};
+
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	ACCEL_RST = Bit1,
+	TEMP_RST  = Bit0,
+};
+
+// USER_CTRL
+enum USER_CTRL_BIT : uint8_t {
+	FIFO_EN      = Bit6,
+	I2C_IF_DIS   = Bit4,
+	FIFO_RST     = Bit2,
+	SIG_COND_RST = Bit0,
+};
+
+// PWR_MGMT_1
+enum PWR_MGMT_1_BIT : uint8_t {
+	DEVICE_RESET = Bit7,
+	SLEEP        = Bit6,
+
+	// CLKSEL[2:0]
+	CLKSEL_0     = Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
+};
+enum SMPLRT_DIV_BIT : uint8_t {
+	SPEED     = Bit3 | Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
+};
+namespace FIFO
+{
+static constexpr size_t SIZE = 512; // max is 4 KB, but limited in software to 512 bytes via ACCEL_CONFIG2
+
+// FIFO_DATA layout when FIFO_EN has both {X, Y, Z}G_FIFO_EN and ACCEL_FIFO_EN set
+struct DATA {
+	uint8_t ACCEL_XOUT_H;
+	uint8_t ACCEL_XOUT_L;
+	uint8_t ACCEL_YOUT_H;
+	uint8_t ACCEL_YOUT_L;
+	uint8_t ACCEL_ZOUT_H;
+	uint8_t ACCEL_ZOUT_L;
+	uint8_t GYRO_XOUT_H;
+	uint8_t GYRO_XOUT_L;
+	uint8_t GYRO_YOUT_H;
+	uint8_t GYRO_YOUT_L;
+	uint8_t GYRO_ZOUT_H;
+	uint8_t GYRO_ZOUT_L;
+};
+}
+
+} // namespace InvenSense_IAM20680HP
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_INVENSENSE_IAM20680HP
+	bool "iam20680hp"
+	default n
+	---help---
+		Enable support for iam20680hp
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+
+#include "IAM20680HP.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+void IAM20680HP::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("iam20680hp", "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();
+}
+
+extern "C" int iam20680hp_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = IAM20680HP;
+	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_IAM20680HP);
+
+	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;
+}
@@ -87,7 +87,8 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset;
+		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
+		return _offset + (_rotation.I() * bias).edivide(_scale);
 	}

 	bool ParametersLoad();
@@ -91,7 +91,8 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		return (_rotation.I() * bias) + _thermal_offset + _offset;
+		// updated calibration offset = existing offset + bias rotated to sensor frame
+		return _offset + (_rotation.I() * bias);
 	}

 	bool ParametersLoad();
@@ -89,7 +89,8 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		return _scale.I() * _rotation.I() * bias + _offset;
+		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
+		return _offset + (_scale.I() * _rotation.I() * bias);
 	}

 	bool ParametersLoad();
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -91,13 +91,13 @@ WindEstimator::update(uint64_t time_now)
 		return;
 	}

-	float dt = (float)(time_now - _time_last_update) * 1e-6f;
+	const float dt = (float)(time_now - _time_last_update) * 1e-6f;
 	_time_last_update = time_now;

 	matrix::Matrix3f Qk;
-	Qk(0, 0) = _wind_psd * dt;
-	Qk(1, 1) = Qk(0, 0);
-	Qk(2, 2) = _tas_scale_psd * dt;
+	Qk(INDEX_W_N, INDEX_W_N) = _wind_psd * dt;
+	Qk(INDEX_W_E, INDEX_W_E) = Qk(INDEX_W_N, INDEX_W_N);
+	Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * dt;
 	_P += Qk;
 }

@@ -127,7 +127,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const

 	const bool meas_is_rejected = check_if_meas_is_rejected(_tas_innov, _tas_innov_var, _tas_gate);

-	if (_tas_innov_var < 0.0f) {
+	if (_tas_innov_var < FLT_EPSILON) {
 		// re init filter in case of a negative variance, and trigger early return to not fuse measurement
 		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
 		return;
@@ -137,9 +137,9 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	}

 	// apply correction to state
-	_state(INDEX_W_N) += _tas_innov * K(0, 0);
-	_state(INDEX_W_E) += _tas_innov * K(1, 0);
-	_state(INDEX_TAS_SCALE) += _tas_innov * K(2, 0);
+	_state(INDEX_W_N) += _tas_innov * K(INDEX_W_N, 0);
+	_state(INDEX_W_E) += _tas_innov * K(INDEX_W_E, 0);
+	_state(INDEX_TAS_SCALE) += _tas_innov * K(INDEX_TAS_SCALE, 0);

 	// update covariance matrix
 	_P = _P - K * H_tas * _P;
@@ -172,7 +172,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const

 	const bool meas_is_rejected = check_if_meas_is_rejected(_beta_innov, _beta_innov_var, _beta_gate);

-	if (_beta_innov_var < 0.0f) {
+	if (_beta_innov_var < FLT_EPSILON) {
 		// re init filter in case of a negative variance, and trigger early return to not fuse measurement
 		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
 		return;
@@ -182,9 +182,9 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 	}

 	// apply correction to state
-	_state(INDEX_W_N) += _beta_innov * K(0, 0);
-	_state(INDEX_W_E) += _beta_innov * K(1, 0);
-	_state(INDEX_TAS_SCALE) += _beta_innov * K(2, 0);
+	_state(INDEX_W_N) += _beta_innov * K(INDEX_W_N, 0);
+	_state(INDEX_W_E) += _beta_innov * K(INDEX_W_E, 0);
+	_state(INDEX_TAS_SCALE) += _beta_innov * K(INDEX_TAS_SCALE, 0);

 	// update covariance matrix
 	_P = _P - K * H_beta * _P;
@@ -211,7 +211,7 @@ WindEstimator::run_sanity_checks()
 		}
 	}

-	if (!PX4_ISFINITE(_state(INDEX_W_N)) || !PX4_ISFINITE(_state(INDEX_W_E)) || !PX4_ISFINITE(_state(INDEX_TAS_SCALE))) {
+	if (!_state.isAllFinite()) {
 		_initialised = false;
 		return;
 	}
@@ -219,7 +219,7 @@ WindEstimator::run_sanity_checks()
 	// attain symmetry
 	for (unsigned row = 0; row < 3; row++) {
 		for (unsigned column = 0; column < row; column++) {
-			float tmp = (_P(row, column) + _P(column, row)) / 2;
+			const float tmp = (_P(row, column) + _P(column, row)) * 0.5f;
 			_P(row, column) = tmp;
 			_P(column, row) = tmp;
 		}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-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
@@ -76,12 +76,12 @@ public:

 	// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
 	float get_tas_scale() { return 1.f / math::constrain(_state(INDEX_TAS_SCALE), 0.1f, 10.0f); }
-	float get_tas_scale_var() { return _P(2, 2); }
+	float get_tas_scale_var() { return _P(INDEX_TAS_SCALE, INDEX_TAS_SCALE); }
 	float get_tas_innov() { return _tas_innov; }
 	float get_tas_innov_var() { return _tas_innov_var; }
 	float get_beta_innov() { return _beta_innov; }
 	float get_beta_innov_var() { return _beta_innov_var; }
-	matrix::Vector2f  get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; }
+	matrix::Vector2f  get_wind_var() { return matrix::Vector2f{_P(INDEX_W_N, INDEX_W_N), _P(INDEX_W_E, INDEX_W_E)}; }
 	bool get_wind_estimator_reset() { return _wind_estimator_reset; }

 	// unaided, the state uncertainty (diagonal of sqrt(P)) grows by the process noise spectral density every second
@@ -38,6 +38,8 @@ from symforce import symbolic as sm
 from symforce import geo
 from symforce import typing as T

+import re
+
 # q: quaternion describing rotation from frame 1 to frame 2
 # returns a rotation matrix derived form q which describes the same
 # rotation
@@ -87,6 +89,10 @@ def generate_px4_function(function_name, output_names):
            line = line.replace("std::min", "math::min")
            line = line.replace("Eigen", "matrix")
            line = line.replace("matrix/Dense", "matrix/math.hpp")
+
+            # don't allow underscore + uppercase identifier naming (always reserved for any use)
+            line = re.sub(r'_([A-Z])', lambda x: '_' + x.group(1).lower(), line)
+
            print(line, end='')

 def generate_python_function(function_name, output_names):
@@ -58,19 +58,19 @@ void FuseAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local,

  // Output terms (4)
  if (H != nullptr) {
-    matrix::Matrix<Scalar, 1, 3>& _H = (*H);
+    matrix::Matrix<Scalar, 1, 3>& _h = (*H);

-    _H(0, 0) = -_tmp4;
-    _H(0, 1) = -_tmp5;
-    _H(0, 2) = _tmp2;
+    _h(0, 0) = -_tmp4;
+    _h(0, 1) = -_tmp5;
+    _h(0, 2) = _tmp2;
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 3, 1>& _K = (*K);
+    matrix::Matrix<Scalar, 3, 1>& _k = (*K);

-    _K(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6);
-    _K(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7);
-    _K(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8);
+    _k(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6);
+    _k(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7);
+    _k(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8);
  }

  if (innov_var != nullptr) {
@@ -43,7 +43,7 @@ def fuse_airspeed(v_local, state, P, airspeed, R, epsilon):
    # Intermediate terms (11)
    _tmp0 = -state[0] + v_local[0]
    _tmp1 = -state[1] + v_local[1]
-    _tmp2 = math.sqrt(_tmp0**2 + _tmp1**2 + epsilon + v_local[2] ** 2)
+    _tmp2 = math.sqrt(_tmp0 ** 2 + _tmp1 ** 2 + epsilon + v_local[2] ** 2)
    _tmp3 = state[2] / _tmp2
    _tmp4 = _tmp0 * _tmp3
    _tmp5 = _tmp1 * _tmp3
@@ -70,19 +70,19 @@ void FuseBeta(const matrix::Matrix<Scalar, 3, 1>& v_local, const matrix::Matrix<

  // Output terms (4)
  if (H != nullptr) {
-    matrix::Matrix<Scalar, 1, 3>& _H = (*H);
+    matrix::Matrix<Scalar, 1, 3>& _h = (*H);

-    _H(0, 0) = _tmp17;
-    _H(0, 1) = _tmp18;
-    _H(0, 2) = 0;
+    _h(0, 0) = _tmp17;
+    _h(0, 1) = _tmp18;
+    _h(0, 2) = 0;
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 3, 1>& _K = (*K);
+    matrix::Matrix<Scalar, 3, 1>& _k = (*K);

-    _K(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19);
-    _K(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20);
-    _K(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18);
+    _k(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19);
+    _k(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20);
+    _k(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18);
  }

  if (innov_var != nullptr) {
@@ -56,12 +56,12 @@ void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Sc
  }

  if (P != nullptr) {
-    matrix::Matrix<Scalar, 2, 2>& _P = (*P);
+    matrix::Matrix<Scalar, 2, 2>& _p = (*P);

-    _P(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var;
-    _P(1, 0) = _tmp6;
-    _P(0, 1) = _tmp6;
-    _P(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var;
+    _p(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var;
+    _p(1, 0) = _tmp6;
+    _p(0, 1) = _tmp6;
+    _p(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var;
  }
 }  // NOLINT(readability/fn_size)

@@ -47,80 +47,80 @@ static constexpr int LON_DIM = 37;
 // Magnetic declination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.937,
+// Date: 2023.074,
 static constexpr const int16_t declination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ {  25977, 24232, 22486, 20741, 18996, 17250, 15505, 13760, 12014, 10269,  8524,  6778,  5033,  3288,  1543,  -203, -1948, -3693, -5438, -7184, -8929,-10674,-12420,-14165,-15910,-17656,-19401,-21146,-22892,-24637,-26383,-28128,-29873, 31213, 29468, 27722, 25977, },
-	/* LAT: -80 */ {  22542, 20412, 18473, 16699, 15059, 13521, 12057, 10646,  9271,  7921,  6590,  5271,  3962,  2657,  1350,    30, -1311, -2684, -4094, -5547, -7042, -8579,-10157,-11777,-13442,-15160,-16946,-18819,-20802,-22921,-25196,-27630,-30199, 29994, 27377, 24872, 22542, },
-	/* LAT: -70 */ {  14979, 13580, 12453, 11491, 10621,  9789,  8947,  8059,  7106,  6087,  5017,  3924,  2839,  1785,   763,  -247, -1284, -2391, -3593, -4894, -6276, -7707, -9156,-10599,-12025,-13442,-14871,-16358,-17983,-19899,-22424,-26229, 30683, 24132, 19622, 16850, 14979, },
-	/* LAT: -60 */ {   8427,  8180,  7898,  7622,  7368,  7113,  6804,  6371,  5755,  4935,  3934,  2822,  1703,   677,  -203,  -959, -1688, -2511, -3517, -4718, -6051, -7423, -8745, -9956,-11020,-11919,-12634,-13128,-13300,-12851,-10748, -3476,  4938,  7673,  8439,  8563,  8427, },
-	/* LAT: -50 */ {   5490,  5526,  5469,  5379,  5304,  5266,  5231,  5103,  4759,  4095,  3084,  1811,   471,  -707, -1574, -2134, -2528, -2969, -3659, -4679, -5928, -7208, -8350, -9251, -9847,-10084, -9887, -9126, -7615, -5251, -2345,   403,  2513,  3938,  4812,  5286,  5490, },
-	/* LAT: -40 */ {   3958,  4051,  4058,  4012,  3951,  3917,  3921,  3910,  3736,  3200,  2175,   732,  -831, -2142, -3000, -3446, -3625, -3683, -3855, -4435, -5421, -6502, -7387, -7923, -8025, -7643, -6748, -5362, -3652, -1953,  -495,   726,  1770,  2637,  3295,  3726,  3958, },
-	/* LAT: -30 */ {   2986,  3072,  3102,  3086,  3027,  2948,  2887,  2853,  2726,  2247,  1209,  -313, -1920, -3169, -3904, -4244, -4318, -4103, -3668, -3456, -3834, -4588, -5284, -5629, -5498, -4906, -3943, -2738, -1534,  -593,    82,   681,  1306,  1911,  2423,  2786,  2986, },
-	/* LAT: -20 */ {   2344,  2389,  2406,  2407,  2363,  2268,  2160,  2083,  1938,  1441,   381, -1110, -2588, -3645, -4171, -4282, -4074, -3510, -2647, -1856, -1591, -1954, -2605, -3070, -3101, -2738, -2101, -1286,  -514,   -30,   230,   531,   973,  1454,  1880,  2189,  2344, },
-	/* LAT: -10 */ {   1950,  1944,  1921,  1917,  1888,  1802,  1691,  1599,  1415,   863,  -202, -1581, -2852, -3676, -3936, -3702, -3123, -2340, -1495,  -743,  -284,  -323,  -797, -1302, -1511, -1419, -1104,  -607,  -113,   124,   170,   328,   701,  1142,  1539,  1829,  1950, },
-	/* LAT:   0 */ {   1737,  1703,  1646,  1636,  1624,  1554,  1447,  1332,  1078,   457,  -585, -1812, -2863, -3444, -3437, -2932, -2163, -1382,  -731,  -197,   214,   322,    40,  -377,  -630,  -685,  -587,  -326,   -39,    47,   -17,    70,   413,   859,  1283,  1606,  1737, },
-	/* LAT:  10 */ {   1600,  1608,  1564,  1580,  1604,  1554,  1431,  1243,   867,   150,  -871, -1946, -2770, -3104, -2895, -2286, -1510,  -799,  -286,    94,   418,   565,   401,    78,  -155,  -263,  -288,  -209,  -107,  -151,  -294,  -269,    35,   493,   978,  1388,  1600, },
-	/* LAT:  20 */ {   1414,  1562,  1622,  1714,  1800,  1778,  1622,  1319,   775,   -81, -1122, -2071, -2665, -2771, -2440, -1837, -1126,  -478,   -19,   290,   548,   693,   600,   357,   158,    39,   -55,  -122,  -205,  -397,  -636,  -695,  -459,    -9,   535,  1055,  1414, },
-	/* LAT:  30 */ {   1109,  1475,  1735,  1959,  2119,  2129,  1941,  1518,   791,  -236, -1351, -2225, -2636, -2572, -2172, -1593,  -937,  -320,   141,   446,   677,   822,   801,   651,   502,   377,   217,     2,  -284,  -660, -1030, -1189, -1028,  -600,   -22,   588,  1109, },
-	/* LAT:  40 */ {    747,  1333,  1828,  2221,  2471,  2515,  2300,  1767,   857,  -367, -1601, -2464, -2783, -2630, -2184, -1592,  -938,  -309,   200,   564,   834,  1031,  1124,  1112,  1036,   887,   615,   199,  -343,  -951, -1473, -1715, -1598, -1180,  -582,    89,   747, },
-	/* LAT:  50 */ {    453,  1199,  1880,  2439,  2808,  2916,  2688,  2036,   898,  -593, -2010, -2922, -3215, -3024, -2534, -1888, -1178,  -481,   135,   640,  1058,  1412,  1691,  1860,  1878,  1689,  1239,   525,  -373, -1277, -1954, -2238, -2112, -1671, -1038,  -309,   453, },
-	/* LAT:  60 */ {    250,  1101,  1907,  2605,  3112,  3328,  3119,  2314,   808, -1141, -2851, -3829, -4083, -3824, -3253, -2509, -1683,  -843,   -35,   716,  1407,  2037,  2579,  2973,  3130,  2940,  2294,  1170,  -249, -1569, -2434, -2740, -2574, -2082, -1393,  -596,   250, },
-	/* LAT:  70 */ {      2,   935,  1832,  2633,  3251,  3550,  3307,  2192,   -14, -2700, -4648, -5480, -5497, -5019, -4253, -3321, -2299, -1234,  -160,   899,  1924,  2890,  3757,  4463,  4908,  4935,  4318,  2832,   628, -1482, -2777, -3218, -3060, -2531, -1786,  -921,     2, },
-	/* LAT:  80 */ {   -747,   173,  1025,  1716,  2104,  1936,   785, -1722, -4840, -6956, -7735, -7633, -7024, -6120, -5038, -3846, -2584, -1282,    41,  1367,  2681,  3963,  5190,  6330,  7327,  8089,  8433,  7983,  6027,  2185, -1455, -3170, -3527, -3194, -2517, -1668,  -747, },
-	/* LAT:  90 */ { -29691,-27945,-26200,-24454,-22709,-20964,-19218,-17473,-15728,-13983,-12237,-10492, -8747, -7002, -5257, -3512, -1766,   -21,  1724,  3469,  5214,  6960,  8705, 10450, 12196, 13941, 15686, 17432, 19177, 20923, 22668, 24414, 26159, 27905, 29650, 31396,-29691, },
+	/* LAT: -90 */ {  25974, 24228, 22483, 20738, 18992, 17247, 15502, 13756, 12011, 10266,  8520,  6775,  5030,  3285,  1539,  -206, -1951, -3697, -5442, -7187, -8932,-10678,-12423,-14168,-15914,-17659,-19404,-21150,-22895,-24641,-26386,-28131,-29877, 31210, 29464, 27719, 25974, },
+	/* LAT: -80 */ {  22539, 20409, 18470, 16697, 15057, 13519, 12055, 10644,  9269,  7919,  6588,  5269,  3960,  2655,  1348,    28, -1314, -2686, -4097, -5550, -7045, -8583,-10161,-11781,-13446,-15165,-16951,-18824,-20807,-22927,-25201,-27636,-30204, 29989, 27372, 24868, 22539, },
+	/* LAT: -70 */ {  14980, 13581, 12454, 11491, 10621,  9788,  8946,  8058,  7105,  6086,  5016,  3923,  2838,  1784,   762,  -247, -1285, -2392, -3595, -4897, -6279, -7711, -9160,-10603,-12030,-13447,-14876,-16363,-17989,-19907,-22434,-26242, 30671, 24126, 19621, 16851, 14980, },
+	/* LAT: -60 */ {   8431,  8183,  7900,  7624,  7369,  7114,  6804,  6370,  5754,  4934,  3932,  2821,  1702,   676,  -203,  -958, -1687, -2511, -3518, -4720, -6054, -7427, -8749, -9960,-11024,-11923,-12637,-13132,-13304,-12855,-10750, -3465,  4953,  7682,  8446,  8568,  8431, },
+	/* LAT: -50 */ {   5493,  5528,  5471,  5380,  5305,  5267,  5231,  5103,  4758,  4093,  3081,  1808,   468,  -708, -1573, -2132, -2525, -2967, -3658, -4680, -5931, -7213, -8355, -9255, -9851,-10086, -9887, -9126, -7614, -5249, -2342,   406,  2516,  3941,  4815,  5289,  5493, },
+	/* LAT: -40 */ {   3960,  4053,  4059,  4013,  3952,  3917,  3921,  3909,  3735,  3198,  2172,   728,  -835, -2144, -3000, -3444, -3621, -3678, -3852, -4435, -5425, -6507, -7392, -7926, -8027, -7642, -6746, -5359, -3650, -1951,  -494,   727,  1771,  2639,  3297,  3728,  3960, },
+	/* LAT: -30 */ {   2988,  3074,  3103,  3087,  3027,  2948,  2887,  2852,  2725,  2245,  1205,  -318, -1925, -3172, -3904, -4243, -4315, -4097, -3662, -3454, -3836, -4592, -5288, -5631, -5498, -4904, -3940, -2735, -1532,  -593,    81,   681,  1306,  1912,  2424,  2788,  2988, },
+	/* LAT: -20 */ {   2346,  2391,  2407,  2407,  2363,  2267,  2158,  2082,  1936,  1438,   377, -1116, -2593, -3648, -4172, -4280, -4070, -3504, -2640, -1851, -1591, -1957, -2607, -3071, -3101, -2736, -2098, -1284,  -514,   -30,   229,   529,   972,  1454,  1880,  2191,  2346, },
+	/* LAT: -10 */ {   1952,  1946,  1923,  1917,  1888,  1801,  1689,  1597,  1412,   860,  -206, -1586, -2856, -3678, -3935, -3699, -3119, -2335, -1489,  -739,  -282,  -324,  -798, -1303, -1511, -1418, -1103,  -605,  -113,   123,   168,   326,   699,  1142,  1540,  1831,  1952, },
+	/* LAT:   0 */ {   1739,  1705,  1647,  1636,  1624,  1553,  1445,  1330,  1075,   454,  -589, -1816, -2866, -3445, -3435, -2928, -2158, -1377,  -727,  -193,   217,   323,    39,  -378,  -630,  -684,  -586,  -326,   -39,    46,   -19,    67,   411,   859,  1283,  1607,  1739, },
+	/* LAT:  10 */ {   1601,  1609,  1565,  1580,  1604,  1553,  1429,  1240,   864,   146,  -875, -1949, -2771, -3104, -2893, -2282, -1505,  -796,  -283,    96,   421,   566,   401,    78,  -155,  -262,  -288,  -209,  -108,  -153,  -296,  -271,    33,   493,   978,  1389,  1601, },
+	/* LAT:  20 */ {   1414,  1563,  1623,  1714,  1799,  1777,  1620,  1316,   772,   -84, -1125, -2073, -2665, -2769, -2437, -1833, -1122,  -474,   -16,   292,   550,   694,   601,   357,   159,    40,   -55,  -123,  -206,  -398,  -638,  -697,  -461,   -10,   535,  1056,  1414, },
+	/* LAT:  30 */ {   1108,  1475,  1735,  1959,  2118,  2128,  1939,  1516,   789,  -238, -1352, -2225, -2635, -2570, -2169, -1589,  -933,  -316,   144,   449,   679,   824,   802,   652,   504,   378,   217,     1,  -285,  -662, -1032, -1191, -1030,  -601,   -23,   587,  1108, },
+	/* LAT:  40 */ {    746,  1331,  1826,  2219,  2469,  2513,  2298,  1765,   855,  -368, -1602, -2463, -2780, -2626, -2180, -1587,  -934,  -305,   204,   568,   837,  1034,  1125,  1114,  1038,   888,   615,   198,  -346,  -954, -1475, -1718, -1599, -1181,  -583,    88,   746, },
+	/* LAT:  50 */ {    449,  1195,  1877,  2436,  2805,  2913,  2686,  2034,   897,  -593, -2008, -2918, -3211, -3019, -2529, -1883, -1173,  -476,   139,   644,  1062,  1415,  1693,  1862,  1880,  1689,  1238,   523,  -377, -1280, -1957, -2240, -2113, -1673, -1040,  -312,   449, },
+	/* LAT:  60 */ {    244,  1095,  1901,  2599,  3107,  3324,  3116,  2312,   809, -1137, -2844, -3822, -4075, -3817, -3246, -2502, -1676,  -837,   -30,   721,  1412,  2042,  2583,  2976,  3133,  2940,  2293,  1167,  -254, -1573, -2437, -2742, -2576, -2084, -1396,  -601,   244, },
+	/* LAT:  70 */ {     -8,   925,  1822,  2622,  3241,  3541,  3301,  2190,    -8, -2686, -4631, -5465, -5483, -5007, -4243, -3312, -2290, -1226,  -152,   907,  1931,  2896,  3763,  4468,  4912,  4938,  4317,  2828,   621, -1489, -2783, -3223, -3065, -2537, -1794,  -930,    -8, },
+	/* LAT:  80 */ {   -771,   149,  1000,  1691,  2080,  1915,   776, -1705, -4800, -6913, -7700, -7604, -7000, -6100, -5021, -3830, -2570, -1269,    54,  1379,  2692,  3974,  5202,  6341,  7339,  8100,  8444,  7991,  6025,  2161, -1488, -3200, -3553, -3219, -2541, -1692,  -771, },
+	/* LAT:  90 */ { -29638,-27893,-26147,-24402,-22657,-20911,-19166,-17421,-15675,-13930,-12185,-10440, -8695, -6949, -5204, -3459, -1714,    31,  1776,  3521,  5267,  7012,  8757, 10503, 12248, 13993, 15739, 17484, 19229, 20975, 22720, 24466, 26211, 27957, 29702,-31384,-29638, },
 };

 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.937,
+// Date: 2023.074,
 static constexpr const int16_t inclination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ { -12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569, },
-	/* LAT: -80 */ { -13653,-13519,-13359,-13178,-12985,-12783,-12579,-12379,-12188,-12012,-11856,-11721,-11611,-11525,-11461,-11420,-11400,-11403,-11430,-11484,-11567,-11682,-11827,-12001,-12199,-12416,-12645,-12878,-13106,-13319,-13505,-13655,-13759,-13810,-13806,-13752,-13653, },
-	/* LAT: -70 */ { -14101,-13783,-13463,-13140,-12809,-12465,-12110,-11753,-11410,-11102,-10850,-10666,-10554,-10502,-10488,-10491,-10497,-10506,-10529,-10584,-10693,-10868,-11114,-11429,-11803,-12221,-12669,-13133,-13599,-14052,-14469,-14813,-15001,-14946,-14715,-14417,-14101, },
-	/* LAT: -60 */ { -13516,-13162,-12824,-12491,-12148,-11775,-11360,-10906,-10439,-10009, -9680, -9505, -9503, -9640, -9844,-10035,-10159,-10200,-10186,-10171,-10218,-10378,-10668,-11078,-11578,-12134,-12721,-13317,-13906,-14467,-14964,-15253,-15075,-14690,-14283,-13889,-13516, },
-	/* LAT: -50 */ { -12495,-12152,-11821,-11499,-11175,-10829,-10430, -9959, -9429, -8909, -8521, -8397, -8600, -9062, -9625,-10135,-10491,-10650,-10617,-10463,-10321,-10328,-10552,-10971,-11514,-12107,-12694,-13232,-13677,-13975,-14083,-14009,-13806,-13522,-13193,-12845,-12495, },
-	/* LAT: -40 */ { -11239,-10890,-10543,-10197, -9858, -9520, -9159, -8734, -8214, -7650, -7226, -7188, -7652, -8477, -9398,-10225,-10881,-11312,-11446,-11274,-10930,-10659,-10656,-10941,-11405,-11910,-12355,-12675,-12833,-12838,-12748,-12612,-12433,-12201,-11915,-11586,-11239, },
-	/* LAT: -30 */ {  -9602, -9222, -8842, -8451, -8059, -7684, -7328, -6938, -6426, -5815, -5366, -5456, -6232, -7437, -8688, -9780,-10691,-11396,-11789,-11773,-11397,-10889,-10559,-10563,-10817,-11138,-11393,-11504,-11440,-11265,-11092,-10959,-10814,-10608,-10327, -9981, -9602, },
-	/* LAT: -20 */ {  -7372, -6929, -6510, -6082, -5637, -5208, -4818, -4407, -3842, -3158, -2714, -2984, -4105, -5722, -7349, -8713, -9773,-10543,-10975,-11003,-10633,-10017, -9461, -9220, -9272, -9434, -9574, -9589, -9416, -9144, -8953, -8869, -8768, -8562, -8246, -7832, -7372, },
-	/* LAT: -10 */ {  -4416, -3877, -3421, -2983, -2526, -2076, -1663, -1212,  -588,   114,   470,    31, -1310, -3233, -5206, -6806, -7893, -8520, -8779, -8710, -8295, -7606, -6943, -6601, -6564, -6657, -6773, -6795, -6606, -6311, -6160, -6175, -6140, -5926, -5542, -5013, -4416, },
-	/* LAT:   0 */ {   -908,  -281,   187,   591,  1009,  1426,  1814,  2254,  2835,  3409,  3605,  3099,  1792,  -125, -2167, -3807, -4805, -5225, -5278, -5103, -4657, -3936, -3231, -2863, -2804, -2872, -2996, -3068, -2938, -2704, -2654, -2804, -2874, -2691, -2266, -1634,  -908, },
-	/* LAT:  10 */ {   2559,  3189,  3626,  3968,  4323,  4690,  5040,  5424,  5871,  6238,  6269,  5773,  4682,  3111,  1422,    55,  -740,  -979,  -882,  -643,  -227,   415,  1048,  1383,  1446,  1403,  1304,  1213,  1255,  1357,  1277,  1003,   805,   874,  1226,  1831,  2559, },
-	/* LAT:  20 */ {   5415,  5945,  6326,  6622,  6936,  7279,  7621,  7965,  8296,  8493,  8402,  7934,  7094,  5995,  4871,  3968,  3446,  3333,  3487,  3733,  4067,  4534,  4995,  5249,  5306,  5291,  5244,  5186,  5174,  5154,  4977,  4639,  4337,  4245,  4417,  4842,  5415, },
-	/* LAT:  30 */ {   7568,  7942,  8260,  8543,  8852,  9199,  9556,  9895, 10170, 10281, 10133,  9709,  9078,  8367,  7708,  7200,  6912,  6874,  7019,  7232,  7481,  7781,  8070,  8244,  8301,  8316,  8320,  8310,  8286,  8202,  7978,  7622,  7265,  7043,  7027,  7223,  7568, },
-	/* LAT:  40 */ {   9266,  9487,  9743, 10028, 10354, 10715, 11082, 11420, 11670, 11751, 11600, 11239, 10765, 10293,  9898,  9617,  9470,  9468,  9581,  9743,  9920, 10105, 10277, 10402, 10480, 10542, 10598, 10631, 10614, 10502, 10258,  9907,  9538,  9253,  9110,  9123,  9266, },
-	/* LAT:  50 */ {  10802, 10923, 11124, 11393, 11716, 12069, 12423, 12739, 12959, 13018, 12878, 12580, 12216, 11872, 11597, 11412, 11321, 11319, 11386, 11490, 11605, 11721, 11838, 11954, 12072, 12196, 12310, 12382, 12372, 12246, 11998, 11670, 11330, 11045, 10856, 10775, 10802, },
-	/* LAT:  60 */ {  12319, 12391, 12541, 12759, 13028, 13328, 13630, 13895, 14071, 14098, 13963, 13715, 13426, 13156, 12936, 12780, 12690, 12660, 12676, 12724, 12793, 12879, 12985, 13117, 13277, 13453, 13618, 13726, 13731, 13610, 13385, 13106, 12827, 12591, 12421, 12329, 12319, },
-	/* LAT:  70 */ {  13758, 13800, 13894, 14035, 14214, 14417, 14624, 14805, 14910, 14891, 14756, 14555, 14338, 14135, 13962, 13827, 13733, 13679, 13660, 13673, 13716, 13788, 13892, 14029, 14196, 14383, 14567, 14707, 14753, 14680, 14518, 14319, 14125, 13960, 13840, 13771, 13758, },
-	/* LAT:  80 */ {  14996, 15008, 15044, 15103, 15179, 15263, 15341, 15387, 15371, 15294, 15183, 15060, 14938, 14824, 14725, 14643, 14581, 14540, 14522, 14526, 14552, 14602, 14674, 14766, 14878, 15005, 15141, 15273, 15380, 15424, 15384, 15297, 15201, 15116, 15051, 15010, 14996, },
+	/* LAT: -90 */ { -12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568, },
+	/* LAT: -80 */ { -13652,-13518,-13358,-13177,-12983,-12782,-12578,-12378,-12187,-12011,-11855,-11721,-11610,-11524,-11461,-11419,-11399,-11402,-11429,-11483,-11567,-11681,-11826,-12000,-12198,-12415,-12645,-12878,-13106,-13318,-13504,-13654,-13758,-13809,-13805,-13751,-13652, },
+	/* LAT: -70 */ { -14100,-13781,-13462,-13139,-12807,-12464,-12109,-11752,-11409,-11101,-10849,-10666,-10554,-10502,-10488,-10491,-10497,-10505,-10528,-10583,-10692,-10867,-11114,-11429,-11803,-12221,-12669,-13133,-13599,-14052,-14469,-14813,-15000,-14945,-14714,-14416,-14100, },
+	/* LAT: -60 */ { -13515,-13161,-12823,-12490,-12147,-11774,-11360,-10905,-10438,-10009, -9680, -9506, -9504, -9641, -9845,-10036,-10159,-10199,-10185,-10169,-10216,-10376,-10668,-11078,-11578,-12135,-12722,-13318,-13906,-14468,-14965,-15254,-15075,-14689,-14282,-13889,-13515, },
+	/* LAT: -50 */ { -12494,-12152,-11820,-11498,-11175,-10828,-10429, -9958, -9429, -8909, -8521, -8399, -8603, -9065, -9628,-10137,-10492,-10650,-10616,-10461,-10319,-10327,-10551,-10971,-11515,-12108,-12695,-13233,-13678,-13975,-14083,-14010,-13806,-13522,-13193,-12845,-12494, },
+	/* LAT: -40 */ { -11239,-10890,-10542,-10196, -9857, -9519, -9159, -8734, -8214, -7650, -7227, -7191, -7656, -8482, -9403,-10229,-10884,-11313,-11446,-11272,-10927,-10657,-10655,-10942,-11406,-11912,-12356,-12676,-12833,-12838,-12748,-12612,-12433,-12202,-11915,-11587,-11239, },
+	/* LAT: -30 */ {  -9602, -9222, -8841, -8450, -8058, -7683, -7328, -6938, -6426, -5815, -5368, -5459, -6238, -7445, -8695, -9786,-10696,-11400,-11791,-11772,-11394,-10886,-10557,-10563,-10818,-11139,-11394,-11504,-11440,-11264,-11091,-10959,-10815,-10609,-10328, -9981, -9602, },
+	/* LAT: -20 */ {  -7372, -6929, -6509, -6080, -5636, -5206, -4817, -4407, -3842, -3158, -2716, -2989, -4113, -5732, -7358, -8721, -9779,-10547,-10977,-11002,-10632,-10014, -9458, -9219, -9271, -9434, -9573, -9588, -9414, -9143, -8952, -8869, -8769, -8563, -8247, -7833, -7372, },
+	/* LAT: -10 */ {  -4417, -3876, -3419, -2981, -2523, -2073, -1661, -1211,  -588,   113,   467,    25, -1319, -3245, -5217, -6815, -7899, -8523, -8780, -8710, -8293, -7603, -6940, -6599, -6563, -6656, -6771, -6793, -6604, -6309, -6158, -6175, -6141, -5928, -5544, -5014, -4417, },
+	/* LAT:   0 */ {   -909,  -280,   189,   593,  1012,  1428,  1816,  2255,  2834,  3407,  3602,  3092,  1782,  -137, -2178, -3815, -4810, -5227, -5279, -5102, -4655, -3933, -3228, -2861, -2801, -2870, -2993, -3065, -2935, -2701, -2652, -2804, -2876, -2693, -2269, -1636,  -909, },
+	/* LAT:  10 */ {   2559,  3190,  3628,  3970,  4326,  4692,  5042,  5424,  5871,  6236,  6266,  5767,  4674,  3101,  1413,    48,  -744,  -981,  -883,  -643,  -226,   418,  1051,  1385,  1448,  1406,  1307,  1216,  1258,  1360,  1279,  1003,   803,   871,  1224,  1829,  2559, },
+	/* LAT:  20 */ {   5415,  5946,  6328,  6624,  6938,  7281,  7622,  7966,  8295,  8491,  8399,  7930,  7088,  5988,  4865,  3963,  3443,  3332,  3487,  3734,  4068,  4536,  4997,  5251,  5309,  5294,  5247,  5189,  5176,  5156,  4978,  4639,  4335,  4243,  4415,  4840,  5415, },
+	/* LAT:  30 */ {   7568,  7943,  8261,  8544,  8853,  9200,  9556,  9895, 10169, 10279, 10131,  9706,  9074,  8362,  7704,  7196,  6910,  6873,  7019,  7233,  7482,  7783,  8072,  8245,  8303,  8318,  8322,  8312,  8288,  8204,  7979,  7623,  7264,  7043,  7027,  7222,  7568, },
+	/* LAT:  40 */ {   9266,  9487,  9743, 10028, 10355, 10715, 11082, 11419, 11669, 11749, 11597, 11236, 10762, 10290,  9896,  9615,  9469,  9468,  9581,  9744,  9921, 10106, 10278, 10403, 10482, 10544, 10600, 10633, 10615, 10503, 10259,  9907,  9538,  9253,  9110,  9123,  9266, },
+	/* LAT:  50 */ {  10802, 10923, 11124, 11393, 11716, 12069, 12422, 12737, 12958, 13016, 12876, 12578, 12214, 11870, 11596, 11411, 11320, 11318, 11386, 11491, 11606, 11722, 11839, 11955, 12074, 12197, 12311, 12384, 12373, 12247, 11999, 11670, 11330, 11045, 10856, 10776, 10802, },
+	/* LAT:  60 */ {  12319, 12391, 12541, 12758, 13027, 13327, 13628, 13894, 14069, 14096, 13961, 13713, 13425, 13155, 12935, 12779, 12690, 12660, 12676, 12725, 12794, 12879, 12986, 13119, 13278, 13455, 13619, 13728, 13732, 13611, 13385, 13106, 12828, 12591, 12421, 12329, 12319, },
+	/* LAT:  70 */ {  13758, 13799, 13893, 14034, 14212, 14415, 14622, 14803, 14908, 14890, 14755, 14554, 14338, 14134, 13961, 13827, 13733, 13679, 13660, 13674, 13716, 13789, 13893, 14030, 14197, 14384, 14568, 14708, 14753, 14680, 14518, 14320, 14125, 13961, 13840, 13771, 13758, },
+	/* LAT:  80 */ {  14995, 15007, 15043, 15102, 15177, 15261, 15339, 15385, 15369, 15293, 15183, 15060, 14938, 14824, 14725, 14643, 14581, 14541, 14522, 14526, 14553, 14603, 14674, 14767, 14879, 15006, 15142, 15274, 15381, 15425, 15385, 15297, 15201, 15116, 15050, 15010, 14995, },
 	/* LAT:  90 */ {  15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, },
 };

 // Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.937,
+// Date: 2023.074,
 static constexpr const int16_t strength_table[19][37] {
 	//    LONGITUDE:  -180, -170, -160, -150, -140, -130, -120, -110, -100,  -90,  -80,  -70,  -60,  -50,  -40,  -30,  -20,  -10,    0,   10,   20,   30,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130,  140,  150,  160,  170,  180,
-	/* LAT: -90 */ {  5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, },
-	/* LAT: -80 */ {  6056, 5992, 5913, 5821, 5717, 5606, 5488, 5366, 5243, 5124, 5009, 4904, 4811, 4731, 4668, 4623, 4599, 4597, 4619, 4665, 4735, 4829, 4944, 5076, 5220, 5370, 5520, 5663, 5794, 5908, 6000, 6069, 6113, 6132, 6128, 6102, 6056, },
-	/* LAT: -70 */ {  6300, 6167, 6016, 5851, 5672, 5480, 5274, 5060, 4841, 4626, 4423, 4241, 4083, 3952, 3848, 3772, 3725, 3713, 3741, 3818, 3946, 4127, 4355, 4624, 4919, 5226, 5530, 5814, 6063, 6267, 6418, 6513, 6555, 6547, 6497, 6412, 6300, },
-	/* LAT: -60 */ {  6185, 5993, 5791, 5582, 5363, 5128, 4871, 4593, 4301, 4011, 3745, 3518, 3338, 3204, 3105, 3032, 2981, 2961, 2987, 3078, 3247, 3499, 3827, 4213, 4636, 5070, 5491, 5875, 6200, 6448, 6611, 6690, 6691, 6628, 6514, 6362, 6185, },
-	/* LAT: -50 */ {  5843, 5612, 5380, 5149, 4916, 4671, 4401, 4097, 3769, 3437, 3135, 2896, 2735, 2644, 2595, 2561, 2527, 2500, 2505, 2578, 2753, 3044, 3441, 3913, 4420, 4926, 5401, 5819, 6158, 6399, 6537, 6578, 6534, 6424, 6262, 6063, 5843, },
-	/* LAT: -40 */ {  5392, 5146, 4901, 4661, 4426, 4189, 3934, 3650, 3336, 3010, 2712, 2490, 2373, 2348, 2367, 2388, 2392, 2379, 2366, 2395, 2527, 2804, 3223, 3738, 4285, 4809, 5275, 5663, 5955, 6142, 6230, 6233, 6163, 6031, 5850, 5632, 5392, },
-	/* LAT: -30 */ {  4878, 4637, 4397, 4163, 3936, 3717, 3498, 3267, 3011, 2736, 2478, 2296, 2227, 2252, 2319, 2390, 2455, 2504, 2526, 2538, 2606, 2806, 3171, 3660, 4192, 4688, 5106, 5422, 5624, 5724, 5750, 5723, 5643, 5511, 5331, 5115, 4878, },
-	/* LAT: -20 */ {  4321, 4108, 3899, 3694, 3498, 3315, 3146, 2983, 2805, 2607, 2416, 2282, 2242, 2286, 2376, 2486, 2614, 2741, 2829, 2864, 2890, 2987, 3229, 3614, 4065, 4491, 4839, 5073, 5177, 5185, 5155, 5108, 5025, 4898, 4731, 4534, 4321, },
-	/* LAT: -10 */ {  3790, 3629, 3476, 3329, 3193, 3073, 2970, 2878, 2780, 2665, 2543, 2444, 2399, 2424, 2511, 2640, 2796, 2954, 3077, 3139, 3153, 3180, 3305, 3558, 3885, 4207, 4471, 4633, 4668, 4615, 4547, 4484, 4396, 4271, 4122, 3958, 3790, },
-	/* LAT:   0 */ {  3412, 3319, 3235, 3162, 3106, 3068, 3042, 3023, 2998, 2950, 2871, 2776, 2696, 2666, 2709, 2812, 2944, 3080, 3194, 3269, 3300, 3322, 3397, 3556, 3765, 3979, 4158, 4264, 4269, 4202, 4114, 4021, 3910, 3778, 3644, 3520, 3412, },
-	/* LAT:  10 */ {  3283, 3251, 3231, 3227, 3251, 3298, 3354, 3407, 3441, 3431, 3362, 3248, 3121, 3027, 3002, 3043, 3125, 3223, 3323, 3408, 3472, 3534, 3623, 3742, 3877, 4016, 4135, 4206, 4207, 4145, 4036, 3892, 3730, 3571, 3435, 3339, 3283, },
-	/* LAT:  20 */ {  3399, 3402, 3428, 3481, 3573, 3694, 3822, 3939, 4020, 4032, 3958, 3817, 3650, 3511, 3436, 3424, 3460, 3533, 3629, 3727, 3818, 3916, 4027, 4139, 4248, 4360, 4462, 4528, 4539, 4480, 4342, 4140, 3912, 3703, 3540, 3438, 3399, },
-	/* LAT:  30 */ {  3722, 3729, 3783, 3882, 4024, 4195, 4370, 4526, 4634, 4661, 4588, 4433, 4244, 4081, 3976, 3930, 3934, 3986, 4073, 4171, 4269, 4375, 4491, 4609, 4729, 4856, 4977, 5063, 5089, 5031, 4874, 4634, 4360, 4106, 3906, 3778, 3722, },
-	/* LAT:  40 */ {  4222, 4219, 4284, 4407, 4573, 4760, 4943, 5100, 5205, 5230, 5162, 5014, 4828, 4654, 4527, 4452, 4426, 4449, 4510, 4589, 4676, 4773, 4888, 5022, 5173, 5335, 5487, 5596, 5635, 5580, 5424, 5186, 4912, 4653, 4442, 4297, 4222, },
-	/* LAT:  50 */ {  4832, 4823, 4878, 4988, 5133, 5292, 5442, 5563, 5637, 5647, 5584, 5457, 5295, 5132, 4995, 4898, 4844, 4832, 4857, 4908, 4977, 5068, 5187, 5337, 5513, 5697, 5864, 5981, 6025, 5980, 5849, 5653, 5429, 5213, 5033, 4904, 4832, },
-	/* LAT:  60 */ {  5392, 5379, 5406, 5468, 5553, 5647, 5735, 5801, 5835, 5826, 5773, 5679, 5559, 5431, 5312, 5216, 5150, 5116, 5115, 5143, 5199, 5286, 5404, 5551, 5718, 5886, 6033, 6136, 6180, 6157, 6075, 5949, 5803, 5660, 5538, 5447, 5392, },
-	/* LAT:  70 */ {  5726, 5705, 5702, 5713, 5735, 5761, 5785, 5800, 5800, 5781, 5742, 5686, 5616, 5540, 5466, 5402, 5355, 5327, 5322, 5342, 5387, 5457, 5549, 5657, 5774, 5888, 5987, 6060, 6099, 6102, 6073, 6020, 5953, 5883, 5817, 5764, 5726, },
-	/* LAT:  80 */ {  5790, 5772, 5757, 5745, 5735, 5725, 5715, 5704, 5689, 5671, 5649, 5624, 5597, 5570, 5546, 5526, 5512, 5507, 5512, 5527, 5553, 5588, 5631, 5679, 5729, 5777, 5819, 5854, 5878, 5891, 5893, 5886, 5871, 5852, 5831, 5810, 5790, },
+	/* LAT: -90 */ {  5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, },
+	/* LAT: -80 */ {  6055, 5991, 5912, 5820, 5716, 5605, 5486, 5365, 5242, 5122, 5008, 4903, 4809, 4730, 4667, 4622, 4598, 4596, 4618, 4664, 4735, 4829, 4944, 5076, 5220, 5370, 5519, 5663, 5794, 5907, 6000, 6069, 6113, 6132, 6127, 6101, 6055, },
+	/* LAT: -70 */ {  6299, 6166, 6015, 5850, 5671, 5478, 5273, 5058, 4839, 4624, 4422, 4240, 4082, 3951, 3847, 3771, 3724, 3712, 3741, 3817, 3946, 4126, 4355, 4624, 4919, 5226, 5530, 5814, 6063, 6267, 6418, 6513, 6554, 6547, 6496, 6412, 6299, },
+	/* LAT: -60 */ {  6185, 5992, 5790, 5581, 5361, 5127, 4870, 4592, 4299, 4010, 3743, 3516, 3337, 3203, 3104, 3031, 2980, 2960, 2987, 3078, 3247, 3499, 3827, 4214, 4636, 5071, 5492, 5876, 6200, 6449, 6612, 6690, 6691, 6627, 6513, 6362, 6185, },
+	/* LAT: -50 */ {  5842, 5612, 5379, 5148, 4915, 4670, 4399, 4096, 3767, 3436, 3134, 2895, 2734, 2643, 2595, 2560, 2526, 2499, 2505, 2578, 2752, 3044, 3441, 3914, 4421, 4927, 5402, 5820, 6159, 6400, 6537, 6578, 6535, 6424, 6261, 6063, 5842, },
+	/* LAT: -40 */ {  5392, 5146, 4900, 4660, 4426, 4188, 3933, 3649, 3335, 3009, 2711, 2489, 2373, 2347, 2366, 2387, 2391, 2378, 2365, 2394, 2526, 2804, 3224, 3739, 4286, 4810, 5276, 5664, 5955, 6143, 6230, 6233, 6163, 6031, 5850, 5631, 5392, },
+	/* LAT: -30 */ {  4878, 4636, 4397, 4162, 3936, 3717, 3498, 3266, 3010, 2735, 2477, 2296, 2227, 2252, 2319, 2390, 2454, 2504, 2524, 2536, 2605, 2807, 3171, 3661, 4193, 4690, 5107, 5422, 5625, 5724, 5750, 5723, 5644, 5511, 5331, 5114, 4878, },
+	/* LAT: -20 */ {  4321, 4107, 3898, 3693, 3497, 3314, 3146, 2982, 2804, 2606, 2415, 2282, 2242, 2286, 2376, 2486, 2614, 2741, 2828, 2863, 2889, 2987, 3229, 3615, 4066, 4493, 4840, 5073, 5178, 5185, 5155, 5108, 5026, 4898, 4731, 4534, 4321, },
+	/* LAT: -10 */ {  3790, 3629, 3475, 3329, 3193, 3073, 2969, 2877, 2779, 2664, 2542, 2443, 2399, 2424, 2512, 2641, 2796, 2954, 3077, 3138, 3152, 3180, 3305, 3559, 3886, 4208, 4472, 4633, 4668, 4615, 4547, 4484, 4396, 4271, 4122, 3958, 3790, },
+	/* LAT:   0 */ {  3412, 3319, 3234, 3161, 3106, 3068, 3041, 3022, 2997, 2948, 2870, 2775, 2696, 2666, 2709, 2812, 2945, 3080, 3194, 3269, 3299, 3322, 3398, 3556, 3766, 3980, 4159, 4264, 4270, 4202, 4114, 4021, 3910, 3779, 3644, 3520, 3412, },
+	/* LAT:  10 */ {  3283, 3251, 3230, 3227, 3251, 3298, 3353, 3406, 3440, 3430, 3361, 3247, 3120, 3027, 3002, 3044, 3125, 3223, 3323, 3408, 3472, 3534, 3623, 3742, 3878, 4017, 4136, 4206, 4208, 4146, 4036, 3892, 3731, 3571, 3436, 3339, 3283, },
+	/* LAT:  20 */ {  3399, 3402, 3427, 3481, 3572, 3693, 3821, 3938, 4018, 4030, 3957, 3815, 3649, 3510, 3435, 3424, 3460, 3533, 3630, 3727, 3818, 3917, 4028, 4140, 4249, 4361, 4464, 4529, 4539, 4480, 4343, 4140, 3913, 3703, 3540, 3439, 3399, },
+	/* LAT:  30 */ {  3722, 3728, 3782, 3881, 4023, 4194, 4369, 4525, 4633, 4659, 4586, 4431, 4243, 4080, 3975, 3930, 3934, 3986, 4073, 4172, 4270, 4376, 4492, 4610, 4730, 4857, 4978, 5065, 5090, 5032, 4875, 4635, 4361, 4106, 3907, 3778, 3722, },
+	/* LAT:  40 */ {  4222, 4219, 4283, 4406, 4572, 4759, 4942, 5098, 5203, 5229, 5161, 5013, 4827, 4654, 4526, 4452, 4426, 4449, 4510, 4590, 4677, 4774, 4889, 5023, 5174, 5336, 5488, 5598, 5636, 5581, 5425, 5187, 4913, 4653, 4443, 4297, 4222, },
+	/* LAT:  50 */ {  4832, 4823, 4878, 4987, 5132, 5291, 5441, 5562, 5636, 5646, 5583, 5456, 5294, 5131, 4995, 4898, 4844, 4833, 4858, 4908, 4978, 5069, 5188, 5339, 5514, 5699, 5865, 5982, 6025, 5980, 5849, 5653, 5429, 5214, 5034, 4904, 4832, },
+	/* LAT:  60 */ {  5392, 5378, 5405, 5467, 5552, 5646, 5733, 5800, 5834, 5826, 5772, 5679, 5559, 5431, 5312, 5216, 5150, 5117, 5115, 5143, 5200, 5287, 5405, 5552, 5719, 5887, 6034, 6137, 6180, 6158, 6075, 5949, 5804, 5661, 5539, 5447, 5392, },
+	/* LAT:  70 */ {  5726, 5705, 5701, 5712, 5734, 5760, 5784, 5799, 5799, 5781, 5742, 5685, 5616, 5540, 5466, 5403, 5355, 5327, 5323, 5343, 5388, 5458, 5549, 5658, 5775, 5889, 5988, 6060, 6099, 6103, 6074, 6021, 5954, 5883, 5818, 5764, 5726, },
+	/* LAT:  80 */ {  5790, 5772, 5757, 5745, 5734, 5725, 5715, 5703, 5689, 5671, 5649, 5624, 5597, 5571, 5546, 5526, 5513, 5508, 5513, 5528, 5554, 5589, 5632, 5680, 5729, 5777, 5820, 5854, 5878, 5891, 5893, 5886, 5872, 5853, 5831, 5810, 5790, },
 	/* LAT:  90 */ {  5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, },
 };
@@ -25,7 +25,7 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f);
 * @decimal 5
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 0.0001f);
+PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f);

 /**
 * Airspeed Selector: Wind estimator true airspeed measurement noise
@@ -1713,9 +1713,14 @@ void Commander::run()

 			perf_begin(_preflight_check_perf);
 			_health_and_arming_checks.update();
-			_vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
-			perf_end(_preflight_check_perf);
+			bool pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);

+			if (_vehicle_status.pre_flight_checks_pass != pre_flight_checks_pass) {
+				_vehicle_status.pre_flight_checks_pass = pre_flight_checks_pass;
+				_status_changed = true;
+			}
+
+			perf_end(_preflight_check_perf);
 			checkAndInformReadyForTakeoff();
 		}

@@ -1766,6 +1771,7 @@ void Commander::run()
 			_last_disarmed_timestamp = hrt_absolute_time();

 			_user_mode_intention.onDisarm();
+			_vehicle_status.takeoff_time = 0;
 		}

 		if (!_arm_state_machine.isArmed()) {
@@ -1971,7 +1977,6 @@ void Commander::landDetectorUpdate()
 			if (!was_landed && _vehicle_land_detected.landed) {
 				mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
 				events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
-				_vehicle_status.takeoff_time = 0;

 			} else if (was_landed && !_vehicle_land_detected.landed) {
 				mavlink_log_info(&_mavlink_log_pub, "Takeoff detected\t");
@@ -37,7 +37,7 @@
 using namespace time_literals;

 AirspeedChecks::AirspeedChecks()
-	: _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_trim_handle(param_find("FW_AIRSPD_TRIM"))
+	: _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_max_handle(param_find("FW_AIRSPD_MAX"))
 {
 }

@@ -58,10 +58,10 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)

 		reporter.setIsPresent(health_component_t::differential_pressure);

-		float airspeed_trim = 10.0f;
-		param_get(_param_fw_airspd_trim_handle, &airspeed_trim);
-
-		const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
+		// Maximally allow the airspeed reading to be at FW_AIRSPD_MAX when arming. This is to catch very badly calibrated
+		// airspeed sensors, but also high wind conditions that prevent a forward flight of the vehicle.
+		float arming_max_airspeed_allowed = 20.f;
+		param_get(_param_fw_airspd_max_handle, &arming_max_airspeed_allowed);

 		/*
 		 * Check if airspeed is declared valid or not by airspeed selector.
@@ -79,21 +79,17 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 			}
 		}

-		/*
-		 * Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying
-		 * Negative and positive offsets are considered. This check is optional, because the pitot cover
-		 * might have been removed before arming.
-		 */
 		if (!context.isArmed() && _param_com_arm_arsp_en.get()
 		    && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) {
 			/* EVENT
 			 * @description
-			 * Check the airspeed calibration and the pitot.
+			 * Current airspeed reading too high. Check if wind is below maximum airspeed and redo airspeed
+			 * calibration if the measured airspeed does not correspond to wind conditions.
 			 *
 			 * <profile name="dev">
 			 * Measured: {1:.1m/s}, limit: {2:.1m/s}.
 			 *
-			 * This check can be configured via <param>COM_ARM_ARSP_EN</param> parameter.
+			 * This check can be configured via <param>COM_ARM_ARSP_EN</param> and <param>FW_AIRSPD_MAX</param> parameter.
 			 * </profile>
 			 */
 			reporter.armingCheckFailure<float, float>(NavModes::None, health_component_t::differential_pressure,
@@ -101,7 +97,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 					events::Log::Error, "Airspeed too high", airspeed_validated.calibrated_airspeed_m_s, arming_max_airspeed_allowed);

 			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed too high - check cal or pitot");
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed too high - check airspeed calibration");
 			}
 		}

@@ -50,7 +50,7 @@ private:
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};

 	const param_t _param_fw_arsp_mode_handle;
-	const param_t _param_fw_airspd_trim_handle;
+	const param_t _param_fw_airspd_max_handle;

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
@@ -126,6 +126,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 	if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) {
 		gpsNoLongerValid(context, reporter);
 	}
+
+	lowPositionAccuracy(context, reporter, lpos);
 }

 void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
@@ -548,7 +550,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 			_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
 		}

-		// Check for a magnetomer fault and notify the user
+		// Check for a magnetometer fault and notify the user
 		if (estimator_status_flags.cs_mag_fault) {
 			/* EVENT
 			 * @description
@@ -669,6 +671,35 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter)
 	}
 }

+void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter,
+		const vehicle_local_position_s &lpos) const
+{
+	const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid
+			&& (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
+
+	if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy) {
+
+		// only report if armed
+		if (context.isArmed()) {
+			/* EVENT
+			 * @description Local position estimate valid but has low accuracy. Warn user.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_POS_LOW_EPH</param> parameter.
+			 * </profile>
+			 */
+			events::send(events::ID("check_estimator_low_position_accuracy"), {events::Log::Error, events::LogInternal::Info},
+				     "Local position estimate has low accuracy");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_warning(reporter.mavlink_log_pub(), "Local position estimate has low accuracy\t");
+			}
+		}
+	}
+
+	reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy;
+}
+
 void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
 		bool pre_flt_fail_innov_vel_horiz,
 		const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags)
@@ -67,6 +67,7 @@ private:

 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void gpsNoLongerValid(const Context &context, Report &reporter) const;
+	void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
 	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
 				     const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
 				     failsafe_flags_s &failsafe_flags);
@@ -117,6 +118,7 @@ private:
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
-					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay
+					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
+					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph
 				       )
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-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
@@ -55,4 +55,61 @@ void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter)
 	} else {
 		reporter.failsafeFlags().flight_time_limit_exceeded = false;
 	}
+
+	// report warning when approaching max flight time
+	if (!reporter.failsafeFlags().flight_time_limit_exceeded
+	    && context.status().takeoff_time != 0
+	    && _param_com_flt_time_max.get() > 0) {
+
+		const float remaining_flight_time_sec = (context.status().takeoff_time < hrt_absolute_time()) ?
+							_param_com_flt_time_max.get() - 1.e-6f * (hrt_absolute_time() - context.status().takeoff_time) :
+							_param_com_flt_time_max.get();
+
+		if (remaining_flight_time_sec < 0.1f * _param_com_flt_time_max.get()) {
+			// send warnings every minute until RTL
+
+			const int floored_remaining_flight_time_sec = int(remaining_flight_time_sec);
+
+			if (floored_remaining_flight_time_sec <= 60 && _last_flight_time_warning_sec == -1) {
+				// less than or equal to a minute remaining on first pass
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_warning(reporter.mavlink_log_pub(), "Approaching max flight time (system will RTL in %i seconds)\t",
+							    floored_remaining_flight_time_sec);
+				}
+
+				/* EVENT
+				* @description
+				* Maximal flight time warning (less than 1min remaining)
+				*/
+				events::send<int16_t>(events::ID("commander_max_flight_time_warning_seconds"), events::Log::Warning,
+						      "Approaching max flight time (system will RTL in {1} seconds)", floored_remaining_flight_time_sec);
+
+			} else if ((floored_remaining_flight_time_sec % 60) == 0 && floored_remaining_flight_time_sec >= 60
+				   && floored_remaining_flight_time_sec != _last_flight_time_warning_sec) {
+
+				const int floored_remaining_flight_time_min = (int)(remaining_flight_time_sec * 0.016666667f);
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_warning(reporter.mavlink_log_pub(), "Approaching max flight time (system will RTL in %i minutes)\t",
+							    floored_remaining_flight_time_min);
+				}
+
+				/* EVENT
+				* @description
+				* Maximal flight time warning (more than 1min remaining)
+				*/
+				events::send<int16_t>(events::ID("commander_max_flight_time_warning_minutes"), events::Log::Warning,
+						      "Approaching max flight time (system will RTL in {1} minutes)", floored_remaining_flight_time_min);
+			}
+
+			_last_flight_time_warning_sec = floored_remaining_flight_time_sec;
+
+		} else {
+			_last_flight_time_warning_sec = -1; //reset if not in last 10%
+		}
+
+	} else {
+		_last_flight_time_warning_sec = -1; //reset if not enabled, landed or currently already exceeded
+	}
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-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
@@ -44,6 +44,8 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;

 private:
+	int _last_flight_time_warning_sec{-1};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max
 				       )
@@ -147,6 +147,12 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		// Here we expect there is already an event reported for the failing check (this is for external modes)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other);
 	}
+
+	if ((reporter.failsafeFlags().flight_time_limit_exceeded || reporter.failsafeFlags().wind_limit_exceeded)
+	    && reporter.failsafeFlags().mode_req_wind_and_flight_time_compliance != 0) {
+		// Already reported
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_wind_and_flight_time_compliance);
+	}
 }

 void ModeChecks::checkArmingRequirement(const Context &context, Report &reporter)
@@ -54,6 +54,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	flags.mode_req_mission = 0;
 	flags.mode_req_offboard_signal = 0;
 	flags.mode_req_home_position = 0;
+	flags.mode_req_wind_and_flight_time_compliance = 0;
 	flags.mode_req_prevent_arming = 0;
 	flags.mode_req_other = 0;

@@ -82,6 +83,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_wind_and_flight_time_compliance);

 	// NAVIGATION_STATE_AUTO_LOITER
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity);
@@ -89,6 +91,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance);

 	// NAVIGATION_STATE_AUTO_RTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity);
@@ -139,6 +142,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_prevent_arming);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_wind_and_flight_time_compliance);

 	// NAVIGATION_STATE_AUTO_PRECLAND
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_angular_velocity);
@@ -153,6 +157,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_prevent_arming);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_wind_and_flight_time_compliance);

 	// NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_angular_velocity);
@@ -43,11 +43,6 @@ UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_
 bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
 {
 	_ever_had_mode_change = true;
-	_had_mode_change = true;
-
-	if (_user_intented_nav_state == user_intended_nav_state) {
-		return true;
-	}

 	// Always allow mode change while disarmed
 	bool always_allow = force || !isArmed();
@@ -67,6 +62,7 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
 	}

 	if (allow_change) {
+		_had_mode_change = true;
 		_user_intented_nav_state = user_intended_nav_state;

 		if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -714,32 +714,47 @@ PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
 /**
 * Horizontal position error threshold.
 *
- * This is the horizontal position error (EPH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * This is the horizontal position error (EPH) threshold that will trigger a failsafe.
+ * The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * If the previous position error was below this threshold, there is an additional
+ * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
 *
 * @unit m
+ * @min 0
+ * @decimal 1
 * @group Commander
 */
-PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5);
+PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5.f);

 /**
 * Vertical position error threshold.
 *
- * This is the vertical position error (EPV) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * This is the vertical position error (EPV) threshold that will trigger a failsafe.
+ * The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * If the previous position error was below this threshold, there is an additional
+ * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
 *
 * @unit m
+ * @min 0
+ * @decimal 1
 * @group Commander
 */
-PARAM_DEFINE_FLOAT(COM_POS_FS_EPV, 10);
+PARAM_DEFINE_FLOAT(COM_POS_FS_EPV, 10.f);

 /**
 * Horizontal velocity error threshold.
 *
- * This is the horizontal velocity error (EVH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * This is the horizontal velocity error (EVH) threshold that will trigger a failsafe.
+ * The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * If the previous velocity error was below this threshold, there is an additional
+ * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
 *
 * @unit m/s
+ * @min 0
+ * @decimal 1
 * @group Commander
 */
-PARAM_DEFINE_FLOAT(COM_VEL_FS_EVH, 1);
+PARAM_DEFINE_FLOAT(COM_VEL_FS_EVH, 1.f);

 /**
 * Next flight UUID
@@ -1025,12 +1040,11 @@ PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f);
 * Wind speed warning threshold
 *
 * A warning is triggered if the currently estimated wind speed is above this value.
- * Warning is sent periodically (every 1min).
+ * Warning is sent periodically (every 1 minute).
 *
- * A negative value disables the feature.
+ * Set to -1 to disable.
 *
 * @min -1
- * @max 30
 * @decimal 1
 * @increment 0.1
 * @group Commander
@@ -1043,32 +1057,51 @@ PARAM_DEFINE_FLOAT(COM_WIND_WARN, -1.f);
 *
 * The vehicle aborts the current operation and returns to launch when
 * the time since takeoff is above this value. It is not possible to resume the
- * mission or switch to any mode other than RTL or Land.
+ * mission or switch to any auto mode other than RTL or Land. Taking over in any manual
+ * mode is still possible.
 *
- * Set a negative value to disable.
+ * Starting from 90% of the maximum flight time, a warning message will be sent
+ * every 1 minute with the remaining time until automatic RTL.
 *
+ * Set to -1 to disable.
 *
 * @unit s
 * @min -1
- * @max 10000
- * @value 0 Disable
 * @group Commander
 */
 PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1);

 /**
- * Wind speed RLT threshold
+ * Wind speed RTL threshold
 *
- * Wind speed threshold above which an automatic return to launch is triggered
- * and enforced as long as the threshold is exceeded.
+ * Wind speed threshold above which an automatic return to launch is triggered.
+ * It is not possible to resume the mission or switch to any auto mode other than
+ * RTL or Land if this threshold is exceeded. Taking over in any manual
+ * mode is still possible.
 *
- * A negative value disables the feature.
+ * Set to -1 to disable.
 *
 * @min -1
- * @max 30
 * @decimal 1
 * @increment 0.1
 * @group Commander
 * @unit m/s
 */
 PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
+
+/**
+ * EPH threshold for RTL
+ *
+ * Specify the threshold for triggering a warning for low local position accuracy. Additionally triggers
+ * a RTL if currently in Mission or Loiter mode.
+ * Local position has to be still declared valid, which is most of all depending on COM_POS_FS_EPH.
+ * Use this feature on systems with dead-reckoning capabilites (e.g. fixed-wing vehicles with airspeed sensor)
+ * to improve the user notification and failure mitigation when flying in GNSS-denied areas.
+ *
+ * Set to -1 to disable.
+ *
+ * @group Commander
+ * @min -1
+ * @unit m
+ */
+PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f);
@@ -32,7 +32,10 @@
      }
      .checkbox-field {
          display: flex;
+          align-items: center;
          flex-direction: row;
+          margin-bottom: 8px;
+          line-height: 100%;
      }
      .box {
          background-color: rgb(231, 233, 235);
@@ -393,8 +393,13 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,

 	CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
 		       ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
-	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded,
-		       ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
+	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL));
+
+	// trigger RTL if low position accurancy is detected
+	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
+	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
+		CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
+	}

 	CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()));

@@ -614,6 +614,7 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended
 bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode)
 {
 	uint32_t mode_mask = 1u << mode;
+	// mode_req_wind_and_flight_time_compliance: does not need to be handled here (these are separate failsafe triggers)
 	return
 		(!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) &&
 		(!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0)) &&
@@ -102,6 +102,7 @@ px4_add_module(
 		EKF/gps_checks.cpp
 		EKF/gps_control.cpp
 		EKF/gps_yaw_fusion.cpp
+		EKF/gravity_fusion.cpp
 		EKF/height_control.cpp
 		EKF/imu_down_sampler.cpp
 		EKF/mag_control.cpp
@@ -53,6 +53,7 @@ add_library(ecl_EKF
 	gps_checks.cpp
 	gps_control.cpp
 	gps_yaw_fusion.cpp
+	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
 	mag_control.cpp
@@ -81,10 +81,7 @@ public:

 		_size = size;

-		_head = 0;
-		_tail = 0;
-
-		_first_write = true;
+		reset();

 		return true;
 	}
@@ -171,6 +168,19 @@ public:
 		return count;
 	}

+	void reset()
+	{
+		if (_buffer) {
+			for (uint8_t i = 0; i < _size; i++) {
+				_buffer[i] = {};
+			}
+
+			_head = 0;
+			_tail = 0;
+			_first_write = true;
+		}
+	}
+
 private:
 	data_type *_buffer{nullptr};

@@ -157,6 +157,8 @@ void Ekf::resetWindUsingAirspeed()
 	_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
 	_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);

+	ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
+
 	resetWindCovarianceUsingAirspeed();

 	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
@@ -164,8 +166,11 @@ void Ekf::resetWindUsingAirspeed()

 void Ekf::resetWindToZero()
 {
+	ECL_INFO("reset wind to zero");
+
 	// If we don't have an airspeed measurement, then assume the wind is zero
 	_state.wind_vel.setZero();
+
 	// start with a small initial uncertainty to improve the initial estimate
 	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
 }
@@ -126,6 +126,12 @@ enum class PositionSensor : uint8_t {
 	EV      = 2,
 };

+enum class ImuCtrl : uint8_t {
+	GyroBias      = (1<<0),
+	AccelBias     = (1<<1),
+	GravityVector = (1<<2),
+};
+
 enum GnssCtrl : uint8_t {
 	HPOS  = (1<<0),
 	VPOS  = (1<<1),
@@ -150,7 +156,7 @@ enum SensorFusionMask : uint16_t {
 	// Bit locations for fusion_mode
 	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
 	USE_OPT_FLOW     = (1<<1),      ///< set to true to use optical flow data
-	INHIBIT_ACC_BIAS = (1<<2),      ///< set to true to inhibit estimation of accelerometer delta velocity bias
+	DEPRECATED_INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
 	DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
 	DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
@@ -279,6 +285,8 @@ struct parameters {

 	int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds

+	int32_t imu_ctrl{static_cast<int32_t>(ImuCtrl::GyroBias) | static_cast<int32_t>(ImuCtrl::AccelBias)};
+
 	// measurement source control
 	int32_t fusion_mode{};         ///< bitmasked integer that selects some aiding sources
 	int32_t height_sensor_ref{HeightSensor::BARO};
@@ -386,6 +394,9 @@ struct parameters {
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))

+	// gravity fusion
+	float gravity_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)
+
 	// optical flow fusion
 	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
 	float flow_noise_qual_min{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
@@ -557,6 +568,7 @@ union filter_control_status_u {
 		uint64_t rng_kin_consistent      : 1; ///< 31 - true when the range finder kinematic consistency check is passing
 		uint64_t fake_pos                : 1; ///< 32 - true when fake position measurements are being fused
 		uint64_t fake_hgt                : 1; ///< 33 - true when fake height measurements are being fused
+		uint64_t gravity_vector          : 1; ///< 34 - true when gravity vector measurements are being fused
 	} flags;
 	uint64_t value;
 };
@@ -623,9 +635,9 @@ union warning_event_status_u {
 		bool stopping_navigation                : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
 		bool invalid_accel_bias_cov_reset       : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
 		bool bad_yaw_using_gps_course           : 1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
-		bool stopping_mag_use                   : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
+		bool stopping_mag_use                   : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
 		bool vision_data_stopped                : 1; ///< 9 - true when the vision system data has stopped for a significant time period
-		bool emergency_yaw_reset_mag_stopped    : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
+		bool emergency_yaw_reset_mag_stopped    : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
 		bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
 	} flags;
 	uint32_t value;
@@ -198,6 +198,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlBetaFusion(imu_delayed);
 	controlDragFusion();
 	controlHeightFusion(imu_delayed);
+	controlGravityFusion(imu_delayed);

 	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
@@ -81,9 +81,9 @@ void Ekf::initialiseCovariance()
 	}

 	// gyro bias
-	P(10,10) = sq(_params.switch_on_gyro_bias * dt);
-	P(11,11) = P(10,10);
-	P(12,12) = P(10,10);
+	_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
+	_prev_delta_ang_bias_var(1) = P(11,11) = P(10,10);
+	_prev_delta_ang_bias_var(2) = P(12,12) = P(10,10);

 	// accel bias
 	_prev_dvel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias * dt);
@@ -121,7 +121,36 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
 					     || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;

-	const bool do_inhibit_all_axes = (_params.fusion_mode & SensorFusionMask::INHIBIT_ACC_BIAS)
+	// gyro bias inhibit
+	const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
+
+	for (unsigned stateIndex = 10; stateIndex <= 12; stateIndex++) {
+		const unsigned index = stateIndex - 10;
+
+		bool is_bias_observable = true;
+
+		// TODO: gyro bias conditions
+
+		const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
+
+		if (do_inhibit_axis) {
+			// store the bias state variances to be reinstated later
+			if (!_gyro_bias_inhibit[index]) {
+				_prev_delta_ang_bias_var(index) = P(stateIndex, stateIndex);
+				_gyro_bias_inhibit[index] = true;
+			}
+
+		} else {
+			if (_gyro_bias_inhibit[index]) {
+				// reinstate the bias state variances
+				P(stateIndex, stateIndex) = _prev_delta_ang_bias_var(index);
+				_gyro_bias_inhibit[index] = false;
+			}
+		}
+	}
+
+	// accel bias inhibit
+	const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
 					 || is_manoeuvre_level_high
 					 || _fault_status.flags.bad_acc_vertical;

@@ -141,7 +170,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 			is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
 		}

-		const bool do_inhibit_axis = do_inhibit_all_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
+		const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;

 		if (do_inhibit_axis) {
 			// store the bias state variances to be reinstated later
@@ -239,7 +268,17 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	// the variances, therefore use algorithm to minimise numerical error
 	for (unsigned i = 10; i <= 12; i++) {
 		const int index = i - 10;
-		nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
+
+		if (!_gyro_bias_inhibit[index]) {
+			// add process noise that is not from the IMU
+			// process noise contribution for delta velocity states can be very small compared to
+			// the variances, therefore use algorithm to minimise numerical error
+			nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
+
+		} else {
+			nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_delta_ang_bias_var(index));
+			_delta_angle_bias_var_accum(index) = 0.f;
+		}
 	}

 	for (int i = 13; i <= 15; i++) {
@@ -81,6 +81,62 @@ void Ekf::reset()
 	resetGpsDriftCheckFilters();

 	_output_predictor.reset();
+
+	// Ekf private fields
+	_time_last_horizontal_aiding = 0;
+	_time_last_v_pos_aiding = 0;
+	_time_last_v_vel_aiding = 0;
+
+	_time_last_hor_pos_fuse = 0;
+	_time_last_hgt_fuse = 0;
+	_time_last_hor_vel_fuse = 0;
+	_time_last_ver_vel_fuse = 0;
+	_time_last_heading_fuse = 0;
+
+	_time_last_flow_terrain_fuse = 0;
+	_time_last_zero_velocity_fuse = 0;
+	_time_last_healthy_rng_data = 0;
+
+	_last_known_pos.setZero();
+
+	_time_acc_bias_check = 0;
+
+	_gps_checks_passed = false;
+
+	_gps_alt_ref = NAN;
+
+	_baro_counter = 0;
+	_mag_counter = 0;
+
+	_time_bad_vert_accel = 0;
+	_time_good_vert_accel = 0;
+	_clip_counter = 0;
+
+	resetEstimatorAidStatus(_aid_src_baro_hgt);
+	resetEstimatorAidStatus(_aid_src_rng_hgt);
+	resetEstimatorAidStatus(_aid_src_airspeed);
+	resetEstimatorAidStatus(_aid_src_sideslip);
+
+	resetEstimatorAidStatus(_aid_src_fake_pos);
+	resetEstimatorAidStatus(_aid_src_fake_hgt);
+
+	resetEstimatorAidStatus(_aid_src_ev_hgt);
+	resetEstimatorAidStatus(_aid_src_ev_pos);
+	resetEstimatorAidStatus(_aid_src_ev_vel);
+	resetEstimatorAidStatus(_aid_src_ev_yaw);
+
+	resetEstimatorAidStatus(_aid_src_gnss_hgt);
+	resetEstimatorAidStatus(_aid_src_gnss_pos);
+	resetEstimatorAidStatus(_aid_src_gnss_vel);
+	resetEstimatorAidStatus(_aid_src_gnss_yaw);
+
+	resetEstimatorAidStatus(_aid_src_mag_heading);
+	resetEstimatorAidStatus(_aid_src_mag);
+
+	resetEstimatorAidStatus(_aid_src_aux_vel);
+
+	resetEstimatorAidStatus(_aid_src_optical_flow);
+	resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
 }

 bool Ekf::update()
@@ -198,11 +254,6 @@ bool Ekf::initialiseFilter()
 	// Initialise the terrain estimator
 	initHagl();

-	// reset the essential fusion timeout counters
-	_time_last_hgt_fuse = _time_delayed_us;
-	_time_last_hor_pos_fuse = _time_delayed_us;
-	_time_last_hor_vel_fuse = _time_delayed_us;
-
 	// reset the output predictor state history to match the EKF initial values
 	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);

@@ -191,6 +191,10 @@ public:
 	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
 	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }

+	void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
+	void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
+	void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
+
 	// get the state vector at the delayed time horizon
 	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;

@@ -329,6 +333,7 @@ public:
 	float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss

 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
+	bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; }

 	const auto &state_reset_status() const { return _state_reset_status; }

@@ -432,6 +437,8 @@ public:
 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
 	const auto &aid_src_mag() const { return _aid_src_mag; }

+	const auto &aid_src_gravity() const { return _aid_src_gravity; }
+
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }

 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
@@ -531,8 +538,8 @@ private:

 	SquareMatrix24f P{};	///< state covariance matrix

-	Vector3f _delta_vel_bias_var_accum{};		///< kahan summation algorithm accumulator for delta velocity bias variance
 	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance
+	Vector3f _delta_vel_bias_var_accum{};   ///< kahan summation algorithm accumulator for delta velocity bias variance

 	Vector2f _drag_innov{};		///< multirotor drag measurement innovation (m/sec**2)
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
@@ -572,6 +579,8 @@ private:
 	estimator_aid_source1d_s _aid_src_mag_heading{};
 	estimator_aid_source3d_s _aid_src_mag{};

+	estimator_aid_source3d_s _aid_src_gravity{};
+
 	estimator_aid_source2d_s _aid_src_aux_vel{};

 	estimator_aid_source2d_s _aid_src_optical_flow{};
@@ -614,9 +623,11 @@ private:

 	// variables used to inhibit accel bias learning
 	bool _accel_bias_inhibit[3] {};		///< true when the accel bias learning is being inhibited for the specified axis
+	bool _gyro_bias_inhibit[3] {};		///< true when the gyro bias learning is being inhibited for the specified axis
 	Vector3f _accel_vec_filt{};		///< acceleration vector after application of a low pass filter (m/sec**2)
 	float _accel_magnitude_filt{0.0f};	///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
 	float _ang_rate_magnitude_filt{0.0f};		///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
+	Vector3f _prev_delta_ang_bias_var{};	///< saved delta angle XYZ bias variances (rad/sec)
 	Vector3f _prev_dvel_bias_var{};		///< saved delta velocity XYZ bias variances (m/sec)**2

 	// Terrain height state estimation
@@ -765,6 +776,12 @@ private:
 	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
 	{
 		for (unsigned i = 0; i < 3; i++) {
+			// gyro bias: states 10, 11, 12
+			if (_gyro_bias_inhibit[i]) {
+				K(10 + i) = 0.0f;
+			}
+
+			// accel bias: states 13, 14, 15
 			if (_accel_bias_inhibit[i]) {
 				K(13 + i) = 0.0f;
 			}
@@ -918,6 +935,9 @@ private:

 	void updateGroundEffect();

+	// gravity fusion: heuristically enable / disable gravity fusion
+	void controlGravityFusion(const imuSample &imu_delayed);
+
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar(const flowSample &flow_sample);

@@ -964,17 +984,17 @@ private:

 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
 	{
-		return last_sensor_timestamp + timeout_period < _time_delayed_us;
+		return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us);
 	}

 	bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
 	{
-		return sensor_timestamp + acceptance_interval > _time_delayed_us;
+		return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_delayed_us);
 	}

 	bool isNewestSampleRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
 	{
-		return sensor_timestamp + acceptance_interval > _time_latest_us;
+		return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
 	}

 	void startAirspeedFusion();
@@ -116,8 +116,10 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)

 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;
-	ECL_INFO("reset position to last known position");
+
 	// Used when falling back to non-aiding mode of operation
 	resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
 }
@@ -242,6 +244,8 @@ bool Ekf::resetMagHeading()
 		float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 		float yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.e-2f));

+		ECL_INFO("reset mag heading %.3f -> %.3f rad", (double)getEulerYaw(_R_to_earth), (double)yaw_new);
+
 		// update quaternion states and corresponding covarainces
 		resetQuatStateYaw(yaw_new, yaw_new_variance);

@@ -854,8 +858,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 	}

 	// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
-	bool deadreckon_time_exceeded = (_time_last_horizontal_aiding == 0)
-				    || isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
+	bool deadreckon_time_exceeded = isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);

 	if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) {
 		// deadreckon time now exceeded
@@ -871,7 +874,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
 		_time_last_v_pos_aiding = _time_last_hgt_fuse;
 		_vertical_position_deadreckon_time_exceeded = false;

-	} else if ((_time_last_v_pos_aiding == 0) || isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) {
+	} else if (isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) {
 		_vertical_position_deadreckon_time_exceeded = true;
 	}

@@ -879,8 +882,9 @@ void Ekf::updateVerticalDeadReckoningStatus()
 		_time_last_v_vel_aiding = _time_last_ver_vel_fuse;
 		_vertical_velocity_deadreckon_time_exceeded = false;

-	} else if (((_time_last_v_vel_aiding == 0) || isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max))
+	} else if (isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max)
 		   && _vertical_position_deadreckon_time_exceeded) {
+
 		_vertical_velocity_deadreckon_time_exceeded = true;
 	}
 }
@@ -1180,20 +1184,27 @@ void Ekf::loadMagCovData()

 void Ekf::startAirspeedFusion()
 {
-	// If starting wind state estimation, reset the wind states and covariances before fusing any data
-	if (!_control_status.flags.wind) {
-		// activate the wind states
-		_control_status.flags.wind = true;
-		// reset the wind speed states and corresponding covariances
-		resetWindUsingAirspeed();
-	}
+	if (!_control_status.flags.fuse_aspd) {
+		ECL_INFO("starting airspeed fusion");

-	_control_status.flags.fuse_aspd = true;
+		// If starting wind state estimation, reset the wind states and covariances before fusing any data
+		if (!_control_status.flags.wind) {
+			// activate the wind states
+			_control_status.flags.wind = true;
+			// reset the wind speed states and corresponding covariances
+			resetWindUsingAirspeed();
+		}
+
+		_control_status.flags.fuse_aspd = true;
+	}
 }

 void Ekf::stopAirspeedFusion()
 {
-	_control_status.flags.fuse_aspd = false;
+	if (_control_status.flags.fuse_aspd) {
+		ECL_INFO("stopping airspeed fusion");
+		_control_status.flags.fuse_aspd = false;
+	}
 }

 void Ekf::stopGpsFusion()
@@ -1307,6 +1318,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)

 	_state_reset_status.reset_count.quat++;

+	_time_last_heading_fuse = _time_delayed_us;
+
 	_last_static_yaw = NAN;
 }

@@ -205,7 +205,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 		if (starting_conditions_passing) {
 			// activate fusion, only reset if necessary
 			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
-				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+				ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2));
 				_information_events.flags.reset_vel_to_vision = true;
 				resetVelocityTo(measurement, measurement_var);

@@ -104,7 +104,7 @@ void Ekf::resetFakeHgtFusion()
 void Ekf::resetHeightToLastKnown()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
-	ECL_INFO("reset height to last known");
+	ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2));
 	resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise));
 }

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 ECL 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 gravity_fusion.cpp
+ * Fuse observations from the gravity vector to constrain roll
+ * and pitch (a la complementary filter).
+ *
+ * @author Daniel M. Sahu <danielmohansahu@gmail.com>
+ */
+
+#include "ekf.h"
+#include "python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h"
+
+#include <mathlib/mathlib.h>
+
+void Ekf::controlGravityFusion(const imuSample &imu)
+{
+	// fuse gravity observation if our overall acceleration isn't too big
+	const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G;
+
+	_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
+					       && (((gravity_scale >= 0.9f && gravity_scale <= 1.1f)) || _control_status.flags.vehicle_at_rest)
+					       && !isHorizontalAidingActive();
+
+	// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
+	const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias();
+	const float measurement_var = sq(_params.gravity_noise);
+
+	// calculate kalman gains and innovation variances
+	Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2)
+	Vector3f innovation_variance;
+	Vector24f Kx, Ky, Kz; // Kalman gain vectors
+	sym::ComputeGravityInnovVarAndKAndH(
+		getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON,
+		&innovation, &innovation_variance, &Kx, &Ky, &Kz);
+
+	// fill estimator aid source status
+	resetEstimatorAidStatus(_aid_src_gravity);
+	_aid_src_gravity.timestamp_sample = imu.time_us;
+	measurement.copyTo(_aid_src_gravity.observation);
+
+	for (auto &var : _aid_src_gravity.observation_variance) {
+		var = measurement_var;
+	}
+
+	innovation.copyTo(_aid_src_gravity.innovation);
+	innovation_variance.copyTo(_aid_src_gravity.innovation_variance);
+
+	float innovation_gate = 1.f;
+	setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
+
+	_aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector;
+
+	if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) {
+		// perform fusion for each axis
+		_aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0))
+					 && measurementUpdate(Ky, innovation_variance(1), innovation(1))
+					 && measurementUpdate(Kz, innovation_variance(2), innovation(2));
+
+		if (_aid_src_gravity.fused) {
+			_aid_src_gravity.time_last_fuse = imu.time_us;
+		}
+	}
+}
@@ -51,7 +51,7 @@ void Ekf::controlMagFusion()
 		if (mag_data_ready) {

 			// sensor or calibration has changed, clear any mag bias and reset low pass filter
-			if (mag_sample.reset) {
+			if (mag_sample.reset || (_mag_counter == 0)) {
 				// Zero the magnetometer bias states
 				_state.mag_B.zero();

@@ -416,7 +416,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state
 		// covariances need to be corrected to incorporate knowledge of the declination
-		// before fusing magnetomer data to prevent rapid rotation of the earth field
+		// before fusing magnetometer data to prevent rapid rotation of the earth field
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
@@ -436,7 +436,7 @@ float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const
 	// of the earth magnetic field vector at the current location
 	const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));

-	// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component
+	// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetometer Z component
 	const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.col(2));

 	return (mag_z_body_pred < 0) ? -mag_z_abs : mag_z_abs;
@@ -179,7 +179,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)

 				} else {
 					_information_events.flags.reset_pos_to_last_known = true;
-					ECL_INFO("reset position to last known position");
+					ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
 					resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
 				}
 			}
@@ -212,7 +212,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));

 				// reset position, estimate is relative to initial position in this mode, so we start with zero error
-				ECL_INFO("reset position to last known");
+				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;
 				resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);

@@ -477,6 +477,36 @@ def compute_drag_y_innov_var_and_k(

    return (innov_var, K)

+def compute_gravity_innov_var_and_k_and_h(
+        state: VState,
+        P: MState,
+        meas: sf.V3,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.V3, sf.V3, VState, VState, VState):
+
+    # get transform from earth to body frame
+    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
+    R_to_body = quat_to_rot(q_att).T
+
+    # the innovation is the error between measured acceleration
+    #  and predicted (body frame), assuming no body acceleration
+    meas_pred = R_to_body * sf.Matrix([0,0,-9.80665])
+    innov = meas_pred - 9.80665 * meas.normalized(epsilon=epsilon)
+
+    # initialize outputs
+    innov_var = sf.V3()
+    K = [None] * 3
+
+    # calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
+    #  for each axis
+    for i in range(3):
+        H = sf.V1(meas_pred[i]).jacobian(state)
+        innov_var[i] = (H * P * H.T + R)[0,0]
+        K[i] = P * H.T / innov_var[i]
+
+    return (innov, innov_var, K[0], K[1], K[2])
+
 print("Derive EKF2 equations...")
 generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
 generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
@@ -497,3 +527,4 @@ generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var",
 generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"])
 generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
+generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
@@ -36,6 +36,8 @@ Description:

 import symforce.symbolic as sf

+import re
+
 # q: quaternion describing rotation from frame 1 to frame 2
 # returns a rotation matrix derived form q which describes the same
 # rotation
@@ -107,6 +109,10 @@ def generate_px4_function(function_name, output_names):
            line = line.replace("std::min", "math::min")
            line = line.replace("Eigen", "matrix")
            line = line.replace("matrix/Dense", "matrix/math.hpp")
+
+            # don't allow underscore + uppercase identifier naming (always reserved for any use)
+            line = re.sub(r'_([A-Z])', lambda x: '_' + x.group(1).lower(), line)
+
            print(line, end='')

 def generate_python_function(function_name, output_names):
@@ -47,67 +47,67 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,

  // Output terms (2)
  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(4, 0) = _tmp3;
-    _H(5, 0) = _tmp4;
-    _H(6, 0) = _tmp5;
-    _H(22, 0) = -_tmp3;
-    _H(23, 0) = -_tmp4;
+    _h(4, 0) = _tmp3;
+    _h(5, 0) = _tmp4;
+    _h(6, 0) = _tmp5;
+    _h(22, 0) = -_tmp3;
+    _h(23, 0) = -_tmp4;
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _K = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

-    _K(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 +
+    _k(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 +
                        P(0, 6) * _tmp5);
-    _K(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 +
+    _k(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 +
                        P(1, 6) * _tmp5);
-    _K(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 +
+    _k(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 +
                        P(2, 6) * _tmp5);
-    _K(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 +
+    _k(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 +
                        P(3, 6) * _tmp5);
-    _K(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 +
+    _k(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 +
                        P(4, 6) * _tmp5);
-    _K(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 +
+    _k(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 +
                        P(5, 6) * _tmp5);
-    _K(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 +
+    _k(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 +
                        P(6, 6) * _tmp5);
-    _K(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 +
+    _k(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 +
                        P(7, 6) * _tmp5);
-    _K(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 +
+    _k(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 +
                        P(8, 6) * _tmp5);
-    _K(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 +
+    _k(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 +
                        P(9, 6) * _tmp5);
-    _K(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 +
+    _k(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 +
                         P(10, 5) * _tmp4 + P(10, 6) * _tmp5);
-    _K(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 +
+    _k(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 +
                         P(11, 5) * _tmp4 + P(11, 6) * _tmp5);
-    _K(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 +
+    _k(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 +
                         P(12, 5) * _tmp4 + P(12, 6) * _tmp5);
-    _K(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 +
+    _k(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 +
                         P(13, 5) * _tmp4 + P(13, 6) * _tmp5);
-    _K(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 +
+    _k(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 +
                         P(14, 5) * _tmp4 + P(14, 6) * _tmp5);
-    _K(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 +
+    _k(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 +
                         P(15, 5) * _tmp4 + P(15, 6) * _tmp5);
-    _K(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 +
+    _k(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 +
                         P(16, 5) * _tmp4 + P(16, 6) * _tmp5);
-    _K(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 +
+    _k(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 +
                         P(17, 5) * _tmp4 + P(17, 6) * _tmp5);
-    _K(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 +
+    _k(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 +
                         P(18, 5) * _tmp4 + P(18, 6) * _tmp5);
-    _K(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 +
+    _k(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 +
                         P(19, 5) * _tmp4 + P(19, 6) * _tmp5);
-    _K(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 +
+    _k(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 +
                         P(20, 5) * _tmp4 + P(20, 6) * _tmp5);
-    _K(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 +
+    _k(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 +
                         P(21, 5) * _tmp4 + P(21, 6) * _tmp5);
-    _K(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 +
+    _k(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 +
                         P(22, 5) * _tmp4 + P(22, 6) * _tmp5);
-    _K(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 +
+    _k(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 +
                         P(23, 5) * _tmp4 + P(23, 6) * _tmp5);
  }
 }  // NOLINT(readability/fn_size)
@@ -167,14 +167,14 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _K = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

-    _K.setZero();
+    _k.setZero();

-    _K(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 +
+    _k(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 +
                          P(22, 23) * _tmp66 + P(22, 3) * _tmp70 + P(22, 4) * _tmp63 +
                          P(22, 5) * _tmp69 + P(22, 6) * _tmp51 + _tmp73);
-    _K(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 +
+    _k(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 +
                          P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp63 +
                          P(23, 5) * _tmp69 + P(23, 6) * _tmp51 + _tmp72);
  }
@@ -168,14 +168,14 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _K = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

-    _K.setZero();
+    _k.setZero();

-    _K(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 +
+    _k(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 +
                          P(22, 23) * _tmp58 + P(22, 3) * _tmp53 + P(22, 4) * _tmp65 +
                          P(22, 5) * _tmp71 + P(22, 6) * _tmp70 + _tmp72);
-    _K(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 +
+    _k(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 +
                          P(23, 22) * _tmp66 + P(23, 3) * _tmp53 + P(23, 4) * _tmp65 +
                          P(23, 5) * _tmp71 + P(23, 6) * _tmp70 + _tmp73);
  }
@@ -109,17 +109,17 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(0, 0) = _tmp17;
-    _H(1, 0) = _tmp21;
-    _H(2, 0) = _tmp22;
-    _H(3, 0) = _tmp15;
-    _H(4, 0) = _tmp9;
-    _H(5, 0) = _tmp4;
-    _H(6, 0) = _tmp23;
+    _h(0, 0) = _tmp17;
+    _h(1, 0) = _tmp21;
+    _h(2, 0) = _tmp22;
+    _h(3, 0) = _tmp15;
+    _h(4, 0) = _tmp9;
+    _h(5, 0) = _tmp4;
+    _h(6, 0) = _tmp23;
  }
 }  // NOLINT(readability/fn_size)

@@ -77,17 +77,17 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(0, 0) = -_tmp10;
-    _H(1, 0) = -_tmp11;
-    _H(2, 0) = -_tmp12;
-    _H(3, 0) = -_tmp8;
-    _H(4, 0) = -_tmp1;
-    _H(5, 0) = -_tmp4;
-    _H(6, 0) = -_tmp5;
+    _h(0, 0) = -_tmp10;
+    _h(1, 0) = -_tmp11;
+    _h(2, 0) = -_tmp12;
+    _h(3, 0) = -_tmp8;
+    _h(4, 0) = -_tmp1;
+    _h(5, 0) = -_tmp4;
+    _h(6, 0) = -_tmp5;
  }
 }  // NOLINT(readability/fn_size)

@@ -88,14 +88,14 @@ void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(0, 0) = _tmp26;
-    _H(1, 0) = _tmp25;
-    _H(2, 0) = _tmp27;
-    _H(3, 0) = _tmp19;
+    _h(0, 0) = _tmp26;
+    _h(1, 0) = _tmp25;
+    _h(2, 0) = _tmp27;
+    _h(3, 0) = _tmp19;
  }
 }  // NOLINT(readability/fn_size)

@@ -0,0 +1,276 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_gravity_innov_var_and_k_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     meas: Matrix31
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov: Matrix31
+ *     innov_var: Matrix31
+ *     Kx: Matrix24_1
+ *     Ky: Matrix24_1
+ *     Kz: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                                    const matrix::Matrix<Scalar, 24, 24>& P,
+                                    const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
+                                    const Scalar epsilon,
+                                    matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
+                                    matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
+  // Total ops: 736
+
+  // Input arrays
+
+  // Intermediate terms (54)
+  const Scalar _tmp0 =
+      Scalar(9.8066499999999994) /
+      std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) +
+                       std::pow(meas(2, 0), Scalar(2))));
+  const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0);
+  const Scalar _tmp2 = -P(3, 0) * _tmp1;
+  const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0);
+  const Scalar _tmp4 = P(0, 0) * _tmp3;
+  const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0);
+  const Scalar _tmp6 = P(2, 0) * _tmp5;
+  const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0);
+  const Scalar _tmp8 = P(3, 1) * _tmp1;
+  const Scalar _tmp9 = P(2, 1) * _tmp5;
+  const Scalar _tmp10 = -P(1, 1) * _tmp7;
+  const Scalar _tmp11 = P(0, 2) * _tmp3;
+  const Scalar _tmp12 = P(2, 2) * _tmp5;
+  const Scalar _tmp13 = -P(1, 2) * _tmp7;
+  const Scalar _tmp14 = -P(3, 3) * _tmp1;
+  const Scalar _tmp15 = P(0, 3) * _tmp3;
+  const Scalar _tmp16 = -P(1, 3) * _tmp7;
+  const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) +
+                        _tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) +
+                        _tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) -
+                        _tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9);
+  const Scalar _tmp18 = P(3, 0) * _tmp3;
+  const Scalar _tmp19 = -P(0, 0) * _tmp1;
+  const Scalar _tmp20 = -P(1, 0) * _tmp5;
+  const Scalar _tmp21 = P(3, 2) * _tmp3;
+  const Scalar _tmp22 = -P(2, 2) * _tmp7;
+  const Scalar _tmp23 = P(1, 2) * _tmp5;
+  const Scalar _tmp24 = P(0, 1) * _tmp1;
+  const Scalar _tmp25 = -P(2, 1) * _tmp7;
+  const Scalar _tmp26 = -P(1, 1) * _tmp5;
+  const Scalar _tmp27 = -P(3, 3) * _tmp3;
+  const Scalar _tmp28 = -P(0, 3) * _tmp1;
+  const Scalar _tmp29 = -P(2, 3) * _tmp7;
+  const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) -
+                        _tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) -
+                        _tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) -
+                        _tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23);
+  const Scalar _tmp31 = -P(0, 0) * _tmp5;
+  const Scalar _tmp32 = P(2, 0) * _tmp3;
+  const Scalar _tmp33 = P(1, 0) * _tmp1;
+  const Scalar _tmp34 = -P(3, 2) * _tmp7;
+  const Scalar _tmp35 = P(0, 2) * _tmp5;
+  const Scalar _tmp36 = P(2, 2) * _tmp3;
+  const Scalar _tmp37 = -P(3, 1) * _tmp7;
+  const Scalar _tmp38 = -P(0, 1) * _tmp5;
+  const Scalar _tmp39 = P(1, 1) * _tmp1;
+  const Scalar _tmp40 = -P(3, 3) * _tmp7;
+  const Scalar _tmp41 = P(2, 3) * _tmp3;
+  const Scalar _tmp42 = P(1, 3) * _tmp1;
+  const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) +
+                        _tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) -
+                        _tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) -
+                        _tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42);
+  const Scalar _tmp44 = Scalar(1.0) / (_tmp17);
+  const Scalar _tmp45 = Scalar(19.613299999999999) * P(8, 3);
+  const Scalar _tmp46 = Scalar(19.613299999999999) * P(8, 0);
+  const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 2);
+  const Scalar _tmp48 = Scalar(19.613299999999999) * P(9, 3);
+  const Scalar _tmp49 = Scalar(19.613299999999999) * P(9, 2);
+  const Scalar _tmp50 = Scalar(19.613299999999999) * P(9, 0);
+  const Scalar _tmp51 = Scalar(1.0) / (_tmp30);
+  const Scalar _tmp52 = Scalar(19.613299999999999) * P(4, 0);
+  const Scalar _tmp53 = Scalar(1.0) / (_tmp43);
+
+  // Output terms (5)
+  if (innov != nullptr) {
+    matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
+
+    _innov(0, 0) = -_tmp0 * meas(0, 0) + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) -
+                   Scalar(19.613299999999999) * state(1, 0) * state(3, 0);
+    _innov(1, 0) = -_tmp0 * meas(1, 0) - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) -
+                   Scalar(19.613299999999999) * state(2, 0) * state(3, 0);
+    _innov(2, 0) = -_tmp0 * meas(2, 0) -
+                   Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) +
+                   Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) +
+                   Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) -
+                   Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2));
+  }
+
+  if (innov_var != nullptr) {
+    matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
+
+    _innov_var(0, 0) = _tmp17;
+    _innov_var(1, 0) = _tmp30;
+    _innov_var(2, 0) = _tmp43;
+  }
+
+  if (Kx != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
+
+    _kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4);
+    _kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42);
+    _kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32);
+    _kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37);
+    _kx(4, 0) = _tmp44 * (P(4, 0) * _tmp3 - P(4, 1) * _tmp7 + P(4, 2) * _tmp5 - P(4, 3) * _tmp1);
+    _kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1);
+    _kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1);
+    _kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1);
+    _kx(8, 0) = _tmp44 * (-P(8, 1) * _tmp7 - _tmp45 * state(1, 0) + _tmp46 * state(2, 0) +
+                          _tmp47 * state(0, 0));
+    _kx(9, 0) = _tmp44 * (-P(9, 1) * _tmp7 - _tmp48 * state(1, 0) + _tmp49 * state(0, 0) +
+                          _tmp50 * state(2, 0));
+    _kx(10, 0) =
+        _tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1);
+    _kx(11, 0) =
+        _tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1);
+    _kx(12, 0) =
+        _tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1);
+    _kx(13, 0) =
+        _tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1);
+    _kx(14, 0) =
+        _tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1);
+    _kx(15, 0) =
+        _tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1);
+    _kx(16, 0) =
+        _tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1);
+    _kx(17, 0) =
+        _tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1);
+    _kx(18, 0) =
+        _tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1);
+    _kx(19, 0) =
+        _tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1);
+    _kx(20, 0) =
+        _tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1);
+    _kx(21, 0) =
+        _tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1);
+    _kx(22, 0) =
+        _tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1);
+    _kx(23, 0) =
+        _tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1);
+  }
+
+  if (Ky != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
+
+    _ky(0, 0) = _tmp51 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38);
+    _ky(1, 0) = _tmp51 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33);
+    _ky(2, 0) = _tmp51 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9);
+    _ky(3, 0) = _tmp51 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34);
+    _ky(4, 0) =
+        _tmp51 * (-P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3 - _tmp52 * state(1, 0));
+    _ky(5, 0) = _tmp51 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
+    _ky(6, 0) = _tmp51 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3);
+    _ky(7, 0) = _tmp51 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
+    _ky(8, 0) =
+        _tmp51 * (-P(8, 1) * _tmp5 - P(8, 2) * _tmp7 - _tmp45 * state(2, 0) - _tmp46 * state(1, 0));
+    _ky(9, 0) =
+        _tmp51 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - _tmp48 * state(2, 0) - _tmp50 * state(1, 0));
+    _ky(10, 0) =
+        _tmp51 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
+    _ky(11, 0) =
+        _tmp51 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
+    _ky(12, 0) =
+        _tmp51 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
+    _ky(13, 0) =
+        _tmp51 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
+    _ky(14, 0) =
+        _tmp51 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
+    _ky(15, 0) =
+        _tmp51 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
+    _ky(16, 0) =
+        _tmp51 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
+    _ky(17, 0) =
+        _tmp51 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
+    _ky(18, 0) =
+        _tmp51 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
+    _ky(19, 0) =
+        _tmp51 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
+    _ky(20, 0) =
+        _tmp51 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
+    _ky(21, 0) =
+        _tmp51 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
+    _ky(22, 0) =
+        _tmp51 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
+    _ky(23, 0) =
+        _tmp51 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
+  }
+
+  if (Kz != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
+
+    _kz(0, 0) = _tmp53 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31);
+    _kz(1, 0) = _tmp53 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39);
+    _kz(2, 0) = _tmp53 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6);
+    _kz(3, 0) = _tmp53 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8);
+    _kz(4, 0) =
+        _tmp53 * (P(4, 1) * _tmp1 + P(4, 2) * _tmp3 - P(4, 3) * _tmp7 - _tmp52 * state(0, 0));
+    _kz(5, 0) = _tmp53 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7);
+    _kz(6, 0) = _tmp53 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7);
+    _kz(7, 0) = _tmp53 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7);
+    _kz(8, 0) =
+        _tmp53 * (P(8, 1) * _tmp1 - P(8, 3) * _tmp7 - _tmp46 * state(0, 0) + _tmp47 * state(2, 0));
+    _kz(9, 0) =
+        _tmp53 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp49 * state(2, 0) - _tmp50 * state(0, 0));
+    _kz(10, 0) =
+        _tmp53 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7);
+    _kz(11, 0) =
+        _tmp53 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7);
+    _kz(12, 0) =
+        _tmp53 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7);
+    _kz(13, 0) =
+        _tmp53 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7);
+    _kz(14, 0) =
+        _tmp53 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7);
+    _kz(15, 0) =
+        _tmp53 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7);
+    _kz(16, 0) =
+        _tmp53 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7);
+    _kz(17, 0) =
+        _tmp53 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7);
+    _kz(18, 0) =
+        _tmp53 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7);
+    _kz(19, 0) =
+        _tmp53 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7);
+    _kz(20, 0) =
+        _tmp53 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7);
+    _kz(21, 0) =
+        _tmp53 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7);
+    _kz(22, 0) =
+        _tmp53 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7);
+    _kz(23, 0) =
+        _tmp53 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -61,12 +61,12 @@ void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(16, 0) = -_tmp2;
-    _H(17, 0) = _tmp3;
+    _h(16, 0) = -_tmp2;
+    _h(17, 0) = _tmp3;
  }
 }  // NOLINT(readability/fn_size)

@@ -158,18 +158,18 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _Hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);

-    _Hx.setZero();
+    _hx.setZero();

-    _Hx(0, 0) = _tmp23;
-    _Hx(1, 0) = _tmp24;
-    _Hx(2, 0) = _tmp32;
-    _Hx(3, 0) = _tmp28;
-    _Hx(16, 0) = _tmp6;
-    _Hx(17, 0) = _tmp35;
-    _Hx(18, 0) = _tmp38;
-    _Hx(19, 0) = 1;
+    _hx(0, 0) = _tmp23;
+    _hx(1, 0) = _tmp24;
+    _hx(2, 0) = _tmp32;
+    _hx(3, 0) = _tmp28;
+    _hx(16, 0) = _tmp6;
+    _hx(17, 0) = _tmp35;
+    _hx(18, 0) = _tmp38;
+    _hx(19, 0) = 1;
  }
 }  // NOLINT(readability/fn_size)

@@ -72,18 +72,18 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(0, 0) = _tmp3;
-    _H(1, 0) = _tmp5;
-    _H(2, 0) = _tmp7;
-    _H(3, 0) = _tmp6;
-    _H(16, 0) = _tmp8;
-    _H(17, 0) = _tmp10;
-    _H(18, 0) = _tmp9;
-    _H(20, 0) = 1;
+    _h(0, 0) = _tmp3;
+    _h(1, 0) = _tmp5;
+    _h(2, 0) = _tmp7;
+    _h(3, 0) = _tmp6;
+    _h(16, 0) = _tmp8;
+    _h(17, 0) = _tmp10;
+    _h(18, 0) = _tmp9;
+    _h(20, 0) = 1;
  }
 }  // NOLINT(readability/fn_size)

@@ -72,18 +72,18 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(0, 0) = _tmp3;
-    _H(1, 0) = _tmp5;
-    _H(2, 0) = _tmp7;
-    _H(3, 0) = _tmp6;
-    _H(16, 0) = _tmp8;
-    _H(17, 0) = _tmp9;
-    _H(18, 0) = _tmp10;
-    _H(21, 0) = 1;
+    _h(0, 0) = _tmp3;
+    _h(1, 0) = _tmp5;
+    _h(2, 0) = _tmp7;
+    _h(3, 0) = _tmp6;
+    _h(16, 0) = _tmp8;
+    _h(17, 0) = _tmp9;
+    _h(18, 0) = _tmp10;
+    _h(21, 0) = 1;
  }
 }  // NOLINT(readability/fn_size)

@@ -84,94 +84,94 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,

  // Output terms (2)
  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

-    _H.setZero();
+    _h.setZero();

-    _H(0, 0) = _tmp26;
-    _H(1, 0) = _tmp31;
-    _H(2, 0) = _tmp32;
-    _H(3, 0) = _tmp33;
-    _H(4, 0) = _tmp37;
-    _H(5, 0) = _tmp39;
-    _H(6, 0) = _tmp40;
-    _H(22, 0) = _tmp41;
-    _H(23, 0) = _tmp42;
+    _h(0, 0) = _tmp26;
+    _h(1, 0) = _tmp31;
+    _h(2, 0) = _tmp32;
+    _h(3, 0) = _tmp33;
+    _h(4, 0) = _tmp37;
+    _h(5, 0) = _tmp39;
+    _h(6, 0) = _tmp40;
+    _h(22, 0) = _tmp41;
+    _h(23, 0) = _tmp42;
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _K = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

-    _K(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
+    _k(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
                         P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 +
                         P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40);
-    _K(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
+    _k(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
                         P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 +
                         P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40);
-    _K(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
+    _k(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
                         P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 +
                         P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40);
-    _K(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
+    _k(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
                         P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 +
                         P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40);
-    _K(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
+    _k(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
                         P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 +
                         P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40);
-    _K(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
+    _k(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
                         P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 +
                         P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40);
-    _K(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
+    _k(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
                         P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 +
                         P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40);
-    _K(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
+    _k(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
                         P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 +
                         P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40);
-    _K(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
+    _k(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
                         P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 +
                         P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40);
-    _K(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
+    _k(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
                         P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 +
                         P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40);
-    _K(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
+    _k(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
                          P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 +
                          P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40);
-    _K(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
+    _k(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
                          P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 +
                          P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40);
-    _K(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
+    _k(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
                          P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 +
                          P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40);
-    _K(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
+    _k(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
                          P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 +
                          P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40);
-    _K(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
+    _k(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
                          P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 +
                          P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40);
-    _K(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
+    _k(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
                          P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 +
                          P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40);
-    _K(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
+    _k(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
                          P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 +
                          P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40);
-    _K(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
+    _k(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
                          P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 +
                          P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40);
-    _K(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
+    _k(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
                          P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 +
                          P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40);
-    _K(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
+    _k(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
                          P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 +
                          P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40);
-    _K(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
+    _k(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
                          P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 +
                          P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40);
-    _K(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
+    _k(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
                          P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 +
                          P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40);
-    _K(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
+    _k(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
                          P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 +
                          P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40);
-    _K(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
+    _k(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
                          P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 +
                          P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40);
  }
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Julian Oes	1280a7f92d	mavlink: early return in parser Signed-off-by: Julian Oes <julian@oes.ch>	2023-02-09 21:06:16 +13:00
Julian Oes	5f5cb3fac6	mavlink: add MAV_SIK_RADIO_ID param back in However, this is now without the functionality to reset to factory default. Signed-off-by: Julian Oes <julian@oes.ch>	2023-02-09 21:05:44 +13:00
Julian Oes	7044301148	mavlink: use command to set SiK ID This removes the param MAV_SIK_RADIO_ID and replaces it with a MAVLink command to set the radio's net ID. Using a command has the benefit that we get feedback whether the change has been accepted. The code now also reads back the bytes after doing the config in order to check for the radio's OK feedback. Previously, the commands were just sent blindly and there is the chance they did not actually get through. Signed-off-by: Julian Oes <julian@oes.ch>	2023-02-08 13:15:33 +13:00
Daniel Agar	7b3befded5	ekf2: disable new gravity fusion by default	2023-02-07 13:57:10 -05:00
Daniel Sahu	fa6fda6cce	ekf2: new gravity observation (#21038 ) Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>	2023-02-07 13:28:58 -05:00
alessandro	3e149ee6c5	FlightTaskAuto: landing position updates for precision landing (#20951 ) - precision landing works incorrectly, target position is not updated during the descent above target - _prepareLandSetpoints needs to update _land_position continuously Co-authored-by: kapacheuski <kapacheuski@gmail.com>	2023-02-07 12:07:26 -05:00
Jukka Laitinen	deb6053d56	Update status leds every time when prearm check status changes Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2023-02-07 08:02:32 +01:00
Christian Rauch	ef5761c223	add SPI to stm32f1	2023-02-07 07:54:38 +01:00
Christian Rauch	2f21c590b0	remove some vscode settings	2023-02-06 17:25:36 -05:00
JaeyoungLim	01c5b3934e	Tune GZ plane (rc_cessna) to fly nicely (#21077 ) * Increase control surface joint controller gains * Enable prearm mode and disable airpspeed checks for gz plane	2023-02-06 16:24:01 -05:00
Daniel Agar	661eb2adb4	lib/sensor_calibration: BiasCorrectedSensorOffset() don't incorporate thermal offsets - the thermal offsets are an optional correction applied to the raw data, so when updating an existing calibration offset with new learned bias we don't want this incorporated	2023-02-06 15:09:07 -05:00
Silvan Fuhrer	e153d1defc	ROMFS: fix PY asymmetry (motor 1 was wrongly placed twice as far from the CG as 0) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-04 16:08:40 +01:00
Daniel Agar	04d3e549f5	ekf2: resetQuatStateYaw() set _time_last_heading_fuse	2023-02-03 10:03:09 -05:00
Daniel Agar	66ad7fd06c	ekf2: include 0 timestamp checks in helpers (isTimedOut(), isRecent(), etc) - EKF isTimedOut(), isRecent(), and isNewestSampleRecent() need to handle the case where the timestamp has never been set - reset() more thoroughly reset fields (mainly impacts unit tests)	2023-02-03 10:00:51 -05:00
Daniel Agar	264a99fb77	ekf2: new EKF2_IMU_CTRL parameter and gyro bias inhibit mechanism - EKF2_AID_MASK accel bias inhibit moves to EKF2_IMU_CTRL	2023-02-03 09:52:24 -05:00
Daniel Agar	f668ea5aa6	ekf2: RingBuffer add reset method	2023-02-03 08:49:44 -05:00
Daniel Agar	e3d73cd837	ekf2: mag control reset mag_lpf on first sample	2023-02-02 16:12:09 -05:00
Michał Barciś	c2f13dbccf	setup/ubuntu.sh modified to correctly install all required dependencies for gazebo Signed-off-by: Michał Barciś <michal.barcis@tii.ae>	2023-02-02 10:02:47 -05:00
ShiauweiZhao	35080504f7	drivers/drv_sensor.h fix device type duplicate definition	2023-02-02 09:54:47 -05:00
Vincent Poon	a90bae9e50	ci: build and deploy kakuteh7v2 & Kakuteh7mini Adds Holybro kakuteh7v2 & Kakuteh7mini to Travis CI for building and deployment to S3 for QGC	2023-02-02 09:51:53 -05:00
Hamish Willee	2938db1c60	Apply suggestions from code review	2023-02-02 15:20:34 +01:00
Hamish Willee	7cea384404	Update src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c	2023-02-02 15:20:34 +01:00
Hamish Willee	d65e5969e1	FW_AT_MAN_AUX - define what an Aux input is	2023-02-02 15:20:34 +01:00
Silvan Fuhrer	ba1d02ee75	MPC: improve description of MPC_LAND_RC_HELP Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-02 14:31:22 +01:00
小光	849fbabc47	px4_mtd: the address of 'instances' will never be NULL (#21039 ) Signed-off-by: AuroraRAS <chplee@gmail.com>	2023-02-02 08:06:00 +01:00
Silvan Fuhrer	d9a4d1d5c4	Commander: use FW_AIRSPD_MAX as threshold for airspeed preflight checks Check fails if airspeed reading is above FW_AIRSPD_MAX. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 20:43:38 -05:00
Robbie Drage	cb4235887f	vscode: change cubeorange build target - Fixes typo in Cube Orange build target config Fixes issue #17745	2023-02-01 18:17:45 -05:00
Silvan Fuhrer	32cab66c44	Commander: hide hint to param in low position accuracy event for end users Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 16:47:11 +01:00
Silvan Fuhrer	9b3a28dff5	Commander: add local_position_accuracy_low flag, incl. warning and RTL Set this flag to true if local position is valid but accuracy low, such that the operator can be warned before system switches to position-failure failsafe. Additionally, switch to RTL if currently in Mission or Loiter to try to reach home or fly out of GNSS-denied area. Set low accuracy threshold to 50m by default for FW and VTOL. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 16:47:11 +01:00
Silvan Fuhrer	3ca126cc46	Commander: improve description of COM_FS EPH, EPV, VEL_EVH params and increase FW and VTOL defaults Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 16:47:11 +01:00
Silvan Fuhrer	cbc4c35bcf	Commander params: improve meta data for max flight time and wind Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 08:48:09 +01:00
Silvan Fuhrer	6d84da5cf1	Commander: add max flight time warning (starting at 90%) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 08:48:09 +01:00
Beat Küng	b1709743f7	commander: add wind or flight time limit exceeded mode requirement This prevents switching into any of these modes once the condition is set.	2023-02-01 08:48:09 +01:00
Beat Küng	a8628c9d9c	fix commander: clear takeoff_time only on disarm Otherwise the flight time restriction flag gets cleared too early, before disarming (which puts the vehicle into the previous mode and it might take off again).	2023-02-01 08:48:09 +01:00
Beat Küng	8e4c5884ec	fix commander: need to check for valid mode change even if already the same Fixes the following case: - user intention set to X - failsafe triggers, mode = Y - can_run for X becomes false - user tries to switch to X -> need to re-evaluate can_run	2023-02-01 08:48:09 +01:00
Beat Küng	7988491e37	failsafe simulator: improve spacing for multi-line checkboxes Reduces line-height to 100% in that case.	2023-02-01 08:48:09 +01:00
ShiauweiZhao	931f602995	drivers/imu/invensense: new IAM-20680HP IMU driver (#21025 )	2023-01-31 09:33:26 -05:00
Daniel Agar	2be701f902	platforms/nuttx: cmake debug pass GDB path and set RTOS plugin file extension	2023-01-30 20:19:38 -05:00
Eric Katzfey	a4aa76f0ac	VOXL2 board updates and new Kconfig option for ROOTFSDIR - also includes a couple of miscellaneous changes to VOXL2 support to show Qurt messages on px4 console and put logs in the proper spot	2023-01-30 12:03:40 -05:00
Julian Oes	b1fc0ca0d0	mavlink: always forward messages to/from USB Previously, it was not possible to enable forwarding of messages to/from teh USB instance because it does not have a param for it, like the serial instances have. With this commit, we change the default to always set forwarding on for the USB instance as that is likely desired. Signed-off-by: Julian Oes <julian@oes.ch>	2023-01-30 10:46:14 -05:00
Julian Oes	0446292c75	mavlink: always use public method Signed-off-by: Julian Oes <julian@oes.ch>	2023-01-30 10:46:14 -05:00
Julian Oes	7b8cf4913e	mavlink: show forwarding status Signed-off-by: Julian Oes <julian@oes.ch>	2023-01-30 10:46:14 -05:00
Daniel Agar	47215bb625	lib/wind_estimator: symforce codegen remove reserved identifier naming - from the C standard, "All identifiers that begin with an underscore and either an uppercase letter or another underscore are always reserved for any use"	2023-01-30 09:24:32 -05:00
Daniel Agar	efe1d43550	ekf2: symforce codegen remove reserved identifier naming - from the C standard, "All identifiers that begin with an underscore and either an uppercase letter or another underscore are always reserved for any use"	2023-01-30 09:24:32 -05:00
Beat Küng	0687fd2689	lockstep_scheduler: avoid pthread_cond_destroy on at_exit The static object is destroyed on at_exit while threads might still be inside a CS. This can lead to a hanging process. Cleaner would be to gracefully stop the threads. According to https://linux.die.net/man/3/pthread_cond_destroy: Attempting to destroy a condition variable upon which other threads are currently blocked results in undefined behavior.	2023-01-30 11:45:02 +01:00
Hamish Willee	f25abbc80a	Fix magnetometer typo	2023-01-30 10:24:16 +01:00
Daniel Agar	8da993c30e	Update world_magnetic_model to latest Sat Jan 28 11:14:05 UTC 2023 Co-authored-by: PX4 BuildBot <bot@px4.io>	2023-01-28 11:41:03 -05:00
PX4 BuildBot	88038717dc	update all px4board kconfig	2023-01-28 11:39:51 -05:00
Roman Bapst	4646762f9d	Use multiplication instead of division	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	0c735dea2e	AirspeedSelector: use scientific notation for small param Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	d32f400851	WindEstimator: use isAllFinite() Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	a6d14796e4	WindEstimator: add consts, fix float comparison to 0 and use consistently floats in division Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	15335b194a	WindEstimator: use state indexing enum consistently Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
PX4 BuildBot	21ddb04856	Update submodule mavlink to latest Sat Jan 28 00:39:00 UTC 2023 - mavlink in PX4/Firmware (fa65292bb7542cb1f2a4cdaac10738fa17777a6d): https://github.com/mavlink/mavlink/commit/74dee05f0cd121ae27e021d011a04b161c9d0440 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/e3b8756e37cd2cd01ba658461bb4dbffb2fdf914 - Changes: https://github.com/mavlink/mavlink/compare/74dee05f0cd121ae27e021d011a04b161c9d0440...e3b8756e37cd2cd01ba658461bb4dbffb2fdf914 e3b8756e 2023-01-25 olliw42 - update storm32.xml (#1941)	2023-01-27 21:14:07 -05:00