drivers/dshot: allow min throttle 0

Kconfig

@@ -73,13 +73,6 @@ 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"

boards/modalai/voxl2-slpi/default.px4board

@@ -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

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

@@ -414,7 +414,7 @@ bool ICM42688P::Configure()
 	return success;
 }
 
-static bool interrupt_debug = false;
+static bool interrupt_debug = true;
 static uint32_t interrupt_debug_count = 0;
 static const uint32_t interrupt_debug_trigger = 800;
 static hrt_abstime last_interrupt_time = 0;

boards/modalai/voxl2/default.px4board

@@ -17,4 +17,3 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
-CONFIG_BOARD_ROOTFSDIR="/data/px4"

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

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

msg/EstimatorEventFlags.msg

@@ -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 magnetometer data
+bool stopping_mag_use                   #  8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer 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 magnetometer 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 magnetomer 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

msg/EstimatorStatus.msg

@@ -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 magnetometer has been declared faulty and is no longer being used
+uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetomer 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

msg/FailsafeFlags.msg

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

platforms/common/include/px4_platform_common/defines.h

@@ -40,8 +40,6 @@
 #pragma once
 
 #include <sys/ioctl.h>
-#include <px4_boardconfig.h>
-
 
 /****************************************************************************
  * Defines for all platforms.
@@ -97,7 +95,7 @@ __BEGIN_DECLS
 extern long PX4_TICKS_PER_SEC;
 __END_DECLS
 
-#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR
+#define PX4_ROOTFSDIR "."
 
 #define PX4_STORAGEDIR PX4_ROOTFSDIR
 

platforms/nuttx/CMakeLists.txt

@@ -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.so)
+		set(JLINK_RTOS_PATH ${NUTTX_DIR}/tools/jlink-nuttx)
 	else()
 		set(JLINK_RTOS_PATH "")
 	endif()

platforms/nuttx/Debug/launch.json.in

@@ -3,7 +3,6 @@
     "configurations": [
         {
             "name": "jlink (@PX4_BOARD@)",
-            "gdbPath": "@CMAKE_GDB@",
             "device": "@DEBUG_DEVICE@",
             "svdFile": "@DEBUG_SVD_FILE_PATH@",
             "rtos": "@JLINK_RTOS_PATH@",
@@ -25,7 +24,6 @@
         },
         {
             "name": "stlink (@PX4_BOARD@)",
-            "gdbPath": "@CMAKE_GDB@",
             "device": "@DEBUG_DEVICE@",
             "svdFile": "@DEBUG_SVD_FILE_PATH@",
             "executable": "${command:cmake.launchTargetPath}",
@@ -44,7 +42,6 @@
         },
         {
             "name": "blackmagic (@PX4_BOARD@)",
-            "gdbPath": "@CMAKE_GDB@",
             "device": "@DEBUG_DEVICE@",
             "svdFile": "@DEBUG_SVD_FILE_PATH@",
             "executable": "${command:cmake.launchTargetPath}",

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

@@ -53,7 +53,7 @@
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
-static LockstepScheduler lockstep_scheduler {true};
+static LockstepScheduler lockstep_scheduler {};
 #endif
 
 // Intervals in usec

platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h

@@ -46,7 +46,7 @@
 class LockstepComponents
 {
 public:
-	LockstepComponents(bool no_cleanup_on_destroy = false);
+	LockstepComponents();
 	~LockstepComponents();
 
 	/**
@@ -69,7 +69,6 @@ public:
 	void wait_for_components();
 
 private:
-	const bool _no_cleanup_on_destroy;
 
 	px4_sem_t _components_sem;
 

platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h

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

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp

@@ -42,19 +42,14 @@
 #include <px4_platform_common/tasks.h>
 #include <limits.h>
 
-LockstepComponents::LockstepComponents(bool no_cleanup_on_destroy)
-	: _no_cleanup_on_destroy(no_cleanup_on_destroy)
+LockstepComponents::LockstepComponents()
 {
 	px4_sem_init(&_components_sem, 0, 0);
 }
 
 LockstepComponents::~LockstepComponents()
 {
-	// 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);
-	}
+	px4_sem_destroy(&_components_sem);
 }
 
 int LockstepComponents::register_component()

platforms/qurt/include/qurt_log.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- * Copyright (C) 2022-2023 ModalAI, Inc. All rights reserved.
+ * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,9 +39,7 @@
 
 __BEGIN_DECLS
 
-extern void qurt_log_to_apps(int level, const char *message);
-
-// Defining hap_debug
+//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,
@@ -53,8 +51,6 @@ 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

platforms/qurt/src/px4/CMakeLists.txt

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

platforms/qurt/src/px4/qurt_log.cpp

@@ -1,48 +0,0 @@
-/****************************************************************************
- *
- *   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); }
-	}
-}

src/drivers/drv_sensor.h

@@ -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

src/drivers/dshot/DShot.h

@@ -55,7 +55,7 @@ static constexpr unsigned int DSHOT600  =  600000u;
 static constexpr unsigned int DSHOT1200 = 1200000u;
 
 static constexpr int DSHOT_DISARM_VALUE = 0;
-static constexpr int DSHOT_MIN_THROTTLE = 1;
+static constexpr int DSHOT_MIN_THROTTLE = 0;
 static constexpr int DSHOT_MAX_THROTTLE = 1999;
 
 class DShot final : public ModuleBase<DShot>, public OutputModuleInterface

src/drivers/imu/invensense/iam20680hp/CMakeLists.txt

@@ -1,47 +0,0 @@
-############################################################################
-#
-#   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
-	)

src/drivers/imu/invensense/iam20680hp/IAM20680HP.cpp

@@ -1,641 +0,0 @@
-/****************************************************************************
- *
- *   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);
-	}
-}

src/drivers/imu/invensense/iam20680hp/IAM20680HP.hpp

@@ -1,172 +0,0 @@
-/****************************************************************************
- *
- *   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 },
-	};
-};

src/drivers/imu/invensense/iam20680hp/InvenSense_IAM20680HP_registers.hpp

@@ -1,193 +0,0 @@
-/****************************************************************************
- *
- *   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

src/drivers/imu/invensense/iam20680hp/Kconfig

@@ -1,5 +0,0 @@
-menuconfig DRIVERS_IMU_INVENSENSE_IAM20680HP
-	bool "iam20680hp"
-	default n
-	---help---
-		Enable support for iam20680hp

src/drivers/imu/invensense/iam20680hp/iam20680hp_main.cpp

@@ -1,88 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}

src/lib/CMakeLists.txt

@@ -53,7 +53,6 @@ add_subdirectory(l1 EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
-add_subdirectory(mission EXCLUDE_FROM_ALL)
 add_subdirectory(mixer_module EXCLUDE_FROM_ALL)
 add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)

src/lib/mission/CMakeLists.txt

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

src/lib/mission/planned_mission_interface.cpp

@@ -1,595 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file planned_mission_interface.cpp
- *
- */
-
-#include "planned_mission_interface.h"
-
-#include <stdlib.h>
-
-#include "dataman/dataman.h"
-#include "lib/geo/geo.h"
-#include "modules/navigator/mission_block.h"
-
-void PlannedMissionInterface::update()
-{
-	_home_pos_sub.update();
-
-	if (!_is_land_start_item_searched && _home_pos_sub.get().valid_alt) {
-		findLandStartItem();
-		_is_land_start_item_searched = true;
-	}
-
-	if (_mission_sub.updated()) {
-		mission_s new_mission;
-		_mission_sub.update(&new_mission);
-
-		if (isMissionValid(new_mission)) {
-			/* Check if it was updated externally*/
-			if (new_mission.timestamp > _mission.timestamp) {
-				bool mission_waypoints_changed{checkMissionWaypointsChanged(_mission, new_mission)};
-				_mission = new_mission;
-
-				if (goToItem(new_mission.current_seq, true) == EXIT_SUCCESS) {
-					if (_home_pos_sub.get().valid_alt) {
-						findLandStartItem();
-
-					} else {
-						_is_land_start_item_searched = false;
-					}
-
-					onMissionUpdate(mission_waypoints_changed);
-				}
-			}
-		}
-	}
-}
-
-void PlannedMissionInterface::getPreviousPositionItems(int32_t start_index, struct mission_item_s items[],
-		size_t &num_found_items, uint8_t max_num_items) const
-{
-	num_found_items = 0u;
-
-	int32_t next_mission_index{start_index};
-
-	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
-		if (next_mission_index < 0) {
-			break;
-		}
-
-		mission_item_s next_mission_item;
-		bool found_next_item{false};
-
-		do {
-			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == EXIT_SUCCESS;
-			next_mission_index--;
-		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
-
-		if (found_next_item) {
-			items[item_idx] = next_mission_item;
-			num_found_items = item_idx + 1;
-
-		} else {
-			break;
-		}
-	}
-}
-
-void PlannedMissionInterface::getNextPositionItems(int32_t start_index, struct mission_item_s items[],
-		size_t &num_found_items, uint8_t max_num_items) const
-{
-	// Make sure vector does not contain any preexisting elements.
-	num_found_items = 0u;
-
-	int32_t next_mission_index{start_index};
-
-	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
-		if (next_mission_index >= _mission.count) {
-			break;
-		}
-
-		mission_item_s next_mission_item;
-		bool found_next_item{false};
-
-		do {
-			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == EXIT_SUCCESS;
-			next_mission_index++;
-		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
-
-		if (found_next_item) {
-			items[item_idx] = next_mission_item;
-			num_found_items = item_idx + 1;
-
-		} else {
-			break;
-		}
-	}
-}
-
-int PlannedMissionInterface::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump,
-		bool write_jumps, bool mission_direction_backward) const
-{
-	if (mission_index >= _mission.count || mission_index < 0) {
-		return EXIT_FAILURE;
-	}
-
-	int32_t new_mission_index{mission_index};
-	mission_item_s new_mission;
-
-	for (uint16_t jump_count = 0u; jump_count < _max_jump_iteraion; jump_count++) {
-		if (readMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
-			PX4_ERR("Could not read next item.");
-			return EXIT_FAILURE;
-		}
-
-		if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) {
-			if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) {
-				PX4_ERR("Do Jump mission index is out of bounds.");
-				return EXIT_FAILURE;
-			}
-
-			if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) {
-				if (write_jumps) {
-					new_mission.do_jump_current_count++;
-
-					if (writeMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
-						PX4_ERR("Could not update jump count on mission item.");
-						// Still continue searching for next non jump item.
-					}
-				}
-
-				new_mission_index = new_mission.do_jump_mission_index;
-
-			} else {
-				if (mission_direction_backward) {
-					new_mission_index--;
-
-				} else {
-					new_mission_index++;
-				}
-			}
-
-		} else {
-			break;
-		}
-	}
-
-	mission_index = new_mission_index;
-	mission = new_mission;
-
-	return EXIT_SUCCESS;
-}
-
-bool PlannedMissionInterface::hasMissionLandStart() const
-{
-	return (_land_start_index != _invalid_index) && (_land_start_index < _mission.count);
-}
-
-int PlannedMissionInterface::goToNextItem(bool execute_jump)
-{
-	if (_mission.current_seq + 1 >= (_mission.count)) {
-		return EXIT_FAILURE;
-	}
-
-	return goToItem(_mission.current_seq + 1, execute_jump);
-}
-
-int PlannedMissionInterface::goToPreviousItem(bool execute_jump)
-{
-	if (_mission.current_seq <= 0) {
-		return EXIT_FAILURE;
-	}
-
-	return goToItem(_mission.current_seq - 1, execute_jump, true);
-}
-
-int PlannedMissionInterface::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward)
-{
-	mission_item_s mission_item;
-
-	if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == EXIT_SUCCESS) {
-		if (setMissionIndex(index) == EXIT_SUCCESS) {
-			_current_planned_mission_item = mission_item;
-
-		} else {
-			return EXIT_FAILURE;
-		}
-
-	} else {
-		return EXIT_FAILURE;
-	}
-
-	return EXIT_SUCCESS;
-}
-
-int PlannedMissionInterface::goToPreviousPositionItem(bool execute_jump)
-{
-	do {
-		if (goToPreviousItem(execute_jump) != EXIT_SUCCESS) {
-			return EXIT_FAILURE;
-		}
-	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
-
-	return EXIT_SUCCESS;
-}
-
-int PlannedMissionInterface::goToNextPositionItem(bool execute_jump)
-{
-	do {
-		if (goToNextItem(execute_jump) != EXIT_SUCCESS) {
-			return EXIT_FAILURE;
-		}
-	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
-
-	return EXIT_SUCCESS;
-}
-
-int PlannedMissionInterface::setMissionIndex(int32_t index)
-{
-	// Nothing to do if it is already at the current index.
-	if (index == _mission.current_seq) {
-		return EXIT_SUCCESS;
-	}
-
-	mission_s mission{_mission};
-	mission.current_seq = index;
-	mission.timestamp = hrt_absolute_time();
-
-	if (writeMission(mission) == EXIT_SUCCESS) {
-		_mission = mission;
-		return EXIT_SUCCESS;
-
-	} else {
-		return EXIT_FAILURE;
-	}
-}
-
-int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt,
-		const vehicle_status_s &vehicle_status)
-{
-	int32_t min_dist_index(-1);
-	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
-
-	for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) {
-		mission_item_s mission;
-
-		if (readMissionItem(mission, mission_item_index) != EXIT_SUCCESS) {
-			PX4_ERR("Could not set mission closest to position.");
-			return EXIT_FAILURE;
-		}
-
-		if (MissionBlock::item_contains_position(mission)) {
-			// do not consider land waypoints for a fw
-			if (!((mission.nav_cmd == NAV_CMD_LAND) &&
-			      (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
-			      (!vehicle_status.is_vtol))) {
-				float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon,
-						MissionBlock::get_absolute_altitude_for_item(mission, home_alt),
-						lat,
-						lon,
-						alt,
-						&dist_xy, &dist_z);
-
-				if (dist < min_dist) {
-					min_dist = dist;
-					min_dist_index = mission_item_index;
-				}
-			}
-		}
-	}
-
-	return goToItem(min_dist_index, false);
-}
-
-int PlannedMissionInterface::goToMissionLandStart()
-{
-	if (!hasMissionLandStart()) {
-		return EXIT_FAILURE;
-	}
-
-	return goToItem(_land_start_index, false);
-}
-
-int PlannedMissionInterface::initMission()
-{
-	mission_s mission;
-	bool ret_val{EXIT_FAILURE};
-
-	if (readMission(mission) == EXIT_SUCCESS) {
-		_mission = mission;
-
-		if (goToItem(mission.current_seq, true) == EXIT_SUCCESS) {
-			findLandStartItem();
-			ret_val = EXIT_SUCCESS;
-		}
-
-	} else {
-		resetMission();
-	}
-
-	_mission_pub.advertise();
-
-	return ret_val;
-}
-
-void PlannedMissionInterface::resetMission()
-{
-	/* we do not need to reset mission if there is none.*/
-	if (_mission.count == 0u) {
-		return;
-	}
-
-	/* Set a new mission*/
-	mission_s new_mission{.timestamp = hrt_absolute_time(),
-			      .current_seq = 0,
-			      .count = 0u,
-			      .dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0};
-
-	if (writeMission(new_mission) == EXIT_SUCCESS) {
-		_mission = new_mission;
-
-	} else {
-		PX4_ERR("Mission Initialization failed.");
-	}
-}
-
-void PlannedMissionInterface::resetMissionJumpCounter()
-{
-	for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) {
-		mission_item_s mission_item;
-
-		if (readMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
-			PX4_ERR("Could not read mission item for jump count reset.");
-			break;
-		}
-
-		if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) {
-			mission_item.do_jump_current_count = 0u;
-
-			if (writeMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
-				PX4_ERR("Could not write mission item for jump count reset.");
-				break;
-			}
-		}
-	}
-}
-
-int PlannedMissionInterface::writeMission(mission_s &mission)
-{
-	int ret_val{EXIT_SUCCESS};
-
-	if (!isMissionValid(mission)) {
-		return EXIT_FAILURE;
-	}
-
-	/* lock MISSION_STATE item */
-	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
-		return EXIT_FAILURE;
-	}
-
-	if (dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
-		PX4_ERR("Can't save mission state");
-		ret_val = EXIT_FAILURE;
-
-	} else {
-		_mission_pub.publish(mission);
-	}
-
-	dm_unlock(DM_KEY_MISSION_STATE);
-
-	return ret_val;
-}
-
-int PlannedMissionInterface::readMission(mission_s &read_mission) const
-{
-	int ret_val{EXIT_SUCCESS};
-
-	/* lock MISSION_STATE item */
-	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM lock failed.");
-		return EXIT_FAILURE;
-	}
-
-	mission_s mission;
-
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
-		PX4_ERR("Can't read mission state.");
-		ret_val = EXIT_FAILURE;
-
-	} else {
-		if (isMissionValid(mission)) {
-			read_mission = mission;
-
-		} else {
-			ret_val = EXIT_FAILURE;
-		}
-	}
-
-	dm_unlock(DM_KEY_MISSION_STATE);
-
-	return ret_val;
-}
-
-int PlannedMissionInterface::readMissionItem(mission_item_s &read_mission_item, size_t index) const
-{
-	int ret_val{EXIT_SUCCESS};
-
-	if (index >= _mission.count) {
-		return EXIT_FAILURE;
-	}
-
-	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
-
-	/* lock current mission item */
-	/*int dm_lock_ret = dm_lock(current_dm_item);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM lock failed.");
-		return EXIT_FAILURE;
-	}*/
-
-	mission_item_s mission_item;
-
-	if (dm_read(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
-		PX4_ERR("Can't read mission item from DM.");
-		ret_val = EXIT_FAILURE;
-
-	} else {
-		read_mission_item = mission_item;
-	}
-
-	//dm_unlock(current_dm_item);
-
-	return ret_val;
-}
-
-int PlannedMissionInterface::writeMissionItem(const mission_item_s &mission_item, size_t index) const
-{
-	int ret_val{EXIT_SUCCESS};
-
-	if (index >= _mission.count) {
-		return EXIT_FAILURE;
-	}
-
-	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
-
-	/* lock current mission item */
-	/*int dm_lock_ret = dm_lock(current_dm_item);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM lock failed.");
-		return EXIT_FAILURE;
-	}*/
-
-	if (dm_write(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
-		PX4_ERR("Can't write mission item to DM.");
-		ret_val = EXIT_FAILURE;
-	}
-
-	//dm_unlock(current_dm_item);
-
-	return ret_val;
-}
-
-bool PlannedMissionInterface::isMissionValid(mission_s &mission) const
-{
-	bool ret_val{false};
-
-	if ((mission.current_seq < mission.count) &&
-	    (mission.current_seq >= 0) &&
-	    (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) &&
-	    (mission.timestamp != 0u)) {
-		ret_val = true;
-
-	}
-
-	return ret_val;
-}
-
-void PlannedMissionInterface::findLandStartItem()
-{
-	_land_start_index = _invalid_index;
-	_land_index = _invalid_index;
-
-	for (size_t mission_item_index = 1u; mission_item_index < _mission.count; mission_item_index++) {
-		mission_item_s mission;
-
-		if (readMissionItem(mission, mission_item_index) == EXIT_SUCCESS) {
-			if (mission.nav_cmd == NAV_CMD_DO_LAND_START) {
-				_land_start_index = mission_item_index;
-			}
-
-			if ((mission.nav_cmd == NAV_CMD_VTOL_LAND) || (mission.nav_cmd == NAV_CMD_LAND)) {
-				_land_index = mission_item_index;
-				break;
-			}
-
-		} else {
-			break;
-		}
-	}
-
-	if (_land_start_index != _invalid_index) {
-		mission_item_s mission;
-		size_t num_found_item{0u};
-		getNextPositionItems(_land_start_index, &mission, num_found_item, 1u);
-
-		if (num_found_item == 1u) {
-			_land_start_pos.lat = mission.lat;
-			_land_start_pos.lon = mission.lon;
-			_land_start_pos.alt = mission.altitude_is_relative ?	mission.altitude +
-					      _home_pos_sub.get().alt : mission.altitude;
-
-		} else {
-			PX4_ERR("Could not read land start coordinates.");
-			_land_start_pos.lat = 0.0;
-			_land_start_pos.lon = 0.0;
-		}
-
-		_land_pos.lat = _land_start_pos.lat;
-		_land_pos.lon = _land_start_pos.lon;
-		_land_pos.alt = _land_start_pos.alt;
-	}
-
-	if (_land_index != _invalid_index) {
-		mission_item_s mission;
-
-		if (readMissionItem(mission, _land_index) == EXIT_SUCCESS) {
-			_land_pos.lat = mission.lat;
-			_land_pos.lon = mission.lon;
-			_land_pos.alt = mission.altitude_is_relative ?	mission.altitude +
-					_home_pos_sub.get().alt : mission.altitude;
-		}
-
-		if (_land_start_index == _invalid_index) {
-			_land_start_index = _land_index;
-			_land_start_pos.lat = _land_pos.lat;
-			_land_start_pos.lon = _land_pos.lon;
-			_land_start_pos.alt = _land_pos.alt;
-		}
-	}
-}
-
-bool PlannedMissionInterface::checkMissionWaypointsChanged(const mission_s &old_mission,
-		const mission_s &new_mission) const
-{
-	return (new_mission.count != old_mission.count) || (new_mission.dataman_id != old_mission.dataman_id);
-}

src/lib/mission/planned_mission_interface.h

@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file planned_mission_interface.h
- *
- */
-
-#pragma once
-
-#include <stdint.h>
-
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/uORB.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/vehicle_status.h>
-
-#include "navigator/navigation.h"
-
-class PlannedMissionInterface
-{
-public:
-	void update();
-	void getPreviousPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
-				      uint8_t max_num_items) const;
-	void getNextPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
-				  uint8_t max_num_items) const;
-	bool hasMissionLandStart() const;
-	int goToNextItem(bool execute_jump);
-	int goToPreviousItem(bool execute_jump);
-	int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false);
-	int goToPreviousPositionItem(bool execute_jump);
-	int goToNextPositionItem(bool execute_jump);
-	int goToMissionLandStart();
-	int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status);
-	virtual void onMissionUpdate(bool has_mission_items_changed) = 0;
-
-	int initMission();
-	void resetMission();
-	void resetMissionJumpCounter();
-
-private:
-	int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps,
-			   bool mission_direction_backward = false) const;
-	int setMissionIndex(int32_t index);
-	int writeMission(mission_s &mission);
-	int readMission(mission_s &read_mission) const;
-	int readMissionItem(mission_item_s &read_mission_item, size_t index) const;
-	int writeMissionItem(const mission_item_s &mission_item, size_t index) const ;
-	bool isMissionValid(mission_s &mission) const;
-	void findLandStartItem();
-	bool checkMissionWaypointsChanged(const mission_s &old_mission, const mission_s &new_mission) const;
-public:
-	static const uint16_t _invalid_index{UINT16_MAX};
-private:
-	static const uint16_t _max_jump_iteraion{10u};
-protected:
-	mission_s _mission;
-	mission_item_s _current_planned_mission_item;
-	uint16_t _land_start_index;
-	uint16_t _land_index;
-	struct {
-		double lat;
-		double lon;
-		float alt;
-	} _land_start_pos{.lat = 0.0,
-				  .lon = 0.0,
-				  .alt = 0.0f},
-			       _land_pos{.lat = 0.0,
-					 .lon = 0.0,
-					 .alt = 0.0f};
-
-private:
-	bool _is_land_start_item_searched{false};
-
-	uORB::Subscription _mission_sub{ORB_ID(mission)};
-	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
-	uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
-};

