uavcan: cleanup reset CLI and document

drivers/imu/invensense: new IAM-20680HP IMU driver (#21025 )
platforms/nuttx: cmake debug pass GDB path and set RTOS plugin file extension
2026-06-01 07:40:04 +08:00 · 2023-01-31 14:30:48 -05:00 · 2023-01-31 09:33:26 -05:00 · 2023-01-30 20:19:38 -05:00 · 2023-01-30 12:03:40 -05:00 · 2023-01-30 10:46:14 -05:00
67 changed files with 7168 additions and 5926 deletions
@@ -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"
@@ -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
@@ -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
@@ -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
@@ -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}",
@@ -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
@@ -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;
+}
@@ -1254,9 +1254,11 @@ UavcanNode::get_next_dirty_node_id(uint8_t base)
 */
 static void print_usage()
 {
-	PX4_INFO("usage: \n"
-		 "\tuavcan {start|status|stop|shrink|update}\n"
-		 "\t        param [set|get|list|save] <node-id> <name> <value>|reset <node-id>");
+	PX4_INFO_RAW("usage: \n"
+		     "\tuavcan {start|status|stop|shrink|update}\n"
+		     "\t        param [set|get|list|save] <node-id> <name> <value>|reset <node-id>"
+		     "\t        reset <node-id>"
+		    );
 }

 extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
@@ -1317,6 +1319,17 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}

+	if (!std::strcmp(argv[1], "reset")) {
+		int node_arg = 2;
+
+		if (argc < node_arg + 1) {
+			errx(1, "Node id required");
+		}
+
+		int nodeid = atoi(argv[node_arg]);
+		return inst->reset_node(nodeid);
+	}
+
 	/*
 	 * Parameter setting commands
 	 *
@@ -1326,9 +1339,9 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
 	 *  uavcan param set <node> <name> <value>
 	 *
 	 */
-	int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3;
+	if (!std::strcmp(argv[1], "param")) {
+		int node_arg = 3;

-	if (!std::strcmp(argv[1], "param") || node_arg == 2) {
 		if (argc < node_arg + 1) {
 			errx(1, "Node id required");
 		}
@@ -1339,11 +1352,7 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
 			errx(1, "Invalid Node id");
 		}

-		if (node_arg == 2) {
-
-			return inst->reset_node(nodeid);
-
-		} else if (!std::strcmp(argv[2], "list")) {
+		if (!std::strcmp(argv[2], "list")) {

 			return inst->list_params(nodeid);

@@ -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
@@ -548,7 +548,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
@@ -623,9 +623,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;
@@ -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;
@@ -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)

@@ -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);
  }
@@ -60,14 +60,14 @@ void ComputeYaw312InnovVarAndH(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) = _tmp7;
-    _H(2, 0) = _tmp8;
-    _H(3, 0) = _tmp9;
+    _h(0, 0) = _tmp10;
+    _h(1, 0) = _tmp7;
+    _h(2, 0) = _tmp8;
+    _h(3, 0) = _tmp9;
  }
 }  // NOLINT(readability/fn_size)

@@ -60,14 +60,14 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
  }

  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) = -_tmp8;
-    _H(2, 0) = -_tmp7;
-    _H(3, 0) = -_tmp9;
+    _h(0, 0) = -_tmp10;
+    _h(1, 0) = -_tmp8;
+    _h(2, 0) = -_tmp7;
+    _h(3, 0) = -_tmp9;
  }
 }  // NOLINT(readability/fn_size)

@@ -60,14 +60,14 @@ void ComputeYaw321InnovVarAndH(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) = _tmp9;
-    _H(1, 0) = _tmp8;
-    _H(2, 0) = _tmp7;
-    _H(3, 0) = _tmp10;
+    _h(0, 0) = _tmp9;
+    _h(1, 0) = _tmp8;
+    _h(2, 0) = _tmp7;
+    _h(3, 0) = _tmp10;
  }
 }  // NOLINT(readability/fn_size)

@@ -60,14 +60,14 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
  }

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

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

-    _H(0, 0) = -_tmp9;
-    _H(1, 0) = -_tmp8;
-    _H(2, 0) = -_tmp10;
-    _H(3, 0) = -_tmp7;
+    _h(0, 0) = -_tmp9;
+    _h(1, 0) = -_tmp8;
+    _h(2, 0) = -_tmp10;
+    _h(3, 0) = -_tmp7;
  }
 }  // NOLINT(readability/fn_size)

@@ -56,9 +56,9 @@ void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P
  }

  if (H != nullptr) {
-    Scalar& _H = (*H);
+    Scalar& _h = (*H);

-    _H = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2));
+    _h = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2));
  }
 }  // NOLINT(readability/fn_size)

@@ -55,9 +55,9 @@ void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P,
  }

  if (H != nullptr) {
-    Scalar& _H = (*H);
+    Scalar& _h = (*H);

-    _H = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2));
+    _h = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2));
  }
 }  // NOLINT(readability/fn_size)