src/lib/wind_estimator/WindEstimator.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -91,13 +91,13 @@ WindEstimator::update(uint64_t time_now)
 		return;
 	}
 
-	const float dt = (float)(time_now - _time_last_update) * 1e-6f;
+	float dt = (float)(time_now - _time_last_update) * 1e-6f;
 	_time_last_update = time_now;
 
 	matrix::Matrix3f Qk;
-	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;
+	Qk(0, 0) = _wind_psd * dt;
+	Qk(1, 1) = Qk(0, 0);
+	Qk(2, 2) = _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 < FLT_EPSILON) {
+	if (_tas_innov_var < 0.0f) {
 		// 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(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);
+	_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);
 
 	// 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 < FLT_EPSILON) {
+	if (_beta_innov_var < 0.0f) {
 		// 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(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);
+	_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);
 
 	// update covariance matrix
 	_P = _P - K * H_beta * _P;
@@ -211,7 +211,7 @@ WindEstimator::run_sanity_checks()
 		}
 	}
 
-	if (!_state.isAllFinite()) {
+	if (!PX4_ISFINITE(_state(INDEX_W_N)) || !PX4_ISFINITE(_state(INDEX_W_E)) || !PX4_ISFINITE(_state(INDEX_TAS_SCALE))) {
 		_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++) {
-			const float tmp = (_P(row, column) + _P(column, row)) * 0.5f;
+			float tmp = (_P(row, column) + _P(column, row)) / 2;
 			_P(row, column) = tmp;
 			_P(column, row) = tmp;
 		}

src/lib/wind_estimator/WindEstimator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -76,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(INDEX_TAS_SCALE, INDEX_TAS_SCALE); }
+	float get_tas_scale_var() { return _P(2, 2); }
 	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(INDEX_W_N, INDEX_W_N), _P(INDEX_W_E, INDEX_W_E)}; }
+	matrix::Vector2f  get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; }
 	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

src/lib/wind_estimator/python/derivation_utils.py

@@ -38,8 +38,6 @@ 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
@@ -89,10 +87,6 @@ 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):

src/lib/wind_estimator/python/generated/fuse_airspeed.h

@@ -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) {

src/lib/wind_estimator/python/generated/fuse_airspeed.py

@@ -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

src/lib/wind_estimator/python/generated/fuse_beta.h

@@ -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) {

src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h

@@ -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)
 

src/lib/world_magnetic_model/geo_magnetic_tables.hpp

@@ -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: 2023.074,
+// Date: 2022.937,
 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 */ {  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, },
+	/* 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, },
 };
 
 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2023.074,
+// Date: 2022.937,
 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 */ { -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 */ { -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 */ {  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: 2023.074,
+// Date: 2022.937,
 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 */ {  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 */ {  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 */ {  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, },
 };

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -25,7 +25,7 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f);
  * @decimal 5
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f);
+PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 0.0001f);
 
 /**
  * Airspeed Selector: Wind estimator true airspeed measurement noise

src/modules/commander/Commander.cpp

@@ -1766,7 +1766,6 @@ void Commander::run()
 			_last_disarmed_timestamp = hrt_absolute_time();
 
 			_user_mode_intention.onDisarm();
-			_vehicle_status.takeoff_time = 0;
 		}
 
 		if (!_arm_state_machine.isArmed()) {
@@ -1972,6 +1971,7 @@ void Commander::landDetectorUpdate()
 			if (!was_landed && _vehicle_land_detected.landed) {
 				mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
 				events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
+				_vehicle_status.takeoff_time = 0;
 
 			} else if (was_landed && !_vehicle_land_detected.landed) {
 				mavlink_log_info(&_mavlink_log_pub, "Takeoff detected\t");

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

@@ -548,7 +548,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 			_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
 		}
 
-		// Check for a magnetometer fault and notify the user
+		// Check for a magnetomer fault and notify the user
 		if (estimator_status_flags.cs_mag_fault) {
 			/* EVENT
 			 * @description

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -55,61 +55,4 @@ void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter)
 	} else {
 		reporter.failsafeFlags().flight_time_limit_exceeded = false;
 	}
-
-	// report warning when approaching max flight time
-	if (!reporter.failsafeFlags().flight_time_limit_exceeded
-	    && context.status().takeoff_time != 0
-	    && _param_com_flt_time_max.get() > 0) {
-
-		const float remaining_flight_time_sec = (context.status().takeoff_time < hrt_absolute_time()) ?
-							_param_com_flt_time_max.get() - 1.e-6f * (hrt_absolute_time() - context.status().takeoff_time) :
-							_param_com_flt_time_max.get();
-
-		if (remaining_flight_time_sec < 0.1f * _param_com_flt_time_max.get()) {
-			// send warnings every minute until RTL
-
-			const int floored_remaining_flight_time_sec = int(remaining_flight_time_sec);
-
-			if (floored_remaining_flight_time_sec <= 60 && _last_flight_time_warning_sec == -1) {
-				// less than or equal to a minute remaining on first pass
-
-				if (reporter.mavlink_log_pub()) {
-					mavlink_log_warning(reporter.mavlink_log_pub(), "Approaching max flight time (system will RTL in %i seconds)\t",
-							    floored_remaining_flight_time_sec);
-				}
-
-				/* EVENT
-				* @description
-				* Maximal flight time warning (less than 1min remaining)
-				*/
-				events::send<int16_t>(events::ID("commander_max_flight_time_warning_seconds"), events::Log::Warning,
-						      "Approaching max flight time (system will RTL in {1} seconds)", floored_remaining_flight_time_sec);
-
-			} else if ((floored_remaining_flight_time_sec % 60) == 0 && floored_remaining_flight_time_sec >= 60
-				   && floored_remaining_flight_time_sec != _last_flight_time_warning_sec) {
-
-				const int floored_remaining_flight_time_min = (int)(remaining_flight_time_sec * 0.016666667f);
-
-				if (reporter.mavlink_log_pub()) {
-					mavlink_log_warning(reporter.mavlink_log_pub(), "Approaching max flight time (system will RTL in %i minutes)\t",
-							    floored_remaining_flight_time_min);
-				}
-
-				/* EVENT
-				* @description
-				* Maximal flight time warning (more than 1min remaining)
-				*/
-				events::send<int16_t>(events::ID("commander_max_flight_time_warning_minutes"), events::Log::Warning,
-						      "Approaching max flight time (system will RTL in {1} minutes)", floored_remaining_flight_time_min);
-			}
-
-			_last_flight_time_warning_sec = floored_remaining_flight_time_sec;
-
-		} else {
-			_last_flight_time_warning_sec = -1; //reset if not in last 10%
-		}
-
-	} else {
-		_last_flight_time_warning_sec = -1; //reset if not enabled, landed or currently already exceeded
-	}
 }

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -44,8 +44,6 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
-	int _last_flight_time_warning_sec{-1};
-
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max
 				       )

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