@@ -477,7 +477,7 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
 /**
 * Type of magnetometer fusion
 *
- * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-component vector. The fuson of magnetomer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field.
+ * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-component vector. The fuson of magnetometer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field.
 * If set to 'Automatic' magnetic heading fusion is used when on-ground and 3-axis magnetic field fusion in-flight with fallback to magnetic heading fusion if there is insufficient motion to make yaw or magnetic field states observable.
 * If set to 'Magnetic heading' magnetic heading fusion is used at all times
 * If set to '3-axis' 3-axis field fusion is used at all times.
@@ -498,7 +498,7 @@ PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
 /**
 * Horizontal acceleration threshold used by automatic selection of magnetometer fusion method.
 *
- * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered horizontal acceleration is greater than this parameter value, then the EKF will use 3-axis magnetomer fusion.
+ * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered horizontal acceleration is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion.
 *
 * @group EKF2
 * @min 0.0
@@ -511,7 +511,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f);
 /**
 * Yaw rate threshold used by automatic selection of magnetometer fusion method.
 *
- * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered yaw rate is greater than this parameter value, then the EKF will use 3-axis magnetomer fusion.
+ * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered yaw rate is greater than this parameter value, then the EKF will use 3-axis magnetometer fusion.
 *
 * @group EKF2
 * @min 0.0
@@ -133,7 +133,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 13090000,0.7,0.0012,-0.014,0.71,-0.0096,0.0068,-0.03,-0.002,0.0018,-3.7e+02,-1.3e-05,-5.9e-05,-4.7e-06,7e-06,1.8e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00023,0.00023,9.5e-05,0.073,0.073,0.025,0.057,0.057,0.052,4.2e-09,4.2e-09,2.3e-09,3.6e-06,3.6e-06,9.4e-07,0,0,0,0,0,0,0,0
 13190000,0.7,0.00099,-0.014,0.71,-0.00038,0.0062,-0.027,0.0044,0.0012,-3.7e+02,-1.2e-05,-5.9e-05,-4.6e-06,5.6e-06,1.9e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00021,0.00021,9.4e-05,0.06,0.06,0.025,0.049,0.049,0.051,4e-09,4e-09,2.2e-09,3.6e-06,3.6e-06,9.1e-07,0,0,0,0,0,0,0,0
 13290000,0.7,0.001,-0.014,0.71,-0.00065,0.007,-0.024,0.0043,0.0018,-3.7e+02,-1.2e-05,-5.9e-05,-4.1e-06,4.3e-06,2.1e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00022,0.00022,9.4e-05,0.068,0.069,0.027,0.057,0.057,0.051,4e-09,4e-09,2.2e-09,3.6e-06,3.6e-06,9.1e-07,0,0,0,0,0,0,0,0
-13390000,0.7,0.00085,-0.014,0.71,0.00026,0.006,-0.02,0.0033,0.0011,-3.7e+02,-1.2e-05,-5.9e-05,-3.6e-06,2.6e-06,2.1e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00021,0.00021,9.4e-05,0.056,0.056,0.026,0.049,0.049,0.05,3.8e-09,3.8e-09,2.1e-09,3.6e-06,3.6e-06,8.8e-07,0,0,0,0,0,0,0,0
+13390000,0.7,0.00085,-0.014,0.71,0.00026,0.006,-0.02,0.0033,0.0011,-3.7e+02,-1.2e-05,-5.9e-05,-3.6e-06,2.7e-06,2.1e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00021,0.00021,9.4e-05,0.056,0.056,0.026,0.049,0.049,0.05,3.8e-09,3.8e-09,2.1e-09,3.6e-06,3.6e-06,8.8e-07,0,0,0,0,0,0,0,0
 13490000,0.7,0.00087,-0.014,0.71,-0.00025,0.0059,-0.019,0.0033,0.0017,-3.7e+02,-1.2e-05,-5.9e-05,-3.1e-06,1.8e-06,2.1e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00022,0.00021,9.4e-05,0.064,0.064,0.028,0.057,0.057,0.05,3.8e-09,3.8e-09,2.1e-09,3.6e-06,3.6e-06,8.7e-07,0,0,0,0,0,0,0,0
 13590000,0.7,0.00081,-0.014,0.71,7.7e-05,0.0061,-0.021,0.0024,0.001,-3.7e+02,-1.2e-05,-5.9e-05,-3.4e-06,1.5e-06,2e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.0002,0.0002,9.4e-05,0.053,0.053,0.028,0.049,0.049,0.05,3.6e-09,3.6e-09,2e-09,3.6e-06,3.6e-06,8.3e-07,0,0,0,0,0,0,0,0
 13690000,0.7,0.00079,-0.014,0.71,0.00075,0.0078,-0.025,0.0024,0.0017,-3.7e+02,-1.1e-05,-5.9e-05,-2.7e-06,1.8e-06,2e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00021,0.00021,9.4e-05,0.061,0.061,0.029,0.056,0.056,0.05,3.6e-09,3.6e-09,2e-09,3.6e-06,3.6e-06,8.3e-07,0,0,0,0,0,0,0,0
@@ -208,14 +208,14 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 20590000,0.71,0.00034,-0.012,0.71,-0.0022,-0.011,0.013,0.0019,-0.0021,-3.7e+02,-1.4e-05,-6e-05,7.1e-06,-1.3e-05,7.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,0.0001,0.0001,7.8e-05,0.024,0.024,0.0094,0.044,0.044,0.04,2e-10,2e-10,4.5e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20690000,0.71,0.00037,-0.012,0.71,-0.0022,-0.012,0.015,0.0016,-0.0032,-3.7e+02,-1.4e-05,-6e-05,7.2e-06,-1.2e-05,7.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,0.0001,0.0001,7.8e-05,0.026,0.026,0.0093,0.049,0.049,0.04,2e-10,2e-10,4.4e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20790000,0.71,0.0004,-0.012,0.71,-0.0033,-0.011,0.015,0.0014,-0.0025,-3.7e+02,-1.4e-05,-6e-05,7.3e-06,-1.1e-05,7.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8e-05,0.0001,0.0001,7.7e-05,0.023,0.023,0.0091,0.043,0.043,0.04,1.8e-10,1.8e-10,4.3e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
-20890000,0.71,0.00039,-0.012,0.71,-0.0037,-0.014,0.014,0.001,-0.0038,-3.7e+02,-1.4e-05,-6e-05,7.5e-06,-1.1e-05,7.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,0.0001,0.0001,7.7e-05,0.025,0.025,0.0091,0.049,0.049,0.04,1.8e-10,1.8e-10,4.3e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
+20890000,0.71,0.00038,-0.012,0.71,-0.0037,-0.014,0.014,0.001,-0.0038,-3.7e+02,-1.4e-05,-6e-05,7.5e-06,-1.1e-05,7.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,0.0001,0.0001,7.7e-05,0.025,0.025,0.0091,0.049,0.049,0.04,1.8e-10,1.8e-10,4.3e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20990000,0.71,0.00039,-0.012,0.71,-0.004,-0.014,0.015,0.0027,-0.0031,-3.7e+02,-1.4e-05,-6e-05,7.5e-06,-8.6e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,9.8e-05,9.8e-05,7.7e-05,0.023,0.023,0.0089,0.043,0.043,0.039,1.7e-10,1.7e-10,4.2e-10,2.9e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
-21090000,0.71,0.00039,-0.012,0.71,-0.0041,-0.017,0.015,0.0023,-0.0047,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,-8.6e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,9.9e-05,9.9e-05,7.7e-05,0.025,0.025,0.0089,0.048,0.048,0.039,1.7e-10,1.7e-10,4.1e-10,2.9e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
+21090000,0.71,0.00039,-0.012,0.71,-0.0041,-0.017,0.015,0.0023,-0.0046,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,-8.6e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,9.9e-05,9.9e-05,7.7e-05,0.025,0.025,0.0089,0.048,0.048,0.039,1.7e-10,1.7e-10,4.1e-10,2.9e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 21190000,0.71,0.00042,-0.012,0.71,-0.0033,-0.016,0.014,0.0037,-0.0038,-3.7e+02,-1.4e-05,-6e-05,7.6e-06,-6.2e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,9.7e-05,9.7e-05,7.7e-05,0.022,0.022,0.0087,0.043,0.043,0.039,1.5e-10,1.6e-10,4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21290000,0.71,0.00046,-0.012,0.71,-0.0039,-0.018,0.016,0.0034,-0.0054,-3.7e+02,-1.4e-05,-6e-05,7.9e-06,-6.2e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,9.8e-05,9.8e-05,7.6e-05,0.024,0.024,0.0086,0.048,0.048,0.039,1.6e-10,1.6e-10,4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21390000,0.71,0.00051,-0.012,0.71,-0.0047,-0.017,0.016,0.0029,-0.0034,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,-2.5e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,9.6e-05,9.6e-05,7.6e-05,0.022,0.022,0.0085,0.043,0.043,0.039,1.4e-10,1.4e-10,3.9e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21490000,0.71,0.00052,-0.012,0.71,-0.0053,-0.018,0.015,0.0023,-0.0052,-3.7e+02,-1.4e-05,-6e-05,7.8e-06,-2.5e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.7e-05,9.6e-05,7.6e-05,0.023,0.023,0.0085,0.048,0.048,0.038,1.4e-10,1.4e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
-21590000,0.71,0.00054,-0.012,0.71,-0.0058,-0.015,0.015,0.0019,-0.0032,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,9.1e-07,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.5e-05,9.5e-05,7.6e-05,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-10,1.3e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
+21590000,0.71,0.00054,-0.012,0.71,-0.0058,-0.015,0.015,0.0019,-0.0032,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,9e-07,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.5e-05,9.5e-05,7.6e-05,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-10,1.3e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21690000,0.71,0.00055,-0.012,0.71,-0.0057,-0.016,0.017,0.0014,-0.0048,-3.7e+02,-1.4e-05,-6e-05,7.8e-06,8.9e-07,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.5e-05,9.5e-05,7.6e-05,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-10,1.3e-10,3.7e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21790000,0.71,0.00057,-0.012,0.71,-0.0063,-0.011,0.015,9.1e-05,-0.00074,-3.7e+02,-1.4e-05,-5.9e-05,7.6e-06,6.1e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.4e-05,9.4e-05,7.5e-05,0.021,0.021,0.0082,0.042,0.042,0.038,1.3e-10,1.3e-10,3.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21890000,0.71,0.00057,-0.012,0.71,-0.0063,-0.012,0.016,-0.00054,-0.0019,-3.7e+02,-1.4e-05,-5.9e-05,7.5e-06,6e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.7e-05,9.4e-05,9.4e-05,7.5e-05,0.022,0.022,0.0082,0.047,0.047,0.038,1.3e-10,1.3e-10,3.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
@@ -310,7 +310,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 30790000,0.71,0.0027,0.0059,0.71,-1.9,-0.98,0.78,-11,-5.1,-3.7e+02,-1.3e-05,-5.7e-05,3.2e-06,-0.00021,-0.0002,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8e-05,7.7e-05,6.2e-05,0.025,0.025,0.009,0.22,0.22,0.037,3.2e-11,3.2e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 30890000,0.71,0.0025,0.0055,0.71,-1.9,-0.97,0.76,-11,-5.2,-3.7e+02,-1.3e-05,-5.7e-05,3.2e-06,-0.00021,-0.00019,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8.1e-05,7.7e-05,6.2e-05,0.026,0.027,0.009,0.24,0.24,0.037,3.2e-11,3.2e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 30990000,0.71,0.0025,0.005,0.71,-1.8,-0.96,0.76,-11,-5.3,-3.7e+02,-1.3e-05,-5.7e-05,3.1e-06,-0.00022,-0.00018,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8e-05,7.7e-05,6.2e-05,0.025,0.025,0.0089,0.23,0.23,0.037,3.2e-11,3.1e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
-31090000,0.71,0.0022,0.0045,0.71,-1.8,-0.96,0.74,-11,-5.4,-3.7e+02,-1.3e-05,-5.7e-05,3e-06,-0.00023,-0.00017,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8.1e-05,7.7e-05,6.2e-05,0.026,0.027,0.009,0.25,0.24,0.037,3.2e-11,3.1e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
+31090000,0.71,0.0022,0.0045,0.71,-1.8,-0.96,0.74,-11,-5.4,-3.7e+02,-1.3e-05,-5.7e-05,3.1e-06,-0.00023,-0.00017,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8.1e-05,7.7e-05,6.2e-05,0.026,0.027,0.009,0.25,0.24,0.037,3.2e-11,3.1e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 31190000,0.71,0.0022,0.0043,0.71,-1.8,-0.95,0.73,-12,-5.5,-3.7e+02,-1.4e-05,-5.7e-05,3e-06,-0.00024,-0.00014,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8.1e-05,7.7e-05,6.1e-05,0.025,0.025,0.0089,0.24,0.24,0.037,3.1e-11,3.1e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 31290000,0.71,0.002,0.0038,0.71,-1.8,-0.94,0.73,-12,-5.6,-3.7e+02,-1.4e-05,-5.7e-05,3e-06,-0.00025,-0.00013,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8.1e-05,7.7e-05,6.1e-05,0.026,0.027,0.0089,0.26,0.25,0.037,3.1e-11,3.1e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 31390000,0.71,0.0018,0.0033,0.71,-1.8,-0.93,0.73,-12,-5.7,-3.7e+02,-1.4e-05,-5.7e-05,2.9e-06,-0.00025,-0.00011,-0.001,0.21,0.0021,0.44,0,0,0,0,0,6.3e-05,8.1e-05,7.6e-05,6.1e-05,0.025,0.025,0.0088,0.25,0.25,0.036,3.1e-11,3.1e-11,1.1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
@@ -320,16 +320,16 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 31790000,0.71,0.0011,0.0011,0.71,-1.7,-0.89,0.72,-13,-6,-3.7e+02,-1.4e-05,-5.7e-05,2.8e-06,-0.00028,-5.1e-05,-0.00098,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.6e-05,6.1e-05,0.025,0.025,0.0087,0.27,0.27,0.037,3e-11,3e-11,1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 31890000,0.71,0.00088,0.00039,0.71,-1.6,-0.88,0.72,-13,-6.1,-3.7e+02,-1.4e-05,-5.7e-05,2.8e-06,-0.00029,-3.8e-05,-0.00097,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.6e-05,6e-05,0.026,0.026,0.0087,0.28,0.28,0.037,3e-11,3e-11,1e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 31990000,0.71,0.00075,-7e-05,0.71,-1.6,-0.87,0.71,-13,-6.2,-3.7e+02,-1.4e-05,-5.7e-05,2.7e-06,-0.0003,-1.9e-05,-0.00097,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.6e-05,6e-05,0.025,0.025,0.0086,0.28,0.28,0.036,3e-11,3e-11,9.9e-11,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
-32090000,0.71,0.00046,-0.00079,0.71,-1.6,-0.86,0.72,-13,-6.3,-3.7e+02,-1.4e-05,-5.7e-05,2.6e-06,-0.0003,-3.9e-06,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.6e-05,6e-05,0.026,0.026,0.0087,0.29,0.29,0.037,3e-11,3e-11,9.9e-11,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
+32090000,0.71,0.00046,-0.00079,0.71,-1.6,-0.86,0.72,-13,-6.3,-3.7e+02,-1.4e-05,-5.7e-05,2.6e-06,-0.0003,-4e-06,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.6e-05,6e-05,0.026,0.026,0.0087,0.29,0.29,0.037,3e-11,3e-11,9.9e-11,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 32190000,0.71,0.00025,-0.0016,0.71,-1.5,-0.85,0.72,-13,-6.4,-3.7e+02,-1.4e-05,-5.7e-05,2.5e-06,-0.00031,1.7e-05,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.5e-05,6e-05,0.025,0.025,0.0086,0.29,0.29,0.036,2.9e-11,2.9e-11,9.8e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32290000,0.71,1.5e-05,-0.0023,0.71,-1.5,-0.84,0.71,-13,-6.4,-3.7e+02,-1.4e-05,-5.7e-05,2.4e-06,-0.00032,3.3e-05,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.5e-05,6e-05,0.026,0.026,0.0086,0.3,0.3,0.037,3e-11,2.9e-11,9.7e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32390000,0.71,-0.00017,-0.0029,0.71,-1.5,-0.83,0.71,-14,-6.5,-3.7e+02,-1.4e-05,-5.7e-05,2.5e-06,-0.00032,4.2e-05,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.5e-05,6e-05,0.025,0.025,0.0085,0.3,0.3,0.037,2.9e-11,2.9e-11,9.6e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32490000,0.71,-0.00029,-0.0032,0.71,-1.4,-0.81,0.72,-14,-6.6,-3.7e+02,-1.4e-05,-5.7e-05,2.5e-06,-0.00033,5.3e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.5e-05,6e-05,0.026,0.026,0.0086,0.31,0.31,0.037,2.9e-11,2.9e-11,9.5e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
-32590000,0.71,-0.00028,-0.0034,0.71,-1.4,-0.8,0.71,-14,-6.7,-3.7e+02,-1.5e-05,-5.7e-05,2.4e-06,-0.00033,6e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.5e-05,6e-05,0.025,0.025,0.0084,0.31,0.31,0.036,2.9e-11,2.9e-11,9.4e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
+32590000,0.71,-0.00029,-0.0034,0.71,-1.4,-0.8,0.71,-14,-6.7,-3.7e+02,-1.5e-05,-5.7e-05,2.4e-06,-0.00033,6e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.5e-05,6e-05,0.025,0.025,0.0084,0.31,0.31,0.036,2.9e-11,2.9e-11,9.4e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32690000,0.71,-0.00033,-0.0035,0.71,-1.4,-0.79,0.71,-14,-6.8,-3.7e+02,-1.5e-05,-5.7e-05,2.4e-06,-0.00033,6.6e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.2e-05,8.1e-05,7.5e-05,5.9e-05,0.026,0.026,0.0085,0.32,0.32,0.036,2.9e-11,2.9e-11,9.3e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32790000,0.71,-0.00029,-0.0034,0.71,-1.4,-0.78,0.71,-14,-6.8,-3.7e+02,-1.5e-05,-5.7e-05,2.3e-06,-0.00034,7.5e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,6.1e-05,8.1e-05,7.4e-05,5.9e-05,0.025,0.025,0.0084,0.32,0.31,0.036,2.9e-11,2.9e-11,9.3e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 32890000,0.71,-0.0002,-0.0034,0.71,-1.3,-0.77,0.71,-14,-6.9,-3.7e+02,-1.5e-05,-5.7e-05,2.2e-06,-0.00034,8.6e-05,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,6.1e-05,8.1e-05,7.4e-05,5.9e-05,0.026,0.026,0.0084,0.33,0.33,0.036,2.9e-11,2.9e-11,9.2e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
-32990000,0.71,-6.8e-05,-0.0033,0.71,-1.3,-0.76,0.7,-15,-7,-3.7e+02,-1.5e-05,-5.7e-05,2.3e-06,-0.00035,9.9e-05,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,6.1e-05,8.1e-05,7.4e-05,5.9e-05,0.025,0.025,0.0083,0.32,0.32,0.036,2.8e-11,2.8e-11,9.1e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
+32990000,0.71,-6.8e-05,-0.0033,0.71,-1.3,-0.77,0.7,-15,-7,-3.7e+02,-1.5e-05,-5.7e-05,2.3e-06,-0.00035,9.9e-05,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,6.1e-05,8.1e-05,7.4e-05,5.9e-05,0.025,0.025,0.0083,0.32,0.32,0.036,2.8e-11,2.8e-11,9.1e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 33090000,0.71,-0.0001,-0.0033,0.71,-1.3,-0.76,0.69,-15,-7.1,-3.7e+02,-1.5e-05,-5.7e-05,2.4e-06,-0.00035,0.0001,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,6.1e-05,8.1e-05,7.4e-05,5.9e-05,0.026,0.026,0.0084,0.34,0.34,0.036,2.8e-11,2.8e-11,9e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 33190000,0.7,0.0033,-0.0024,0.71,-1.3,-0.75,0.64,-15,-7.1,-3.7e+02,-1.5e-05,-5.7e-05,2.4e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,6.1e-05,8.1e-05,7.4e-05,5.9e-05,0.025,0.025,0.0083,0.33,0.33,0.036,2.8e-11,2.8e-11,8.9e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
 33290000,0.65,0.015,-0.0015,0.76,-1.3,-0.73,0.62,-15,-7.2,-3.7e+02,-1.5e-05,-5.7e-05,2.4e-06,-0.00036,0.00012,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,5.9e-05,8e-05,7.4e-05,6.1e-05,0.026,0.026,0.0083,0.35,0.35,0.036,2.8e-11,2.8e-11,8.9e-11,2.7e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
@@ -388,4 +388,4 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 38590000,-0.68,0.0028,-0.0028,0.74,0.01,0.067,-0.037,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,7.3e-06,-0.0021,0.0014,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,5.5e-05,1.8e-05,1.8e-05,5.8e-05,0.043,0.044,0.0071,0.62,0.62,0.031,3e-11,3e-11,6.1e-11,1.5e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0
 38690000,-0.68,0.0028,-0.0028,0.74,0.0062,0.067,-0.03,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,7.4e-06,-0.0021,0.0014,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,5.5e-05,1.8e-05,1.9e-05,5.8e-05,0.046,0.047,0.0072,0.63,0.63,0.031,3e-11,3e-11,6e-11,1.5e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0
 38790000,-0.68,0.0028,-0.0028,0.74,0.00071,0.055,-0.022,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,7.6e-06,-0.0022,0.0014,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,5.4e-05,1.8e-05,1.9e-05,5.8e-05,0.042,0.042,0.0073,0.63,0.63,0.031,3e-11,3e-11,6e-11,1.4e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0
-38890000,-0.68,0.0026,-0.0028,0.74,-0.0092,0.044,0.48,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,7.7e-06,-0.0022,0.0014,-0.001,0.21,0.0021,0.44,0,0,0,0,0,5.4e-05,1.8e-05,1.9e-05,5.8e-05,0.045,0.045,0.0075,0.64,0.64,0.032,3e-11,3e-11,6e-11,1.4e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0
+38890000,-0.67,0.0026,-0.0028,0.74,-0.0092,0.044,0.48,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,7.7e-06,-0.0022,0.0014,-0.001,0.21,0.0021,0.44,0,0,0,0,0,5.4e-05,1.8e-05,1.9e-05,5.8e-05,0.045,0.045,0.0075,0.64,0.64,0.032,3e-11,3e-11,6e-11,1.4e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0
@@ -102,7 +102,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 9990000,0.98,-0.0055,-0.013,0.19,0.017,0.11,-0.00068,0.015,0.19,0.025,-1.6e-05,-5.7e-05,-1.3e-06,-8.4e-10,9.3e-10,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4.1e-06,0.00056,0.00056,0.00011,2.2,2.2,0.013,8.9,8.9,0.049,1.2e-08,1.2e-08,4e-09,4e-06,4e-06,5.7e-08,0,0,0,0,0,0,0,0
 10090000,0.98,-0.0056,-0.013,0.19,0.015,0.1,0.00052,0.015,0.18,0.027,-1.6e-05,-5.7e-05,-1.9e-06,-8.4e-10,9.3e-10,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4.1e-06,0.00053,0.00053,0.00011,2.1,2.1,0.013,9,9,0.048,1.1e-08,1.1e-08,4e-09,4e-06,4e-06,5.5e-08,0,0,0,0,0,0,0,0
 10190000,0.98,-0.0056,-0.013,0.19,0.013,0.1,0.0014,0.017,0.19,0.025,-1.6e-05,-5.7e-05,-2.6e-06,-8.4e-10,9.3e-10,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4.1e-06,0.00055,0.00055,0.00011,2.2,2.2,0.012,9.8,9.8,0.048,1.1e-08,1.1e-08,3.9e-09,4e-06,4e-06,5.3e-08,0,0,0,0,0,0,0,0
-10290000,0.98,-0.0057,-0.013,0.19,0.013,0.097,0.00031,0.016,0.18,0.026,-1.5e-05,-5.7e-05,-2.3e-06,-8.4e-10,9.3e-10,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4.1e-06,0.00052,0.00052,0.00011,2.1,2.1,0.012,9.7,9.7,0.048,1.1e-08,1.1e-08,3.9e-09,4e-06,4e-06,5.1e-08,0,0,0,0,0,0,0,0
+10290000,0.98,-0.0057,-0.013,0.19,0.013,0.097,0.00032,0.016,0.18,0.026,-1.5e-05,-5.7e-05,-2.3e-06,-8.4e-10,9.3e-10,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4.1e-06,0.00052,0.00052,0.00011,2.1,2.1,0.012,9.7,9.7,0.048,1.1e-08,1.1e-08,3.9e-09,4e-06,4e-06,5.1e-08,0,0,0,0,0,0,0,0
 10390000,0.98,-0.0057,-0.012,0.19,0.007,0.0047,-0.0025,0.00076,0.00013,0.027,-1.5e-05,-5.7e-05,-2.1e-06,-1.2e-09,1.2e-09,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4.1e-06,0.00054,0.00054,0.00011,0.25,0.25,0.56,0.25,0.25,0.048,1.1e-08,1.1e-08,3.8e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0
 10490000,0.98,-0.0056,-0.012,0.19,0.0084,0.0066,0.007,0.0015,0.00065,0.032,-1.5e-05,-5.7e-05,-2.6e-06,-1.3e-08,9.5e-09,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4e-06,0.00055,0.00055,0.00011,0.26,0.26,0.55,0.26,0.26,0.057,1.1e-08,1.1e-08,3.8e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0
 10590000,0.98,-0.0056,-0.012,0.19,-0.0012,0.0044,0.013,-0.0012,-0.0055,0.035,-1.5e-05,-5.7e-05,-2.2e-06,3.2e-06,-7.6e-08,-0.0014,0.2,0.002,0.43,0,0,0,0,0,4e-06,0.00056,0.00056,0.00011,0.13,0.13,0.27,0.13,0.13,0.055,1.1e-08,1.1e-08,3.7e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0
@@ -215,10 +215,10 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 21290000,0.98,-0.00055,-0.0072,0.19,-0.049,0.054,-0.63,-0.0062,0.0089,-0.081,-1.4e-05,-5.9e-05,2.2e-06,0.00014,4.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.6e-05,9.4e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.6e-10,1.6e-10,4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21390000,0.98,-0.002,-0.0079,0.19,-0.044,0.05,-0.75,-0.005,0.011,-0.15,-1.4e-05,-5.9e-05,2.2e-06,0.00015,1.5e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.3e-05,9.2e-05,7.1e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-10,1.4e-10,3.9e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0093,0.016,-0.23,-1.4e-05,-5.9e-05,2.3e-06,0.00015,4.3e-07,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.4e-05,9.3e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-10,1.4e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
-21590000,0.98,-0.0033,-0.0083,0.19,-0.031,0.043,-1,-0.0079,0.017,-0.32,-1.4e-05,-5.9e-05,2.3e-06,0.00015,8.4e-09,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.2e-05,9e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-10,1.3e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
+21590000,0.98,-0.0033,-0.0083,0.19,-0.031,0.043,-1,-0.0079,0.017,-0.32,-1.4e-05,-5.9e-05,2.3e-06,0.00015,9.1e-09,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.2e-05,9e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-10,1.3e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21690000,0.98,-0.0037,-0.0082,0.19,-0.029,0.039,-1.1,-0.011,0.021,-0.43,-1.4e-05,-5.9e-05,2.5e-06,0.00015,-1.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.2e-05,9.1e-05,7e-05,0.025,0.025,0.0083,0.048,0.048,0.036,1.3e-10,1.3e-10,3.7e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21790000,0.98,-0.004,-0.0084,0.19,-0.021,0.033,-1.3,-0.0038,0.018,-0.55,-1.4e-05,-5.8e-05,2.7e-06,0.00015,-3.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9e-05,8.8e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
-21890000,0.98,-0.0043,-0.0085,0.19,-0.018,0.028,-1.4,-0.0057,0.021,-0.69,-1.4e-05,-5.8e-05,2.5e-06,0.00016,-5.1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9e-05,8.9e-05,6.9e-05,0.025,0.025,0.0082,0.048,0.048,0.036,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
+21890000,0.98,-0.0043,-0.0085,0.19,-0.018,0.029,-1.4,-0.0057,0.021,-0.69,-1.4e-05,-5.8e-05,2.5e-06,0.00016,-5.1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9e-05,8.9e-05,6.9e-05,0.025,0.025,0.0082,0.048,0.048,0.036,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21990000,0.98,-0.005,-0.0088,0.19,-0.014,0.023,-1.4,-0.00033,0.017,-0.83,-1.4e-05,-5.8e-05,2.6e-06,0.00015,-2.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.5e-06,8.8e-05,8.6e-05,6.9e-05,0.022,0.022,0.0082,0.043,0.043,0.036,1.2e-10,1.2e-10,3.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 22090000,0.98,-0.0058,-0.0096,0.19,-0.012,0.019,-1.4,-0.0017,0.019,-0.97,-1.4e-05,-5.8e-05,2.8e-06,0.00015,-4.1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.5e-06,8.8e-05,8.7e-05,6.9e-05,0.024,0.024,0.0082,0.048,0.048,0.036,1.2e-10,1.2e-10,3.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 22190000,0.98,-0.0062,-0.0099,0.19,-0.0035,0.013,-1.4,0.0061,0.013,-1.1,-1.4e-05,-5.8e-05,2.9e-06,0.00015,-3.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.5e-06,8.6e-05,8.5e-05,6.8e-05,0.021,0.021,0.0081,0.043,0.043,0.036,1.1e-10,1.1e-10,3.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -257,7 +257,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 25490000,0.98,-0.011,-0.012,0.18,0.019,-0.044,0.027,0.18,-0.11,-3.5,-1.5e-05,-5.8e-05,2.9e-06,0.00018,-7.3e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.8e-05,0.016,0.016,0.0081,0.043,0.043,0.035,5.2e-11,5.2e-11,2.1e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25590000,0.98,-0.012,-0.012,0.18,0.014,-0.04,0.028,0.18,-0.098,-3.5,-1.5e-05,-5.8e-05,2.8e-06,0.00018,-7.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.8e-05,0.014,0.014,0.008,0.039,0.039,0.035,5e-11,5e-11,2.1e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25690000,0.98,-0.011,-0.011,0.18,0.013,-0.039,0.017,0.18,-0.1,-3.5,-1.5e-05,-5.8e-05,2.8e-06,0.00018,-7.7e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.8e-05,0.015,0.015,0.0081,0.043,0.043,0.035,5e-11,5e-11,2e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
-25790000,0.98,-0.011,-0.011,0.18,0.002,-0.03,0.017,0.17,-0.092,-3.5,-1.6e-05,-5.8e-05,2.7e-06,0.00018,-8.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.7e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.8e-11,4.8e-11,2e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
+25790000,0.98,-0.011,-0.011,0.18,0.002,-0.03,0.017,0.17,-0.092,-3.5,-1.6e-05,-5.8e-05,2.8e-06,0.00018,-8.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.7e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.8e-11,4.8e-11,2e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25890000,0.98,-0.011,-0.011,0.18,-0.0037,-0.028,0.019,0.17,-0.095,-3.5,-1.6e-05,-5.8e-05,2.5e-06,0.00018,-8.2e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.7e-05,0.015,0.015,0.0081,0.043,0.043,0.036,4.8e-11,4.8e-11,2e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 25990000,0.98,-0.011,-0.011,0.18,-0.013,-0.022,0.013,0.16,-0.086,-3.5,-1.6e-05,-5.8e-05,2.5e-06,0.00019,-8.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.7e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.7e-11,4.7e-11,1.9e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 26090000,0.98,-0.011,-0.011,0.18,-0.018,-0.021,0.011,0.16,-0.088,-3.5,-1.6e-05,-5.8e-05,2.5e-06,0.00019,-8.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,2.1e-06,7.9e-05,7.8e-05,5.7e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.7e-11,4.7e-11,1.9e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -303,12 +303,12 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 30090000,0.98,-0.0061,-0.013,0.18,-0.069,0.053,0.77,-0.075,0.05,-1.5,-1.4e-05,-5.8e-05,1.7e-06,0.0002,-0.00011,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.8e-06,8.3e-05,8.1e-05,4.8e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-11,3.2e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30190000,0.98,-0.0061,-0.013,0.18,-0.063,0.05,0.77,-0.068,0.048,-1.4,-1.4e-05,-5.7e-05,1.6e-06,0.00021,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.8e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30290000,0.98,-0.0061,-0.013,0.18,-0.062,0.05,0.77,-0.074,0.053,-1.4,-1.4e-05,-5.7e-05,1.6e-06,0.00021,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.3e-05,8.1e-05,4.8e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-30390000,0.98,-0.0061,-0.013,0.19,-0.055,0.044,0.76,-0.066,0.049,-1.3,-1.4e-05,-5.7e-05,1.7e-06,0.00021,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+30390000,0.98,-0.0061,-0.013,0.18,-0.055,0.044,0.76,-0.066,0.049,-1.3,-1.4e-05,-5.7e-05,1.7e-06,0.00021,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30490000,0.98,-0.0061,-0.013,0.18,-0.057,0.044,0.77,-0.072,0.054,-1.2,-1.4e-05,-5.7e-05,1.8e-06,0.00021,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.3e-05,8.1e-05,4.8e-05,0.014,0.014,0.008,0.041,0.041,0.036,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30590000,0.98,-0.0065,-0.014,0.18,-0.053,0.041,0.76,-0.065,0.05,-1.2,-1.4e-05,-5.7e-05,1.9e-06,0.00021,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30690000,0.98,-0.0068,-0.014,0.18,-0.051,0.04,0.76,-0.07,0.054,-1.1,-1.4e-05,-5.7e-05,1.9e-06,0.00021,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.041,0.041,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30790000,0.98,-0.0065,-0.013,0.18,-0.044,0.035,0.76,-0.063,0.052,-1,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-30890000,0.98,-0.0059,-0.013,0.18,-0.044,0.032,0.76,-0.067,0.056,-0.95,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.041,0.04,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+30890000,0.98,-0.0059,-0.013,0.18,-0.044,0.032,0.76,-0.067,0.056,-0.95,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.041,0.041,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 30990000,0.98,-0.0061,-0.013,0.18,-0.037,0.026,0.76,-0.057,0.049,-0.88,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 31090000,0.98,-0.0062,-0.013,0.18,-0.035,0.025,0.76,-0.061,0.051,-0.81,-1.4e-05,-5.7e-05,1.8e-06,0.00022,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.036,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 31190000,0.98,-0.0064,-0.013,0.18,-0.031,0.021,0.76,-0.052,0.046,-0.74,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.7e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
@@ -336,7 +336,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 33390000,0.98,-0.0082,-0.013,0.18,0.014,-0.066,-0.12,0.059,-0.014,0.0021,-1.4e-05,-5.6e-05,2.3e-06,0.00025,-0.00017,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.6e-06,6.1e-05,6e-05,4.3e-05,0.035,0.035,0.0062,0.039,0.039,0.034,2.6e-11,2.6e-11,8.9e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 33490000,0.98,-0.0082,-0.013,0.18,0.0093,-0.067,-0.12,0.06,-0.021,-0.0072,-1.4e-05,-5.6e-05,2.3e-06,0.00025,-0.00017,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.6e-06,6.1e-05,6e-05,4.3e-05,0.042,0.042,0.0061,0.044,0.044,0.034,2.6e-11,2.6e-11,8.8e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 33590000,0.98,-0.0078,-0.013,0.18,0.0055,-0.058,-0.11,0.063,-0.017,-0.014,-1.4e-05,-5.6e-05,2.4e-06,0.00024,-0.0002,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.6e-06,5.3e-05,5.3e-05,4.3e-05,0.04,0.041,0.006,0.04,0.04,0.034,2.6e-11,2.6e-11,8.7e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
-33690000,0.98,-0.0078,-0.013,0.18,0.00077,-0.058,-0.11,0.063,-0.023,-0.022,-1.4e-05,-5.6e-05,2.4e-06,0.00024,-0.0002,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.6e-06,5.4e-05,5.3e-05,4.3e-05,0.048,0.048,0.0059,0.046,0.046,0.034,2.6e-11,2.6e-11,8.6e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
+33690000,0.98,-0.0078,-0.013,0.18,0.00076,-0.058,-0.11,0.063,-0.023,-0.022,-1.4e-05,-5.6e-05,2.4e-06,0.00024,-0.0002,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.6e-06,5.4e-05,5.3e-05,4.3e-05,0.048,0.048,0.0059,0.046,0.046,0.034,2.6e-11,2.6e-11,8.6e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0
 33790000,0.98,-0.0075,-0.013,0.18,-0.0024,-0.047,-0.11,0.067,-0.018,-0.028,-1.4e-05,-5.6e-05,2.3e-06,0.00023,-0.00022,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.6e-06,4.7e-05,4.6e-05,4.3e-05,0.045,0.045,0.0059,0.041,0.041,0.034,2.6e-11,2.6e-11,8.6e-11,2.6e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 33890000,0.98,-0.0076,-0.013,0.18,-0.0066,-0.045,-0.11,0.066,-0.023,-0.034,-1.4e-05,-5.6e-05,2.4e-06,0.00023,-0.00022,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.5e-06,4.7e-05,4.7e-05,4.3e-05,0.052,0.053,0.0058,0.047,0.047,0.033,2.6e-11,2.6e-11,8.5e-11,2.6e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0
 33990000,0.98,-0.0073,-0.013,0.18,-0.0062,-0.031,-0.1,0.069,-0.015,-0.037,-1.4e-05,-5.6e-05,2.3e-06,0.00021,-0.00025,-0.0011,0.2,0.002,0.43,0,0,0,0,0,1.5e-06,4.1e-05,4.1e-05,4.3e-05,0.047,0.047,0.0058,0.042,0.042,0.033,2.6e-11,2.6e-11,8.4e-11,2.5e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0
@@ -122,7 +122,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0);
 * 3 : Topics for system identification (high rate actuator control and IMU data)
 * 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators)
 * 5 : Debugging topics (debug_*.msg topics, for custom code)
- * 6 : Topics for sensor comparison (low rate raw IMU, Baro and Magnetomer data)
+ * 6 : Topics for sensor comparison (low rate raw IMU, Baro and magnetometer data)
 * 7 : Topics for computer vision and collision avoidance
 * 8 : Raw FIFO high-rate IMU (Gyro)
 * 9 : Raw FIFO high-rate IMU (Accel)
@@ -476,7 +476,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	LockGuard lg{mavlink_module_mutex};

 	for (Mavlink *inst : mavlink_module_instances) {
-		if (inst && (inst != self) && (inst->_forwarding_on)) {
+		if (inst && (inst != self) && (inst->get_forwarding_on())) {
 			// Pass message only if target component was seen before
 			if (inst->_receiver.component_was_seen(target_system_id, target_component_id)) {
 				inst->pass_message(msg);
@@ -2119,6 +2119,9 @@ Mavlink::task_main(int argc, char *argv[])
 		_ftp_on = true;
 		_is_usb_uart = true;

+		// Always forward messages to/from the USB instance.
+		_forwarding_on = true;
+
 		set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
 	}

@@ -2183,7 +2186,7 @@ Mavlink::task_main(int argc, char *argv[])
 	pthread_mutex_init(&_radio_status_mutex, nullptr);

 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
-	if (_forwarding_on) {
+	if (get_forwarding_on()) {
 		/* initialize message buffer if multiplexing is on.
 		 * make space for two messages plus off-by-one space as we use the empty element
 		 * marker ring buffer approach.
@@ -2537,7 +2540,7 @@ Mavlink::task_main(int argc, char *argv[])
 		_events.update(t);

 		/* pass messages from other UARTs */
-		if (_forwarding_on) {
+		if (get_forwarding_on()) {

 			bool is_part;
 			uint8_t *read_ptr;
@@ -2629,7 +2632,7 @@ Mavlink::task_main(int argc, char *argv[])
 		_socket_fd = -1;
 	}

-	if (_forwarding_on) {
+	if (get_forwarding_on()) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
@@ -2949,6 +2952,7 @@ Mavlink::display_status()
 	       _ftp_on ? "YES" : "NO",
 	       _transmitting_enabled ? "YES" : "NO");
 	printf("\tmode: %s\n", mavlink_mode_str(_mode));
+	printf("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off");
 	printf("\tMAVLink version: %" PRId32 "\n", _protocol_version);

 	printf("\ttransport protocol: ");
Author	SHA1	Message	Date
Daniel Agar	622c833768	uavcan: cleanup reset CLI and document	2023-01-31 14:30:48 -05: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