@@ -147,12 +147,6 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		// Here we expect there is already an event reported for the failing check (this is for external modes)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other);
 	}
-
-	if ((reporter.failsafeFlags().flight_time_limit_exceeded || reporter.failsafeFlags().wind_limit_exceeded)
-	    && reporter.failsafeFlags().mode_req_wind_and_flight_time_compliance != 0) {
-		// Already reported
-		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_wind_and_flight_time_compliance);
-	}
 }
 
 void ModeChecks::checkArmingRequirement(const Context &context, Report &reporter)

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -54,7 +54,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	flags.mode_req_mission = 0;
 	flags.mode_req_offboard_signal = 0;
 	flags.mode_req_home_position = 0;
-	flags.mode_req_wind_and_flight_time_compliance = 0;
 	flags.mode_req_prevent_arming = 0;
 	flags.mode_req_other = 0;
 
@@ -83,7 +82,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_wind_and_flight_time_compliance);
 
 	// NAVIGATION_STATE_AUTO_LOITER
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity);
@@ -91,7 +89,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance);
 
 	// NAVIGATION_STATE_AUTO_RTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity);
@@ -142,7 +139,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_prevent_arming);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_wind_and_flight_time_compliance);
 
 	// NAVIGATION_STATE_AUTO_PRECLAND
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_angular_velocity);
@@ -157,7 +153,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_prevent_arming);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_wind_and_flight_time_compliance);
 
 	// NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_angular_velocity);

src/modules/commander/UserModeIntention.cpp

@@ -43,6 +43,11 @@ UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_
 bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
 {
 	_ever_had_mode_change = true;
+	_had_mode_change = true;
+
+	if (_user_intented_nav_state == user_intended_nav_state) {
+		return true;
+	}
 
 	// Always allow mode change while disarmed
 	bool always_allow = force || !isArmed();
@@ -62,7 +67,6 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
 	}
 
 	if (allow_change) {
-		_had_mode_change = true;
 		_user_intented_nav_state = user_intended_nav_state;
 
 		if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {

src/modules/commander/commander_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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
@@ -1025,11 +1025,12 @@ PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f);
  * Wind speed warning threshold
  *
  * A warning is triggered if the currently estimated wind speed is above this value.
- * Warning is sent periodically (every 1 minute).
+ * Warning is sent periodically (every 1min).
  *
- * Set to -1 to disable.
+ * A negative value disables the feature.
  *
  * @min -1
+ * @max 30
  * @decimal 1
  * @increment 0.1
  * @group Commander
@@ -1042,31 +1043,29 @@ PARAM_DEFINE_FLOAT(COM_WIND_WARN, -1.f);
  *
  * The vehicle aborts the current operation and returns to launch when
  * the time since takeoff is above this value. It is not possible to resume the
- * mission or switch to any auto mode other than RTL or Land. Taking over in any manual
- * mode is still possible.
+ * mission or switch to any mode other than RTL or Land.
  *
- * Starting from 90% of the maximum flight time, a warning message will be sent
- * every 1 minute with the remaining time until automatic RTL.
+ * Set a negative value to disable.
  *
- * Set to -1 to disable.
  *
  * @unit s
  * @min -1
+ * @max 10000
+ * @value 0 Disable
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1);
 
 /**
- * Wind speed RTL threshold
+ * Wind speed RLT threshold
  *
- * Wind speed threshold above which an automatic return to launch is triggered.
- * It is not possible to resume the mission or switch to any auto mode other than
- * RTL or Land if this threshold is exceeded. Taking over in any manual
- * mode is still possible.
+ * Wind speed threshold above which an automatic return to launch is triggered
+ * and enforced as long as the threshold is exceeded.
  *
- * Set to -1 to disable.
+ * A negative value disables the feature.
  *
  * @min -1
+ * @max 30
  * @decimal 1
  * @increment 0.1
  * @group Commander

src/modules/commander/failsafe/emscripten_template.html

@@ -32,10 +32,7 @@
       }
       .checkbox-field {
           display: flex;
-          align-items: center;
           flex-direction: row;
-          margin-bottom: 8px;
-          line-height: 100%;
       }
       .box {
           background-color: rgb(231, 233, 235);

src/modules/commander/failsafe/failsafe.cpp

@@ -393,7 +393,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 
 	CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
 		       ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
-	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL));
+	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded,
+		       ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
 
 	CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()));
 

src/modules/commander/failsafe/framework.cpp

@@ -614,7 +614,6 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended
 bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode)
 {
 	uint32_t mode_mask = 1u << mode;
-	// mode_req_wind_and_flight_time_compliance: does not need to be handled here (these are separate failsafe triggers)
 	return
 		(!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) &&
 		(!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0)) &&

src/modules/ekf2/EKF/common.h

@@ -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 magnetometer data
+		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 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 magnetometer 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 magnetomer 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;

src/modules/ekf2/EKF/mag_control.cpp

@@ -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 magnetometer data to prevent rapid rotation of the earth field
+		// before fusing magnetomer data to prevent rapid rotation of the earth field
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -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 magnetometer Z component
+	// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer 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;

src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py

@@ -36,8 +36,6 @@ 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
@@ -109,10 +107,6 @@ 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):

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h

@@ -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)

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h

@@ -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);
   }

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h

@@ -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);
   }

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h

@@ -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);
   }

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h

@@ -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)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h

@@ -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)
 

src/modules/ekf2/ekf2_params.c

@@ -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 magnetometer 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 magnetomer 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 magnetometer 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 magnetomer 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 magnetometer 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 magnetomer fusion.
  *
  * @group EKF2
  * @min 0.0

src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv

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

src/modules/ekf2/test/change_indication/iris_gps.csv

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

src/modules/logger/params.c

@@ -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 magnetometer data)
+ * 6 : Topics for sensor comparison (low rate raw IMU, Baro and Magnetomer data)
  * 7 : Topics for computer vision and collision avoidance
  * 8 : Raw FIFO high-rate IMU (Gyro)
  * 9 : Raw FIFO high-rate IMU (Accel)

src/modules/mavlink/mavlink

@@ -1 +1 @@
-Subproject commit e3b8756e37cd2cd01ba658461bb4dbffb2fdf914
+Subproject commit 74dee05f0cd121ae27e021d011a04b161c9d0440

src/modules/mavlink/mavlink_main.cpp

@@ -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->get_forwarding_on())) {
+		if (inst && (inst != self) && (inst->_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,9 +2119,6 @@ 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);
 	}
 
@@ -2186,7 +2183,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 (get_forwarding_on()) {
+	if (_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.
@@ -2540,7 +2537,7 @@ Mavlink::task_main(int argc, char *argv[])
 		_events.update(t);
 
 		/* pass messages from other UARTs */
-		if (get_forwarding_on()) {
+		if (_forwarding_on) {
 
 			bool is_part;
 			uint8_t *read_ptr;
@@ -2632,7 +2629,7 @@ Mavlink::task_main(int argc, char *argv[])
 		_socket_fd = -1;
 	}
 
-	if (get_forwarding_on()) {
+	if (_forwarding_on) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
@@ -2952,7 +2949,6 @@ 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: ");

src/modules/navigator/CMakeLists.txt

@@ -53,5 +53,4 @@ px4_add_module(
 		geo
 		geofence_breach_avoidance
 		motion_planning
-		planned_mission_interface
 	)

src/modules/navigator/mission.cpp

@@ -67,33 +67,28 @@ Mission::Mission(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
 {
-	_is_current_planned_mission_item_valid = initMission() == EXIT_SUCCESS;
+	mission_init();
 }
 
-void Mission::onMissionUpdate(bool has_mission_items_changed)
+void Mission::mission_init()
 {
-	if (has_mission_items_changed && !_navigator->get_land_detected()->landed) {
-		_mission_waypoints_changed = true;
-	}
+	// init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start
+	mission_s mission{};
 
-	_is_current_planned_mission_item_valid = true;
-	update_mission();
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+		if ((mission.timestamp != 0)
+		    && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) {
+			if (mission.count > 0) {
+				PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", mission.dataman_id, mission.count);
+			}
 
-	if (isActive()) {
-		_navigator->reset_triplets();
+		} else {
+			PX4_ERR("reading mission state failed");
 
-		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
-			_is_current_planned_mission_item_valid = setMissionToClosestItem(_navigator->get_global_position()->lat,
-					_navigator->get_global_position()->lon, _navigator->get_global_position()->alt, _navigator->get_home_position()->alt,
-					*_navigator->get_vstatus()) == EXIT_SUCCESS;
-		}
-
-		_mission_waypoints_changed = false;
-		set_mission_items();
-
-	} else {
-		if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
-			_mission_type = MISSION_TYPE_MISSION;
+			// initialize mission state in dataman
+			mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
+			mission.timestamp = hrt_absolute_time();
+			dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
 		}
 	}
 }
@@ -101,25 +96,58 @@ void Mission::onMissionUpdate(bool has_mission_items_changed)
 void
 Mission::on_inactive()
 {
-	PlannedMissionInterface::update();
-
-	// if we were executing a landing but have been inactive for 2 seconds, then make the landing invalid
+	// if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid
 	// this prevents RTL to just continue at the current mission index
 	if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) {
 		_navigator->setMissionLandingInProgress(false);
 	}
 
-	/* reset the current mission if needed */
-	if (need_to_reset_mission()) {
-		reset_mission(_mission);
-		update_mission();
-		_navigator->reset_cruising_speed();
+	/* Without home a mission can't be valid yet anyway, let's wait. */
+	if (!_navigator->home_global_position_valid()) {
+		return;
 	}
 
-	/* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */
-	if (_navigator->home_global_position_valid() && !_initialized_mission_checked) {
-		check_mission_valid();
-		_initialized_mission_checked = true;
+	if (_inited) {
+		if (_mission_sub.updated()) {
+			update_mission();
+
+			if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
+				_mission_type = MISSION_TYPE_MISSION;
+			}
+		}
+
+		/* reset the current mission if needed */
+		if (need_to_reset_mission()) {
+			reset_mission(_mission);
+			update_mission();
+			_navigator->reset_cruising_speed();
+		}
+
+	} else {
+
+		/* load missions from storage */
+		mission_s mission_state = {};
+
+		dm_lock(DM_KEY_MISSION_STATE);
+
+		/* read current state */
+		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+		dm_unlock(DM_KEY_MISSION_STATE);
+
+		if (read_res == sizeof(mission_s)) {
+			_mission.dataman_id = mission_state.dataman_id;
+			_mission.count = mission_state.count;
+			_current_mission_index = mission_state.current_seq;
+
+			// find and store landing start marker (if available)
+			find_mission_land_start();
+		}
+
+		/* On init let's check the mission, maybe there is already one available. */
+		check_mission_valid(false);
+
+		_inited = true;
 	}
 
 	/* require takeoff after non-loiter or landing */
@@ -164,16 +192,12 @@ Mission::on_activation()
 	if (_mission_waypoints_changed) {
 		// do not set the closest mission item in the normal mission mode
 		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
-			_is_current_planned_mission_item_valid = setMissionToClosestItem(_navigator->get_global_position()->lat,
-					_navigator->get_global_position()->lon, _navigator->get_global_position()->alt, _navigator->get_home_position()->alt,
-					*_navigator->get_vstatus()) == EXIT_SUCCESS;
+			_current_mission_index = index_closest_mission_item();
 		}
 
 		_mission_waypoints_changed = false;
 	}
 
-	check_mission_valid();
-
 	// we already reset the mission items
 	_execution_mode_changed = false;
 
@@ -191,7 +215,15 @@ Mission::on_activation()
 void
 Mission::on_active()
 {
-	PlannedMissionInterface::update();
+	check_mission_valid(false);
+
+	/* Check if stored mission plan has changed */
+	const bool mission_sub_updated = _mission_sub.updated();
+
+	if (mission_sub_updated) {
+		_navigator->reset_triplets();
+		update_mission();
+	}
 
 	/* mission is running (and we are armed), need reset after disarm */
 	_need_mission_reset = true;
@@ -199,7 +231,16 @@ Mission::on_active()
 	_mission_changed = false;
 
 	/* reset mission items if needed */
-	if (_execution_mode_changed) {
+	if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) {
+		if (_mission_waypoints_changed) {
+			// do not set the closest mission item in the normal mission mode
+			if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
+				_current_mission_index = index_closest_mission_item();
+			}
+
+			_mission_waypoints_changed = false;
+		}
+
 		_execution_mode_changed = false;
 		set_mission_items();
 	}
@@ -267,21 +308,23 @@ Mission::on_active()
 bool
 Mission::set_current_mission_index(uint16_t index)
 {
-	if (_navigator->get_mission_result()->valid && (index < _mission.count)) {
-		_is_current_planned_mission_item_valid = (goToItem(index, true) == EXIT_SUCCESS);
+	if (index == _current_mission_index) {
+		return true; // nothing to do, so return true
 
-		if (!_is_current_planned_mission_item_valid) {
-			return false;
-		}
+	} else if (_navigator->get_mission_result()->valid && (index < _mission.count)) {
+
+		_current_mission_index = index;
 
 		// a mission index is set manually which has the higher priority than the closest mission item
 		// as it is set by the user
 		_mission_waypoints_changed = false;
 
 		// update mission items if already in active mission
-		if (isActive()) {
+		if (_navigator->is_planned_mission()) {
 			// prevent following "previous - current" line
-			_navigator->reset_triplets();
+			_navigator->get_position_setpoint_triplet()->previous.valid = false;
+			_navigator->get_position_setpoint_triplet()->current.valid = false;
+			_navigator->get_position_setpoint_triplet()->next.valid = false;
 			set_mission_items();
 		}
 
@@ -294,10 +337,7 @@ Mission::set_current_mission_index(uint16_t index)
 void
 Mission::set_closest_item_as_current()
 {
-	_is_current_planned_mission_item_valid = (setMissionToClosestItem(_navigator->get_global_position()->lat,
-			_navigator->get_global_position()->lon,
-			_navigator->get_global_position()->alt, _navigator->get_home_position()->alt,
-			*_navigator->get_vstatus()) == EXIT_SUCCESS);
+	_current_mission_index = index_closest_mission_item();
 }
 
 void
@@ -332,7 +372,12 @@ Mission::set_execution_mode(const uint8_t mode)
 					_mission_type = MISSION_TYPE_MISSION;
 				}
 
-				_is_current_planned_mission_item_valid = goToPreviousItem(false) == EXIT_SUCCESS;
+				if (_current_mission_index > _mission.count - 1) {
+					_current_mission_index = _mission.count - 1;
+
+				} else if (_current_mission_index > 0) {
+					--_current_mission_index;
+				}
 
 				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			}
@@ -343,7 +388,12 @@ Mission::set_execution_mode(const uint8_t mode)
 			if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) ||
 			    (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) {
 				// handle switch from reverse to forward mission
-				_is_current_planned_mission_item_valid = goToNextItem(true) == EXIT_SUCCESS;
+				if (_current_mission_index < 0) {
+					_current_mission_index = 0;
+
+				} else if (_current_mission_index < _mission.count - 1) {
+					++_current_mission_index;
+				}
 
 				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			}
@@ -356,20 +406,82 @@ Mission::set_execution_mode(const uint8_t mode)
 	}
 }
 
+bool
+Mission::find_mission_land_start()
+{
+	/* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index
+	 *  return false if not found
+	 */
+
+	const dm_item_t dm_current = (dm_item_t)_mission.dataman_id;
+	struct mission_item_s missionitem = {};
+	struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START
+
+	_land_start_available = false;
+
+	bool found_land_start_marker = false;
+
+	for (size_t i = 1; i < _mission.count; i++) {
+		const ssize_t len = sizeof(missionitem);
+		missionitem_prev = missionitem; // store the last mission item before reading a new one
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			PX4_ERR("dataman read failure");
+			break;
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
+			found_land_start_marker = true;
+			_land_start_index = i;
+		}
+
+		if (found_land_start_marker && !_land_start_available && i > _land_start_index
+		    && item_contains_position(missionitem)) {
+			// use the position of any waypoint after the land start marker which specifies a position.
+			_landing_start_lat = missionitem.lat;
+			_landing_start_lon = missionitem.lon;
+			_landing_start_alt = missionitem.altitude_is_relative ?	missionitem.altitude +
+					     _navigator->get_home_position()->alt : missionitem.altitude;
+			_landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius)
+						  && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) :
+						 _navigator->get_loiter_radius();
+			_land_start_available = true;
+		}
+
+		if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
+		    (missionitem.nav_cmd == NAV_CMD_LAND)) {
+
+			_landing_lat = missionitem.lat;
+			_landing_lon = missionitem.lon;
+			_landing_alt = missionitem.altitude_is_relative ?	missionitem.altitude + _navigator->get_home_position()->alt :
+				       missionitem.altitude;
+
+			// don't have a valid land start yet, use the landing item itself then
+			if (!_land_start_available) {
+				_land_start_index = i;
+				_landing_start_lat = _landing_lat;
+				_landing_start_lon = _landing_lon;
+				_landing_start_alt = _landing_alt;
+				_land_start_available = true;
+			}
+
+		}
+	}
+
+	return _land_start_available;
+}
+
 bool
 Mission::land_start()
 {
 	// if not currently landing, jump to do_land_start
-	if (hasMissionLandStart()) {
+	if (_land_start_available) {
 		if (_navigator->getMissionLandingInProgress()) {
 			return true;
 
 		} else {
-			_is_current_planned_mission_item_valid = (goToMissionLandStart() == EXIT_SUCCESS);
-
-			if (!_is_current_planned_mission_item_valid) {
-				return false;
-			}
+			set_current_mission_index(get_land_start_index());
 
 			const bool can_land_now = landing();
 			_navigator->setMissionLandingInProgress(can_land_now);
@@ -387,7 +499,7 @@ Mission::landing()
 	//  mission valid, still flying, and in the landing portion of mission
 
 	const bool mission_valid = _navigator->get_mission_result()->valid;
-	const bool on_landing_stage = hasMissionLandStart() && (_mission.current_seq >= _land_start_index);
+	const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index());
 
 	return mission_valid && on_landing_stage;
 }
@@ -403,21 +515,50 @@ Mission::update_mission()
 	 * an existing ROI setting from previous missions */
 	_navigator->reset_vroi();
 
-	check_mission_valid();
+	const mission_s old_mission = _mission;
 
-	failed = !_navigator->get_mission_result()->valid;
+	if (_mission_sub.copy(&_mission)) {
+		/* determine current index */
+		if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
+			_current_mission_index = _mission.current_seq;
 
-	if (!failed) {
-		/* reset mission failure if we have an updated valid mission */
-		_navigator->get_mission_result()->failure = false;
+		} else {
+			/* if less items available, reset to first item */
+			if (_current_mission_index >= (int)_mission.count) {
+				_current_mission_index = 0;
 
-		/* reset sequence info as well */
-		_navigator->get_mission_result()->seq_reached = -1;
-		_navigator->get_mission_result()->seq_total = _mission.count;
+			} else if (_current_mission_index < 0) {
+				/* if not initialized, set it to 0 */
+				_current_mission_index = 0;
+			}
 
-		/* reset work item if new mission has been accepted */
-		_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-		_mission_changed = true;
+			/* otherwise, just leave it */
+		}
+
+		check_mission_valid(true);
+
+		failed = !_navigator->get_mission_result()->valid;
+
+		if (!failed) {
+			/* reset mission failure if we have an updated valid mission */
+			_navigator->get_mission_result()->failure = false;
+
+			/* reset sequence info as well */
+			_navigator->get_mission_result()->seq_reached = -1;
+			_navigator->get_mission_result()->seq_total = _mission.count;
+
+			/* reset work item if new mission has been accepted */
+			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			_mission_changed = true;
+		}
+
+		/* check if the mission waypoints changed while the vehicle is in air
+		 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
+		if (((_mission.count != old_mission.count) ||
+		     (_mission.dataman_id != old_mission.dataman_id)) &&
+		    !_navigator->get_land_detected()->landed) {
+			_mission_waypoints_changed = true;
+		}
 
 	} else {
 		PX4_ERR("mission update failed");
@@ -430,13 +571,18 @@ Mission::update_mission()
 		}
 
 		// reset the mission
-		resetMission();
-		_is_current_planned_mission_item_valid = false;
+		_mission.count = 0;
+		_mission.current_seq = 0;
+		_current_mission_index = 0;
 	}
 
+	// find and store landing start marker (if available)
+	find_mission_land_start();
+
 	set_current_mission_item();
 }
 
+
 void
 Mission::advance_mission()
 {
@@ -450,18 +596,37 @@ Mission::advance_mission()
 		switch (_mission_execution_mode) {
 		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
 		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
-				_is_current_planned_mission_item_valid = goToNextItem(true) == EXIT_SUCCESS;
+				_current_mission_index++;
 				break;
 			}
 
 		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: {
 				// find next position item in reverse order
-				_is_current_planned_mission_item_valid = goToPreviousPositionItem(true) == EXIT_SUCCESS;
+				dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
+
+				for (int32_t i = _current_mission_index - 1; i >= 0; i--) {
+					struct mission_item_s missionitem = {};
+					const ssize_t len = sizeof(missionitem);
+
+					if (dm_read(dm_current, i, &missionitem, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+						PX4_ERR("dataman read failure");
+						break;
+					}
+
+					if (item_contains_position(missionitem)) {
+						_current_mission_index = i;
+						return;
+					}
+				}
+
+				// finished flying back the mission
+				_current_mission_index = -1;
 				break;
 			}
 
 		default:
-			_is_current_planned_mission_item_valid = goToNextItem(true) == EXIT_SUCCESS;
+			_current_mission_index++;
 		}
 
 		break;
@@ -479,20 +644,15 @@ Mission::set_mission_items()
 	bool user_feedback_done = false;
 
 	/* mission item that comes after current if available */
-	constexpr static size_t max_num_next_items{2u};
-	mission_item_s next_mission_items[max_num_next_items];
-	size_t num_found_items;
+	struct mission_item_s mission_item_next_position;
+	struct mission_item_s mission_item_after_next_position;
+	bool has_next_position_item = false;
+	bool has_after_next_position_item = false;
 
 	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 
-	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
-		getPreviousPositionItems(_mission.current_seq - 1, next_mission_items, num_found_items, max_num_next_items);
-
-	} else {
-		getNextPositionItems(_mission.current_seq + 1, next_mission_items, num_found_items, max_num_next_items);
-	}
-
-	if (_is_current_planned_mission_item_valid) {
+	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
+				  &mission_item_after_next_position, &has_after_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_MISSION) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -510,8 +670,6 @@ Mission::set_mission_items()
 		}
 
 		_mission_type = MISSION_TYPE_MISSION;
-		/* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */
-		_mission_item = _current_planned_mission_item;
 
 	} else {
 		if (_mission_type != MISSION_TYPE_NONE) {
@@ -631,10 +789,9 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
 					/* use current mission item as next position item */
-					mission_item_s next_mission_item = _mission_item;
-					next_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-					next_mission_items[0u] = next_mission_item;
-					num_found_items = 1u;
+					mission_item_next_position = _mission_item;
+					mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
+					has_next_position_item = true;
 
 					float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
@@ -750,8 +907,8 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 					/* use current mission item as next position item */
-					next_mission_items[0u] = _mission_item;
-					num_found_items = 1u;
+					mission_item_next_position = _mission_item;
+					has_next_position_item = true;
 
 					float altitude = _navigator->get_global_position()->alt;
 
@@ -794,8 +951,8 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 					/* use current mission item as next position item */
-					next_mission_items[0u] = _mission_item;
-					num_found_items = 1u;
+					mission_item_next_position = _mission_item;
+					has_next_position_item = true;
 
 					/*
 					 * Ignoring waypoint altitude:
@@ -909,18 +1066,18 @@ Mission::set_mission_items()
 				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
 				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 				    && !_navigator->get_land_detected()->landed
-				    && num_found_items > 0u) {
+				    && has_next_position_item) {
 
 					/* disable weathervane before front transition for allowing yaw to align */
 					pos_sp_triplet->current.disable_weather_vane = true;
 
 					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
 
-					set_align_mission_item(&_mission_item, &next_mission_items[0u]);
+					set_align_mission_item(&_mission_item, &mission_item_next_position);
 
 					/* set position setpoint to target during the transition */
 					mission_apply_limitation(_mission_item);
-					mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current);
+					mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
 				}
 
 				/* yaw is aligned now */
@@ -976,10 +1133,11 @@ Mission::set_mission_items()
 		// The mission item is a gate, let's check if the next item in the list provides
 		// a position to go towards.
 
-		if (num_found_items > 0u) {
+		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
+		if (has_next_position_item) {
 			// We have a position, convert it to the setpoint and update setpoint triplet
-			mission_apply_limitation(next_mission_items[0u]);
-			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current);
+			mission_apply_limitation(mission_item_next_position);
+			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
 		}
 
 		// ELSE: The current position setpoint stays unchanged.
@@ -1025,10 +1183,10 @@ Mission::set_mission_items()
 	// If the mission item under evaluation contains a gate, we need to check if we have a next position item so
 	// the controller can fly the correct line between the current and next setpoint
 	if (item_contains_gate(_mission_item)) {
-		if (num_found_items > 1u) {
+		if (has_after_next_position_item) {
 			/* got next mission item, update setpoint triplet */
-			mission_apply_limitation(next_mission_items[1u]);
-			mission_item_to_position_setpoint(next_mission_items[1u], &pos_sp_triplet->next);
+			mission_apply_limitation(mission_item_next_position);
+			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
 
 		} else {
 			pos_sp_triplet->next.valid = false;
@@ -1045,9 +1203,9 @@ Mission::set_mission_items()
 
 		if (_mission_item.autocontinue && !brake_for_hold) {
 			/* try to process next mission item */
-			if (num_found_items > 0u) {
+			if (has_next_position_item) {
 				/* got next mission item, update setpoint triplet */
-				mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
+				mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
 
 			} else {
 				/* next mission item is not available */
@@ -1316,12 +1474,12 @@ Mission::do_abort_landing()
 			    "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing);
 
 	// reset mission index to start of landing
-	if (hasMissionLandStart()) {
-		_is_current_planned_mission_item_valid = goToMissionLandStart() == EXIT_SUCCESS;
+	if (_land_start_available) {
+		_current_mission_index = get_land_start_index();
 
 	} else {
 		// move mission index back (landing approach point)
-		_is_current_planned_mission_item_valid = goToItem(_mission.current_seq - 1u, false, true);
+		_current_mission_index -= 1;
 	}
 
 	// send reposition cmd to get out of mission
@@ -1337,6 +1495,213 @@ Mission::do_abort_landing()
 	_navigator->publish_vehicle_cmd(&vcmd);
 }
 
+bool
+Mission::prepare_mission_items(struct mission_item_s *mission_item,
+			       struct mission_item_s *next_position_mission_item, bool *has_next_position_item,
+			       struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item)
+{
+	*has_next_position_item = false;
+	bool first_res = false;
+	int offset = 1;
+
+	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+		offset = -1;
+	}
+
+	if (read_mission_item(0, mission_item)) {
+
+		first_res = true;
+
+		/* trying to find next position mission item */
+		while (read_mission_item(offset, next_position_mission_item)) {
+			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+				offset--;
+
+			} else {
+				offset++;
+			}
+
+			if (item_contains_position(*next_position_mission_item)) {
+				*has_next_position_item = true;
+				break;
+			}
+		}
+
+		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
+		    after_next_position_mission_item && has_after_next_position_item) {
+			/* trying to find next next position mission item */
+			while (read_mission_item(offset, after_next_position_mission_item)) {
+				offset++;
+
+				if (item_contains_position(*after_next_position_mission_item)) {
+					*has_after_next_position_item = true;
+					break;
+				}
+			}
+		}
+	}
+
+	return first_res;
+}
+
+bool
+Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
+{
+	/* select mission */
+	const int current_index = _current_mission_index;
+	int index_to_read = current_index + offset;
+
+	int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read;
+	const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
+
+	/* do not work on empty missions */
+	if (_mission.count == 0) {
+		return false;
+	}
+
+	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
+	for (int i = 0; i < 10; i++) {
+		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)_mission.count) {
+			/* mission item index out of bounds - if they are equal, we just reached the end */
+			if ((*mission_index_ptr != (int)_mission.count) && (*mission_index_ptr != -1)) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Mission item index out of bound, index: %d, max: %" PRIu16 ".\t",
+						     *mission_index_ptr, _mission.count);
+				events::send<uint16_t, uint16_t>(events::ID("mission_index_out_of_bound"), events::Log::Error,
+								 "Mission item index out of bound, index: {1}, max: {2}", *mission_index_ptr, _mission.count);
+			}
+
+			return false;
+		}
+
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
+		struct mission_item_s mission_item_tmp;
+
+		/* read mission item from datamanager */
+		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t");
+			events::send<uint16_t>(events::ID("mission_failed_to_read_wp"), events::Log::Error,
+					       "Waypoint {1} could not be read from storage", *mission_index_ptr);
+			return false;
+		}
+
+		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
+		if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {
+			const bool execute_jumps = _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL;
+
+			/* do DO_JUMP as many times as requested if not in reverse mode */
+			if ((mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) && execute_jumps) {
+
+				/* only raise the repeat count if this is for the current mission item
+				 * but not for the read ahead mission item */
+				if (offset == 0) {
+					(mission_item_tmp.do_jump_current_count)++;
+
+					/* save repeat count */
+					if (dm_write(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the dataman */
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t");
+						events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error,
+							     "DO JUMP waypoint could not be written");
+						return false;
+					}
+
+					report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count);
+				}
+
+				/* set new mission item index and repeat
+				 * we don't have to validate here, if it's invalid, we should realize this later .*/
+				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
+
+			} else {
+				if (offset == 0 && execute_jumps) {
+					mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed.\t");
+					events::send(events::ID("mission_do_jump_rep_completed"), events::Log::Info,
+						     "DO JUMP repetitions completed");
+				}
+
+				/* no more DO_JUMPS, therefore just try to continue with next mission item */
+				if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+					(*mission_index_ptr)--;
+
+				} else {
+					(*mission_index_ptr)++;
+				}
+			}
+
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
+			return true;
+		}
+	}
+
+	/* we have given up, we don't want to cycle forever */
+	mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.\t");
+	events::send(events::ID("mission_do_jump_cycle"), events::Log::Error, "DO JUMP is cycling, giving up");
+	return false;
+}
+
+void
+Mission::save_mission_state()
+{
+	mission_s mission_state = {};
+
+	/* lock MISSION_STATE item */
+	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+	}
+
+	/* read current state */
+	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	if (read_res == sizeof(mission_s)) {
+		/* data read successfully, check dataman ID and items count */
+		if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) {
+			/* navigator may modify only sequence, write modified state only if it changed */
+			if (mission_state.current_seq != _current_mission_index) {
+				mission_state.current_seq = _current_mission_index;
+				mission_state.timestamp = hrt_absolute_time();
+
+				if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+
+					PX4_ERR("Can't save mission state");
+				}
+			}
+		}
+
+	} else {
+		/* invalid data, this must not happen and indicates error in mission publisher */
+		mission_state.timestamp = hrt_absolute_time();
+		mission_state.dataman_id = _mission.dataman_id;
+		mission_state.count = _mission.count;
+		mission_state.current_seq = _current_mission_index;
+
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Invalid mission state.\t");
+		/* EVENT
+		 * @description No mission or storage failure
+		 */
+		events::send(events::ID("mission_invalid_mission_state"), events::Log::Error, "Invalid mission state");
+
+		/* write modified state only if changed */
+		if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+
+			PX4_ERR("Can't save mission state");
+		}
+	}
+
+	/* unlock MISSION_STATE item */
+	if (dm_lock_ret == 0) {
+		dm_unlock(DM_KEY_MISSION_STATE);
+	}
+}
+
 void
 Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
 {
@@ -1351,7 +1716,7 @@ Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
 void
 Mission::set_mission_item_reached()
 {
-	_navigator->get_mission_result()->seq_reached = _mission.current_seq;
+	_navigator->get_mission_result()->seq_reached = _current_mission_index;
 	_navigator->set_mission_result_updated();
 
 	// let the navigator know that we are currently executing the mission landing.
@@ -1366,40 +1731,84 @@ void
 Mission::set_current_mission_item()
 {
 	_navigator->get_mission_result()->finished = false;
-	_navigator->get_mission_result()->seq_current = _mission.current_seq;
+	_navigator->get_mission_result()->seq_current = _current_mission_index;
 
 	_navigator->set_mission_result_updated();
+
+	save_mission_state();
 }
 
 void
-Mission::check_mission_valid()
+Mission::check_mission_valid(bool force)
 {
+	if ((!_home_inited && _navigator->home_global_position_valid()) || force) {
 
-	MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
+		MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
 
-	_navigator->get_mission_result()->valid =
-		_missionFeasibilityChecker.checkMissionFeasible(_mission,
-				_param_mis_dist_1wp.get(),
-				_param_mis_dist_wps.get());
+		_navigator->get_mission_result()->valid =
+			_missionFeasibilityChecker.checkMissionFeasible(_mission,
+					_param_mis_dist_1wp.get(),
+					_param_mis_dist_wps.get());
 
-	_navigator->get_mission_result()->seq_total = _mission.count;
-	_navigator->increment_mission_instance_count();
-	_navigator->set_mission_result_updated();
+		_navigator->get_mission_result()->seq_total = _mission.count;
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+		_home_inited = _navigator->home_global_position_valid();
+
+		// find and store landing start marker (if available)
+		find_mission_land_start();
+	}
 }
 
 void
 Mission::reset_mission(struct mission_s &mission)
 {
-	if (goToItem(0u, true) == EXIT_SUCCESS) {
-		resetMissionJumpCounter();
+	dm_lock(DM_KEY_MISSION_STATE);
 
-	} else {
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t");
-		events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission");
-		initMission();
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+		if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
+			/* set current item to 0 */
+			mission.current_seq = 0;
+
+			/* reset jump counters */
+			if (mission.count > 0) {
+				const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
+
+				for (unsigned index = 0; index < mission.count; index++) {
+					struct mission_item_s item;
+					const ssize_t len = sizeof(struct mission_item_s);
+
+					if (dm_read(dm_current, index, &item, len) != len) {
+						PX4_WARN("could not read mission item during reset");
+						break;
+					}
+
+					if (item.nav_cmd == NAV_CMD_DO_JUMP) {
+						item.do_jump_current_count = 0;
+
+						if (dm_write(dm_current, index, &item, len) != len) {
+							PX4_WARN("could not save mission item during reset");
+							break;
+						}
+					}
+				}
+			}
+
+		} else {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t");
+			events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission");
+
+			/* initialize mission state in dataman */
+			mission.timestamp = hrt_absolute_time();
+			mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
+			mission.count = 0;
+			mission.current_seq = 0;
+		}
+
+		dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
 	}
 
-	check_mission_valid();
+	dm_unlock(DM_KEY_MISSION_STATE);
 }
 
 bool
@@ -1414,6 +1823,64 @@ Mission::need_to_reset_mission()
 	return false;
 }
 
+int32_t
+Mission::index_closest_mission_item() const
+{
+	int32_t min_dist_index(0);
+	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
+
+	dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
+
+	for (size_t i = 0; i < _mission.count; i++) {
+		struct mission_item_s missionitem = {};
+		const ssize_t len = sizeof(missionitem);
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			PX4_ERR("dataman read failure");
+			break;
+		}
+
+		if (item_contains_position(missionitem)) {
+			// do not consider land waypoints for a fw
+			if (!((missionitem.nav_cmd == NAV_CMD_LAND) &&
+			      (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+			      (!_navigator->get_vstatus()->is_vtol))) {
+				float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon,
+						get_absolute_altitude_for_item(missionitem),
+						_navigator->get_global_position()->lat,
+						_navigator->get_global_position()->lon,
+						_navigator->get_global_position()->alt,
+						&dist_xy, &dist_z);
+
+				if (dist < min_dist) {
+					min_dist = dist;
+					min_dist_index = i;
+				}
+			}
+		}
+	}
+
+	// for mission reverse also consider the home position
+	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+		float dist = get_distance_to_point_global_wgs84(
+				     _navigator->get_home_position()->lat,
+				     _navigator->get_home_position()->lon,
+				     _navigator->get_home_position()->alt,
+				     _navigator->get_global_position()->lat,
+				     _navigator->get_global_position()->lon,
+				     _navigator->get_global_position()->alt,
+				     &dist_xy, &dist_z);
+
+		if (dist < min_dist) {
+			min_dist = dist;
+			min_dist_index = -1;
+		}
+	}
+
+	return min_dist_index;
+}
+
 bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const
 {
 	return ((p1->valid == p2->valid) &&
@@ -1442,7 +1909,7 @@ void Mission::publish_navigator_mission_item()
 	navigator_mission_item_s navigator_mission_item{};
 
 	navigator_mission_item.instance_count = _navigator->mission_instance_count();
-	navigator_mission_item.sequence_current = _mission.current_seq;
+	navigator_mission_item.sequence_current = _current_mission_index;
 	navigator_mission_item.nav_cmd = _mission_item.nav_cmd;
 	navigator_mission_item.latitude = _mission_item.lat;
 	navigator_mission_item.longitude = _mission_item.lon;

src/modules/navigator/mission.h

@@ -52,8 +52,6 @@
 
 #include <float.h>
 
-#include "lib/mission/planned_mission_interface.h"
-
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
@@ -70,14 +68,12 @@
 
 class Navigator;
 
-class Mission : public MissionBlock, public ModuleParams, protected PlannedMissionInterface
+class Mission : public MissionBlock, public ModuleParams
 {
 public:
 	Mission(Navigator *navigator);
 	~Mission() override = default;
 
-	void onMissionUpdate(bool has_mission_items_changed) override;
-
 	void on_inactive() override;
 	void on_inactivation() override;
 	void on_activation() override;
@@ -89,17 +85,17 @@ public:
 	bool landing();
 
 	uint16_t get_land_start_index() const { return _land_start_index; }
-	bool get_land_start_available() const { return _land_start_index != _invalid_index; }
+	bool get_land_start_available() const { return _land_start_available; }
 	bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
 	bool get_mission_changed() const { return _mission_changed ; }
 	bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }
-	double get_landing_start_lat() { return _land_start_pos.lat; }
-	double get_landing_start_lon() { return _land_start_pos.lon; }
-	float get_landing_start_alt() { return _land_start_pos.alt; }
+	double get_landing_start_lat() { return _landing_start_lat; }
+	double get_landing_start_lon() { return _landing_start_lon; }
+	float get_landing_start_alt() { return _landing_start_alt; }
 
-	double get_landing_lat() { return _land_pos.lat; }
-	double get_landing_lon() { return _land_pos.lon; }
-	float get_landing_alt() { return _land_pos.alt; }
+	double get_landing_lat() { return _landing_lat; }
+	double get_landing_lon() { return _landing_lon; }
+	float get_landing_alt() { return _landing_alt; }
 	float get_landing_loiter_rad() { return _landing_loiter_radius; }
 
 	void set_closest_item_as_current();
@@ -111,6 +107,9 @@ public:
 	 */
 	void set_execution_mode(const uint8_t mode);
 private:
+
+	void mission_init();
+
 	/**
 	 * Update mission topic
 	 */
@@ -174,6 +173,29 @@ private:
 	 */
 	void do_abort_landing();
 
+	/**
+	 * Read the current and the next mission item. The next mission item read is the
+	 * next mission item that contains a position.
+	 *
+	 * @return true if current mission item available
+	 */
+	bool prepare_mission_items(mission_item_s *mission_item,
+				   mission_item_s *next_position_mission_item, bool *has_next_position_item,
+				   mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr);
+
+	/**
+	 * Read current (offset == 0) or a specific (offset > 0) mission item
+	 * from the dataman and watch out for DO_JUMPS
+	 *
+	 * @return true if successful
+	 */
+	bool read_mission_item(int offset, struct mission_item_s *mission_item);
+
+	/**
+	 * Save current mission state to dataman
+	 */
+	void save_mission_state();
+
 	/**
 	 * Inform about a changed mission item after a DO_JUMP
 	 */
@@ -192,7 +214,7 @@ private:
 	/**
 	 * Check whether a mission is ready to go
 	 */
-	void check_mission_valid();
+	void check_mission_valid(bool force);
 
 	/**
 	 * Reset mission
@@ -204,6 +226,16 @@ private:
 	 */
 	bool need_to_reset_mission();
 
+	/**
+	 * Find and store the index of the landing sequence (DO_LAND_START)
+	 */
+	bool find_mission_land_start();
+
+	/**
+	 * Return the index of the closest mission item to the current global position.
+	 */
+	int32_t index_closest_mission_item() const;
+
 	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
 
 	void publish_navigator_mission_item();
@@ -216,12 +248,24 @@ private:
 
 	uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
 
+	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
+	mission_s		_mission {};
+
+	int32_t _current_mission_index{-1};
+
+	// track location of planned mission landing
+	bool	_land_start_available{false};
+	uint16_t _land_start_index{UINT16_MAX};		/**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */
+	double _landing_start_lat{0.0};
+	double _landing_start_lon{0.0};
+	float _landing_start_alt{0.0f};
+
+	double _landing_lat{0.0};
+	double _landing_lon{0.0};
+	float _landing_alt{0.0f};
+
 	float _landing_loiter_radius{0.f};
 
-	bool _is_current_planned_mission_item_valid{false};
-
-	bool _initialized_mission_checked{false};
-
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
 	hrt_abstime _time_mission_deactivated{0};
@@ -231,6 +275,8 @@ private:
 		MISSION_TYPE_MISSION
 	} _mission_type{MISSION_TYPE_NONE};
 
+	bool _inited{false};
+	bool _home_inited{false};
 	bool _need_mission_reset{false};
 	bool _mission_waypoints_changed{false};
 	bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */

src/modules/navigator/mission_block.cpp

@@ -924,14 +924,9 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
 
 float
 MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
-{
-	return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt);
-}
-
-float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt)
 {
 	if (mission_item.altitude_is_relative) {
-		return mission_item.altitude + home_alt;
+		return mission_item.altitude + _navigator->get_home_position()->alt;
 
 	} else {
 		return mission_item.altitude;

src/modules/navigator/mission_block.h

@@ -99,8 +99,6 @@ public:
 	 */
 	static bool item_contains_marker(const mission_item_s &item);
 
-	static float get_absolute_altitude_for_item(const  mission_item_s &mission_item, float home_alt);
-
 	/**
 	 * @brief Set the payload deployment successful flag object
 	 *

src/modules/navigator/navigator_mode.h

@@ -54,8 +54,6 @@ public:
 
 	void run(bool active);
 
-	bool isActive() {return _active;};
-
 	/**
 	 * This function is called while the mode is inactive
 	 */