Fix heightrate feedforard for fixdwings

If NuttX is built without support for SMPS it can brick the hardware.
Therefore, I suggest that we add this additional compile-time check.

Signed-off-by: Julian Oes <julian@oes.ch>

Tools/simulation/gz/models/rc_cessna/model.sdf

@@ -209,7 +209,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <link name="left_elevon">
@@ -638,7 +637,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <joint name="RightWheelJoint" type="revolute">
@@ -655,7 +653,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <joint name="CenterWheelJoint" type="revolute">
@@ -672,7 +669,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
@@ -807,7 +803,7 @@
       <sub_topic>servo_3</sub_topic>
       <p_gain>10.0</p_gain>
     </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
       <jointName>rotor_puller_joint</jointName>
       <linkName>rotor_puller</linkName>
       <turningDirection>cw</turningDirection>

Tools/simulation/gz/models/standard_vtol/model.sdf

@@ -208,7 +208,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <link name='rotor_1'>
@@ -272,7 +271,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <link name='rotor_2'>
@@ -336,7 +334,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <link name='rotor_3'>
@@ -400,7 +397,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
 
@@ -466,7 +462,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
 

Tools/simulation/gz/models/x500/model.sdf

@@ -302,7 +302,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <link name="rotor_1">
@@ -375,7 +374,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <link name="rotor_2">
@@ -448,7 +446,6 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
     <link name="rotor_3">
@@ -521,10 +518,9 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
-   <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+   <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
       <jointName>rotor_0_joint</jointName>
       <linkName>rotor_0</linkName>
       <turningDirection>ccw</turningDirection>

Tools/simulation/gz/models/x500_vision/model.sdf

@@ -2,7 +2,7 @@
 <sdf version='1.9'>
   <model name='x500-vision'>
     <include merge='true'>
-      <uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</uri>
+      <uri>x500</uri>
     </include>
       <plugin
         filename="gz-sim-odometry-publisher-system"

Tools/simulation/gz/worlds/default.sdf

@@ -5,22 +5,22 @@
       <real_time_factor>1.0</real_time_factor>
       <real_time_update_rate>250</real_time_update_rate>
     </physics>
-    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
-    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
-    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
-    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
-    <plugin name='ignition::gazebo::systems::Imu' filename='ignition-gazebo-imu-system'/>
-    <plugin name='ignition::gazebo::systems::AirPressure' filename='ignition-gazebo-air-pressure-system'/>
-    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
+    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
+    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
+    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
+    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
+    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
+    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
+    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
       <render_engine>ogre2</render_engine>
     </plugin>
     <gui fullscreen='false'>
       <plugin name='3D View' filename='GzScene3D'>
-        <ignition-gui>
+        <gz-gui>
           <title>3D View</title>
           <property type='bool' key='showTitleBar'>0</property>
           <property type='string' key='state'>docked</property>
-        </ignition-gui>
+        </gz-gui>
         <engine>ogre2</engine>
         <scene>scene</scene>
         <ambient_light>0.5984631152222222 0.5984631152222222 0.5984631152222222</ambient_light>
@@ -28,7 +28,7 @@
         <camera_pose>-6 0 6 0 0.5 0</camera_pose>
       </plugin>
       <plugin name='World control' filename='WorldControl'>
-        <ignition-gui>
+        <gz-gui>
           <title>World control</title>
           <property type='bool' key='showTitleBar'>0</property>
           <property type='bool' key='resizable'>0</property>
@@ -40,13 +40,13 @@
             <line own='left' target='left'/>
             <line own='bottom' target='bottom'/>
           </anchors>
-        </ignition-gui>
+        </gz-gui>
         <play_pause>1</play_pause>
         <step>1</step>
         <start_paused>1</start_paused>
       </plugin>
       <plugin name='World stats' filename='WorldStats'>
-        <ignition-gui>
+        <gz-gui>
           <title>World stats</title>
           <property type='bool' key='showTitleBar'>0</property>
           <property type='bool' key='resizable'>0</property>
@@ -58,7 +58,7 @@
             <line own='right' target='right'/>
             <line own='bottom' target='bottom'/>
           </anchors>
-        </ignition-gui>
+        </gz-gui>
         <sim_time>1</sim_time>
         <real_time>1</real_time>
         <real_time_factor>1</real_time_factor>

boards/cubepilot/cubeorangeplus/default.px4board

@@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -49,7 +50,6 @@ CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -66,7 +66,6 @@ CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y

boards/cubepilot/cubeorangeplus/init/rc.board_sensors

@@ -4,12 +4,27 @@
 #------------------------------------------------------------------------------
 board_adc start
 
-# SPI4
+# Variants
+# 1. Isolated {ICM42688p, ICM20948(with mag)}, body-fixed {ICM20649}
+# 2. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM20649, ICM45686, AK09918}
+# 3. Isolated {ICM42688p, ICM42688p}, body-fixed  {ICM45686, AK09918}
+
+# SPI4 is isolated, SPI1 is body-fixed
+
+# SPI4, isolated
 ms5611 -s -b 4 start
-icm42688p -s -b 4 -R 10 start
-icm20948 -s -b 4 -R 10 -M start
 
-# SPI1
+icm42688p -s -b 4 -R 10 start -c 15
+if ! icm20948 -s -b 4 -R 10 -M -q start
+then
+	icm42688p -s -b 4 -R 6 start -c 13
+fi
+
+# SPI1, body-fixed
+if ! icm45686 -s -b 1 -R 3 -q start
+then
+	icm20649 -s -b 1 start
+fi
+
 ms5611 -s -b 1 start
-icm20649 -s -b 1 start
 

boards/cubepilot/cubeorangeplus/src/board_config.h

@@ -44,6 +44,16 @@
 #include <stdint.h>
 #include <stm32_gpio.h>
 
+/**
+ * If NuttX is built without support for SMPS it can brick the hardware.
+ * Therefore, we make sure the NuttX headers are correct.
+ */
+#include "hardware/stm32h7x3xx_pwr.h"
+#if STM32_PWR_CR3_SMPSEXTHP != (1 << 3)
+#  error "No SMPS support in NuttX submodule");
+#endif
+
+
 /* PX4IO connection configuration */
 #define BOARD_USES_PX4IO_VERSION       2
 #define PX4IO_SERIAL_DEVICE            "/dev/ttyS3"

boards/cubepilot/cubeorangeplus/src/spi.cpp

@@ -38,6 +38,7 @@
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), // MPU_CS, MPU_DRDY
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortG, GPIO::Pin1}), // ICM45686_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611,  SPI::CS{GPIO::PortD, GPIO::Pin7}), // BARO_CS
 	}),
 
@@ -48,6 +49,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI4, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin4}),  // MPU_EXT_CS
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
 		initSPIDevice(DRV_BARO_DEVTYPE_MS5611, 	SPI::CS{GPIO::PortC, GPIO::Pin14}), // BARO_EXT_CS
 	}),
 };

src/drivers/drv_sensor.h

@@ -84,6 +84,7 @@
 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32
 #define DRV_ACC_DEVTYPE_MPU6050  0x33
+#define DRV_IMU_DEVTYPE_ICM45686 0x34
 
 #define DRV_GYR_DEVTYPE_MPU6050  0x35
 #define DRV_IMU_DEVTYPE_MPU6500  0x36

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

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   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__icm45686
+	MAIN icm45686
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+		#-DDEBUG_BUILD
+	SRCS
+		icm45686_main.cpp
+		ICM45686.cpp
+		ICM45686.hpp
+		InvenSense_ICM45686_registers.hpp
+	DEPENDS
+		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
+	)

src/drivers/imu/invensense/icm45686/ICM45686.cpp

@@ -0,0 +1,752 @@
+/****************************************************************************
+ *
+ *   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 "ICM45686.hpp"
+
+using namespace time_literals;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
+{
+	// 0xXXXAABBC
+	uint32_t high   = ((a << 12) & 0x000FF000);
+	uint32_t low    = ((b << 4)  & 0x00000FF0);
+	uint32_t lowest = (c         & 0x0000000F);
+
+	uint32_t x = high | low | lowest;
+
+	if (a & Bit7) {
+		// sign extend
+		x |= 0xFFF00000u;
+	}
+
+	return static_cast<int32_t>(x);
+}
+
+
+ICM45686::ICM45686(const I2CSPIDriverConfig &config) :
+	SPI(config),
+	I2CSPIDriver(config),
+	_px4_accel(get_device_id(), config.rotation),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (config.custom1 != 0) {
+		_enable_clock_input = true;
+		_input_clock_freq = config.custom1;
+		// TODO: this is not tested
+		ConfigureCLKIN();
+
+	} else {
+		_enable_clock_input = false;
+	}
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+}
+
+ICM45686::~ICM45686()
+{
+	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);
+}
+
+int ICM45686::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool ICM45686::Reset()
+{
+	_state = STATE::RESET;
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void ICM45686::exit_and_cleanup()
+{
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void ICM45686::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+	PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled");
+
+	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);
+}
+
+int ICM45686::probe()
+{
+	for (int i = 0; i < 3; i++) {
+		const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+
+		if (whoami != WHOAMI) {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+			return PX4_ERROR;
+		}
+	}
+
+	return PX4_OK;
+}
+
+void ICM45686::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// DEVICE_CONFIG: Software reset configuration
+		RegisterWrite(Register::BANK_0::REG_MISC2, REG_MISC2_BIT::SOFT_RST);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
+		    && ((RegisterRead(Register::BANK_0::REG_MISC2) & Bit1) == 0x0)) {
+
+			// Wakeup accel and gyro and schedule remaining configuration
+			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+		if (Configure()) {
+			// if configure succeeded then reset the FIFO
+			_state = STATE::FIFO_RESET;
+			ScheduleDelayed(1_ms);
+
+		} 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_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
+			bool success = false;
+
+			if (FIFORead(timestamp_sample)) {
+				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_bank0_cfg[_checked_register_bank0])) {
+					_last_config_check_timestamp = now;
+					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+			}
+		}
+
+		break;
+	}
+}
+
+void ICM45686::ConfigureSampleRate(int sample_rate)
+{
+	// round down to the nearest FIFO sample dt
+	const float min_interval = FIFO_SAMPLE_DT;
+	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
+
+	// recompute FIFO empty interval (us) with actual gyro sample limit
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
+
+	ConfigureFIFOWatermark(_fifo_gyro_samples);
+}
+
+void ICM45686::ConfigureFIFOWatermark(uint8_t samples)
+{
+	// FIFO watermark threshold in number of bytes
+	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
+
+	for (auto &r : _register_bank0_cfg) {
+		if (r.reg == Register::BANK_0::FIFO_CONFIG1_0) {
+			// FIFO_WM[7:0]  FIFO_CONFIG2
+			r.set_bits = fifo_watermark_threshold & 0xFF;
+
+		} else if (r.reg == Register::BANK_0::FIFO_CONFIG1_1) {
+			// FIFO_WM[11:8] FIFO_CONFIG3
+			r.set_bits = (fifo_watermark_threshold >> 8) & 0xFF;
+		}
+	}
+}
+
+void ICM45686::ConfigureCLKIN()
+{
+	for (auto &r0 : _register_bank0_cfg) {
+		if (r0.reg == Register::BANK_0::RTC_CONFIG) {
+			r0.set_bits = RTC_CONFIG_BIT::RTC_MODE;
+		}
+	}
+
+	for (auto &r0 : _register_bank0_cfg) {
+		if (r0.reg == Register::BANK_0::IOC_PAD_SCENARIO_OVRD) {
+			r0.set_bits = PADS_INT2_CFG_OVRD | PADS_INT2_CFG_OVRD_CLKIN;
+		}
+	}
+}
+
+bool ICM45686::Configure()
+{
+	// Set it to little endian first, otherwise the chip doesn't match the manual
+	// which is just utterly confusing.
+	//uint8_t cmd[3] {
+	//    BANK_IPREG_TOP1,
+	//        SREG_CTRL,
+	//    SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIT::SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIG };
+	//transfer(cmd, cmd, sizeof(cmd));
+
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	// now check that all are configured
+	bool success = true;
+
+	for (const auto &reg_cfg : _register_bank0_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	// 20-bits data format used the only FSR settings that are operational
+	// are ±4000dps for gyroscope and ±32 for accelerometer
+	_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
+	_px4_gyro.set_range(math::radians(4000.f));
+
+	return success;
+}
+
+template <typename T>
+bool ICM45686::RegisterCheck(const T &reg_cfg)
+{
+	bool success = true;
+
+	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+
+	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_INFO("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_INFO("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+template <typename T>
+uint8_t ICM45686::RegisterRead(T reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+template <typename T>
+void ICM45686::RegisterWrite(T reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+template <typename T>
+void ICM45686::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
+{
+	const uint8_t orig_val = RegisterRead(reg);
+
+	uint8_t val = (orig_val & ~clearbits) | setbits;
+
+	if (orig_val != val) {
+		RegisterWrite(reg, val);
+	}
+}
+
+uint16_t ICM45686::FIFOReadCount()
+{
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNT_0) | DIR_READ;
+
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
+	// FIFO_COUNT_0 is supposed to contain the high bits and FIFO_COUNT_1 the low bits,
+	// according to the manual, however, the device is configured to little endianness
+	// which means FIFO and FIFO count are pre-swapped..
+	return combine(fifo_count_buf[2], fifo_count_buf[1]);
+}
+
+bool ICM45686::FIFORead(const hrt_abstime &timestamp_sample)
+{
+	const uint16_t fifo_packets = FIFOReadCount();
+
+	if (fifo_packets == 0) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	FIFOTransferBuffer buffer{};
+	const size_t transfer_size = math::min(sizeof(FIFOTransferBuffer), fifo_packets * sizeof(FIFO::DATA) + 1);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	unsigned valid_samples = 0;
+
+	for (unsigned i = 0; i < transfer_size / sizeof(FIFO::DATA); i++) {
+		bool valid = true;
+
+		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
+
+		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
+			// FIFO sample empty if HEADER_MSG set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
+			// accel bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
+			// gyro bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
+			// Packet does not contain a new and valid extended 20-bit data
+			valid = false;
+
+		} else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) {
+			// Packet does not contain ODR timestamp
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+			// accel ODR changed
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+			// gyro ODR changed
+			valid = false;
+		}
+
+		if (valid) {
+			valid_samples++;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			break;
+		}
+	}
+
+	if (valid_samples > 0) {
+		if (ProcessTemperature(buffer.f, valid_samples)) {
+			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void ICM45686::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// Disable FIFO
+	RegisterClearBits(Register::BANK_0::FIFO_CONFIG3,
+			  FIFO_CONFIG3_BIT::FIFO_ES1_EN |
+			  FIFO_CONFIG3_BIT::FIFO_ES0_EN |
+			  FIFO_CONFIG3_BIT::FIFO_HIRES_EN |
+			  FIFO_CONFIG3_BIT::FIFO_GYRO_EN |
+			  FIFO_CONFIG3_BIT::FIFO_ACCEL_EN |
+			  FIFO_CONFIG3_BIT::FIFO_IF_EN);
+
+	// Disable FIFO by switching to bypass mode
+	RegisterSetAndClearBits(Register::BANK_0::FIFO_CONFIG0,
+				FIFO_CONFIG0_BIT::FIFO_MODE_BYPASS_SET,
+				FIFO_CONFIG0_BIT::FIFO_MODE_BYPASS_CLEAR);
+
+	// When the FIFO is disabled we can actually set the FIFO depth
+	RegisterSetBits(Register::BANK_0::FIFO_CONFIG0, FIFO_CONFIG0_BIT::FIFO_DEPTH_8K_SET);
+
+	// And then enable FIFO again
+	RegisterSetAndClearBits(Register::BANK_0::FIFO_CONFIG0, FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_SET,
+				FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_CLEAR);
+
+	// And enable again
+	RegisterSetBits(Register::BANK_0::FIFO_CONFIG3,
+			FIFO_CONFIG3_BIT::FIFO_HIRES_EN |
+			FIFO_CONFIG3_BIT::FIFO_GYRO_EN |
+			FIFO_CONFIG3_BIT::FIFO_ACCEL_EN |
+			FIFO_CONFIG3_BIT::FIFO_IF_EN);
+}
+
+void ICM45686::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;
+
+	// 19-bits of accelerometer data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+
+
+		if (_enable_clock_input) {
+			// Swapped as device is in little endian by default.
+			const uint16_t timestamp_fifo = combine_uint(fifo[i].Timestamp_L, fifo[i].Timestamp_H);
+			accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+		} else {
+			accel.dt = FIFO_TIMESTAMP_SCALING;
+		}
+
+		// 20 bit hires mode
+		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+		// Accel data is 18 bit ()
+		int32_t accel_x = reassemble_20bit(
+					  fifo[i].ACCEL_DATA_XL,
+					  fifo[i].ACCEL_DATA_XH,
+					  fifo[i].HIGHRES_X_LSB & 0xF0 >> 4);
+		int32_t accel_y = reassemble_20bit(
+					  fifo[i].ACCEL_DATA_YL,
+					  fifo[i].ACCEL_DATA_YH,
+					  fifo[i].HIGHRES_Y_LSB & 0xF0 >> 4);
+		int32_t accel_z = reassemble_20bit(
+					  fifo[i].ACCEL_DATA_ZL,
+					  fifo[i].ACCEL_DATA_ZH,
+					  fifo[i].HIGHRES_Z_LSB & 0xF0 >> 4);
+
+		// sample invalid if -524288
+		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+
+			// It's not enough to check if any values are exceeding the
+			// int16 limits because there might be a rotation applied later.
+			// If a rotation is 45 degrees, the new component can be up to
+			// sqrt(2) longer than one component. This means the number has
+			// to be constrained to fit the int16 which then triggers
+			// clipping.
+			//
+			// Therefore, we set the limits at int16_max/min / sqrt(2) plus
+			// a bit of margin.
+			static constexpr int16_t max_accel = static_cast<int16_t>(INT16_MAX / sqrt(2.f)) - 100;
+			static constexpr int16_t min_accel = static_cast<int16_t>(INT16_MIN / sqrt(2.f)) + 100;
+
+			if (accel_x >= max_accel || accel_x <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_y >= max_accel || accel_y <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_z >= max_accel || accel_z <= min_accel) {
+				scale_20bit = true;
+			}
+
+			// least significant bit is always 0)
+			accel.x[accel.samples] = accel_x / 2;
+			accel.y[accel.samples] = accel_y / 2;
+			accel.z[accel.samples] = accel_z / 2;
+			accel.samples++;
+		}
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled accel data is always 8192 LSB/g
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			// 20 bit hires mode
+			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+			// Accel data is 18 bit ()
+			int16_t accel_x = combine(fifo[i].ACCEL_DATA_XL, fifo[i].ACCEL_DATA_XH);
+			int16_t accel_y = combine(fifo[i].ACCEL_DATA_YL, fifo[i].ACCEL_DATA_YH);
+			int16_t accel_z = combine(fifo[i].ACCEL_DATA_ZL, fifo[i].ACCEL_DATA_ZH);
+
+			accel.x[i] = accel_x;
+			accel.y[i] = accel_y;
+			accel.z[i] = accel_z;
+		}
+
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f * 8.0f);
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < accel.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		accel.x[i] = accel.x[i];
+		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
+		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
+	}
+
+	_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);
+	}
+}
+
+void ICM45686::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = 0;
+
+	// 20-bits of gyroscope data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+
+
+		if (_enable_clock_input) {
+			// Swapped as device is in little endian by default.
+			uint16_t timestamp_fifo = combine_uint(fifo[i].Timestamp_L, fifo[i].Timestamp_H);
+			gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+		} else {
+			gyro.dt = FIFO_TIMESTAMP_SCALING;
+		}
+
+		// 20 bit hires mode
+		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_XL, fifo[i].GYRO_DATA_XH, fifo[i].HIGHRES_X_LSB & 0x0F);
+		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_YL, fifo[i].GYRO_DATA_YH, fifo[i].HIGHRES_Y_LSB & 0x0F);
+		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_ZL, fifo[i].GYRO_DATA_ZH, fifo[i].HIGHRES_Z_LSB & 0x0F);
+
+		// It's not enough to check if any values are exceeding the
+		// int16 limits because there might be a rotation applied later.
+		// If a rotation is 45 degrees, the new component can be up to
+		// sqrt(2) longer than one component. This means the number has
+		// to be constrained to fit the int16 which then triggers
+		// clipping.
+		//
+		// Therefore, we set the limits at int16_max/min / sqrt(2) plus
+		// a bit of margin.
+		static constexpr int16_t max_gyro = static_cast<int16_t>(INT16_MAX / sqrt(2.f)) - 100;
+		static constexpr int16_t min_gyro = static_cast<int16_t>(INT16_MIN / sqrt(2.f)) + 100;
+
+		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		gyro.x[gyro.samples] = gyro_x;
+		gyro.y[gyro.samples] = gyro_y;
+		gyro.z[gyro.samples] = gyro_z;
+		gyro.samples++;
+	}
+
+	if (!scale_20bit) {
+		// if highres enabled gyro data is always 131 LSB/dps
+		_px4_gyro.set_scale(math::radians(1.f / 131.f));
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			gyro.x[i] = combine(fifo[i].GYRO_DATA_XL, fifo[i].GYRO_DATA_XH);
+			gyro.y[i] = combine(fifo[i].GYRO_DATA_YL, fifo[i].GYRO_DATA_YH);
+			gyro.z[i] = combine(fifo[i].GYRO_DATA_ZL, fifo[i].GYRO_DATA_ZH);
+		}
+
+		_px4_gyro.set_scale(math::radians(1.f / 131.f * 16.0f));
+	}
+
+	// correct frame for publication
+	for (int i = 0; i < gyro.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		gyro.x[i] = gyro.x[i];
+		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
+		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
+	}
+
+	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+	if (gyro.samples > 0) {
+		_px4_gyro.updateFIFO(gyro);
+	}
+}
+
+bool ICM45686::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
+{
+	int16_t temperature[FIFO_MAX_SAMPLES];
+	float temperature_sum{0};
+
+	int valid_samples = 0;
+
+	for (int i = 0; i < samples; i++) {
+		// Swapped as device is in little endian by default.
+		const int16_t t = combine(fifo[i].TEMP_DATA_L, fifo[i].TEMP_DATA_H);
+
+		// sample invalid if -32768
+		if (t != -32768) {
+			temperature_sum += t;
+			temperature[valid_samples] = t;
+			valid_samples++;
+		}
+	}
+
+	if (valid_samples > 0) {
+		const float temperature_avg = temperature_sum / valid_samples;
+
+		for (int i = 0; i < valid_samples; i++) {
+			// temperature changing wildly is an indication of a transfer error
+			if (fabsf(temperature[i] - temperature_avg) > 1000) {
+				perf_count(_bad_transfer_perf);
+				return false;
+			}
+		}
+
+		// use average temperature reading
+		const float temp_c = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(temp_c)) {
+			_px4_accel.set_temperature(temp_c);
+			_px4_gyro.set_temperature(temp_c);
+			return true;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
+	}
+
+	return false;
+}

src/drivers/imu/invensense/icm45686/ICM45686.hpp

@@ -0,0 +1,165 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ICM45686.hpp
+ *
+ * Driver for the Invensense ICM45686 connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "InvenSense_ICM45686_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>
+
+using namespace InvenSense_ICM45686;
+
+class ICM45686 : public device::SPI, public I2CSPIDriver<ICM45686>
+{
+public:
+	ICM45686(const I2CSPIDriverConfig &config);
+	~ICM45686() 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};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+
+	static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input
+
+	// 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(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::BANK_0::FIFO_DATA) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	} __attribute__((packed));
+	// ensure padding is right
+	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_bank0_config_t {
+		Register::BANK_0 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Reset();
+
+	bool Configure();
+	void ConfigureSampleRate(int sample_rate);
+	void ConfigureFIFOWatermark(uint8_t samples);
+	void ConfigureCLKIN();
+
+	template <typename T> bool RegisterCheck(const T &reg_cfg);
+	template <typename T> uint8_t RegisterRead(T reg);
+	template <typename T> void RegisterWrite(T reg, uint8_t value);
+	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
+	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
+	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample);
+	void FIFOReset();
+
+	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
+
+	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")};
+
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+	int _failure_count{0};
+
+	bool _enable_clock_input{false};
+	float _input_clock_freq{0.f};
+
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_RESET,
+		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_bank0{0};
+	static constexpr uint8_t size_register_bank0_cfg{9};
+	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
+		{ Register::BANK_0::INT1_CONFIG0, 0, 0},
+		{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
+
+		{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_UI_FS_SEL_4000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_6400_HZ_SET, GYRO_CONFIG0_BIT::GYRO_UI_FS_SEL_4000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_6400_HZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_32_G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_6400_HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_32_G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_6400_HZ_CLEAR },
+		{ Register::BANK_0::FIFO_CONFIG4, 0, FIFO_CONFIG4_BIT::FIFO_COMP_EN },
+		{ Register::BANK_0::FIFO_CONFIG0, FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_SET | FIFO_CONFIG0_BIT::FIFO_DEPTH_8K_SET, FIFO_CONFIG0_BIT::FIFO_MODE_STOP_ON_FULL_CLEAR | FIFO_CONFIG0_BIT::FIFO_DEPTH_8K_CLEAR },
+		{ Register::BANK_0::FIFO_CONFIG3, FIFO_CONFIG3_BIT::FIFO_HIRES_EN | FIFO_CONFIG3_BIT::FIFO_GYRO_EN | FIFO_CONFIG3_BIT::FIFO_ACCEL_EN | FIFO_CONFIG3_BIT::FIFO_IF_EN, 0 },
+
+		{ Register::BANK_0::RTC_CONFIG, 0, 0}, // RTC_MODE[5] set at runtime
+		{ Register::BANK_0::IOC_PAD_SCENARIO_OVRD, 0, 0}, // PADS_INT2_CFG_OVRD and PADS_INT2_CFG_OVRD_VAL set at runtime
+	};
+};

src/drivers/imu/invensense/icm45686/InvenSense_ICM45686_registers.hpp

@@ -0,0 +1,266 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file InvenSense_ICM45686_registers.hpp
+ *
+ * Invensense ICM-45686 registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+namespace InvenSense_ICM45686
+{
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0xE9;
+
+static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
+static constexpr float TEMPERATURE_OFFSET = 25.f; // C
+
+namespace Register
+{
+
+enum class BANK_0 : uint8_t {
+	PWR_MGMT0 = 0x10,
+	FIFO_COUNT_0 = 0x12,
+	FIFO_COUNT_1 = 0x13,
+	FIFO_DATA = 0x14,
+
+	INT1_CONFIG0 = 0x16,
+	INT1_CONFIG1 = 0x17,
+	INT1_CONFIG2 = 0x18,
+	INT1_STATUS0 = 0x19,
+	ACCEL_CONFIG0 = 0x1B,
+	GYRO_CONFIG0 = 0x1C,
+	FIFO_CONFIG0 = 0x1D,
+	FIFO_CONFIG1_0 = 0x1E,
+	FIFO_CONFIG1_1 = 0x1F,
+	FIFO_CONFIG2 = 0x20,
+	FIFO_CONFIG3 = 0x21,
+	FIFO_CONFIG4 = 0x22,
+	RTC_CONFIG = 0x26,
+	DMP_EXT_SEN_ODR_CFG = 0x27,
+	EDMP_APEX_EN0 = 0x29,
+	EDMP_APEX_EN1 = 0x2A,
+	APEX_BUFFER_MGMT = 0x2B,
+	INTF_CONFIG0 = 0x2C,
+	INTF_CONFIG1_OVRD = 0x2D,
+	INTF_AUX_CONFIG = 0x2E,
+	IOC_PAD_SCENARIO = 0x2F,
+	IOC_PAD_SCENARIO_AUX_OVRD = 0x30,
+	IOC_PAD_SCENARIO_OVRD = 0x31,
+	DRIVE_CONFIG0 = 0x32,
+	DRIVE_CONFIG1 = 0x33,
+	DRIVE_CONFIG2 = 0x34,
+	INT_APEX_CONFIG1 = 0x3a,
+	INT_APEX_STATUS0 = 0x3b,
+	INT_APEX_STATUS1 = 0x3c,
+
+	INT2_CONFIG0 = 0x56,
+	INT2_CONFIG1 = 0x57,
+	INT2_CONFIG2 = 0x58,
+	INT2_STATUS0 = 0x59,
+
+	WHO_AM_I = 0x72,
+	REG_MISC2 = 0x7F,
+};
+
+};
+
+//---------------- BANK0 Register bits
+
+// PWR_MGMT0
+enum PWR_MGMT0_BIT : uint8_t {
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+};
+
+enum INT1_STATUS0 : uint8_t {
+	INT1_STATUS_RESET_DONE = Bit7,
+	INT1_STATUS_AUX1_AGC = Bit6,
+	INT1_STATUS_AP_AGC_RDY = Bit5,
+	INT1_STATUS_AP_FSYNC = Bit4,
+	INT1_STATUS_AP_AUX1_DRDY = Bit3,
+	INT1_STATUS_AP_DRDY = Bit2,
+	INT1_STATUS_FIFO_THS = Bit1,
+	INT1_STATUS_FIFO_FULL = Bit0,
+};
+
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	ACCEL_UI_FS_SEL_32_G_SET = 0,
+	ACCEL_UI_FS_SEL_32_G_CLEAR = Bit6 | Bit5 | Bit4,
+	ACCEL_UI_FS_SEL_16_G_SET = Bit4,
+	ACCEL_UI_FS_SEL_16_G_CLEAR = Bit6 | Bit5,
+	ACCEL_UI_FS_SEL_8_G_SET = Bit5,
+	ACCEL_UI_FS_SEL_8_G_CLEAR = Bit6 | Bit4,
+	ACCEL_ODR_6400_HZ_SET = Bit0 | Bit1,
+	ACCEL_ODR_6400_HZ_CLEAR = Bit2,
+	ACCEL_ODR_3200_HZ_SET = Bit2,
+	ACCEL_ODR_3200_HZ_CLEAR = Bit0 | Bit1,
+	ACCEL_ODR_1600_HZ_SET = Bit2 | Bit0,
+	ACCEL_ODR_1600_HZ_CLEAR = Bit1,
+	ACCEL_ODR_800_HZ_SET = Bit2 | Bit1,
+	ACCEL_ODR_800_HZ_CLEAR = Bit0,
+};
+
+enum GYRO_CONFIG0_BIT : uint8_t {
+	GYRO_UI_FS_SEL_4000_DPS_SET = 0,
+	GYRO_UI_FS_SEL_4000_DPS_CLEAR = Bit7 | Bit6 | Bit5 | Bit4,
+	GYRO_UI_FS_SEL_2000_DPS_SET = Bit4,
+	GYRO_UI_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
+	GYRO_UI_FS_SEL_1000_DPS_SET = Bit5,
+	GYRO_UI_FS_SEL_1000_DPS_CLEAR = Bit7 | Bit6 | Bit4,
+	GYRO_ODR_6400_HZ_SET = Bit0 | Bit1,
+	GYRO_ODR_6400_HZ_CLEAR = Bit2,
+	GYRO_ODR_3200_HZ_SET = Bit2,
+	GYRO_ODR_3200_HZ_CLEAR = Bit0 | Bit1,
+	GYRO_ODR_1600_HZ_SET = Bit2 | Bit0,
+	GYRO_ODR_1600_HZ_CLEAR = Bit1,
+	GYRO_ODR_800_HZ_SET = Bit2 | Bit1,
+	GYRO_ODR_800_HZ_CLEAR = Bit0,
+};
+
+enum FIFO_CONFIG0_BIT : uint8_t {
+	FIFO_MODE_BYPASS_SET = 0,
+	FIFO_MODE_BYPASS_CLEAR = Bit6 | Bit7,
+	FIFO_MODE_STREAM_SET = Bit6,
+	FIFO_MODE_STREAM_CLEAR = Bit7,
+	FIFO_MODE_STOP_ON_FULL_SET = Bit7,
+	FIFO_MODE_STOP_ON_FULL_CLEAR = Bit6,
+	FIFO_DEPTH_2K_SET = Bit0 | Bit1 | Bit2,
+	FIFO_DEPTH_2K_CLEAR = Bit3 | Bit4,
+	FIFO_DEPTH_8K_SET = Bit0 | Bit1 | Bit2 | Bit3 | Bit4,
+	FIFO_DEPTH_8K_CLEAR = 0,
+};
+
+enum FIFO_CONFIG2_BIT : uint8_t {
+	FIFO_FLUSH = Bit7,
+	FIFO_WR_WM_GT_TH_EQUAL = 0,
+	FIFO_WR_WM_GT_TH_GREATER_THAN = Bit3,
+};
+
+enum FIFO_CONFIG3_BIT : uint8_t {
+	FIFO_ES1_EN = Bit5, // External sensor 1 data insertion into FIFO frame
+	FIFO_ES0_EN = Bit4, // External sensor 0 data insertion into FIFO frame
+	FIFO_HIRES_EN = Bit3, // High resolution accel and gyro data insertion into FIFO frame
+	FIFO_GYRO_EN = Bit2, // Gyro data insertion into FIFO frame
+	FIFO_ACCEL_EN = Bit1, // Accel data insertion into FIFO frame
+	FIFO_IF_EN = Bit0, // Enable FIFO
+};
+
+enum FIFO_CONFIG4_BIT : uint8_t {
+	FIFO_COMP_EN = Bit2, // FIFO compression enabled
+	FIFO_TMST_FSYNC_EN = Bit1, // Timestamp/FSYNC data inserted into FIFO frame
+};
+
+enum RTC_CONFIG_BIT : uint8_t {
+	RTC_ALIGN = Bit6, // Re-align command is generated by writing 1 to this bit
+	RTC_MODE = Bit5, // 0: RTC functionality not enabled, 1: RTC functionality enabled
+};
+
+enum IOC_PAD_SCENARIO_OVRD_BIT : uint8_t {
+	PADS_INT2_CFG_OVRD = Bit2, // Override enable for PADS_INT2_CFG, 0: disable, 1: enable
+	PADS_INT2_CFG_OVRD_INT2 = 0,
+	PADS_INT2_CFG_OVRD_FSYNC = Bit0,
+	PADS_INT2_CFG_OVRD_CLKIN = Bit1,
+};
+
+enum REG_MISC2_BIT : uint8_t {
+	SOFT_RST = Bit1, // 1: Triggers soft reset operation
+};
+
+
+// IPREG_TOP1
+//static constexpr uint8_t BANK_IPREG_TOP1 = 0xA2;
+//static constexpr uint8_t SREG_CTRL = 0x67;
+//enum SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIT : uint8_t {
+//   SREG_CTRL_SREG_DATA_ENDIAN_SEL_BIG = Bit1, // big endian as documented (instead of default little endian)
+//};
+
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 8192;
+
+struct DATA {
+	uint8_t FIFO_Header;
+	uint8_t ACCEL_DATA_XH; // Accel X [19:12]
+	uint8_t ACCEL_DATA_XL; // Accel X [11:4]
+	uint8_t ACCEL_DATA_YH; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_YL; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_ZH; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_ZL; // Accel Z [11:4]
+	uint8_t GYRO_DATA_XH;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_XL;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_YH;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_YL;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_ZH;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_ZL;  // Gyro Z [11:4]
+	uint8_t TEMP_DATA_H;    // Temperature[15:8]
+	uint8_t TEMP_DATA_L;    // Temperature[7:0]
+	uint8_t Timestamp_H;   // Timestamp[15:8]
+	uint8_t Timestamp_L;   // Timestamp[7:0]
+	uint8_t HIGHRES_X_LSB; // Accel X LSB [3:0] Gyro X LSB [3:0]
+	uint8_t HIGHRES_Y_LSB; // Accel Y LSB [3:0] Gyro Y LSB [3:0]
+	uint8_t HIGHRES_Z_LSB; // Accel Z LSB [3:0] Gyro Z LSB [3:0]
+};
+
+// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+enum FIFO_HEADER_BIT : uint8_t {
+	HEADER_MSG             = Bit7, // 1: FIFO is empty
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
+	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp
+	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
+};
+
+}
+} // namespace InvenSense_ICM42688P

src/drivers/imu/invensense/icm45686/Kconfig

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

src/drivers/imu/invensense/icm45686/icm45686_main.cpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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 "ICM45686.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+void ICM45686::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int icm45686_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = ICM45686;
+	BusCLIArguments cli{false, true};
+	cli.default_spi_frequency = SPI_SPEED;
+
+	while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) {
+		switch (ch) {
+		case 'C':
+			cli.custom1 = atoi(cli.optArg());
+			break;
+
+		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_ICM45686);
+
+	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/tecs/TECS.cpp

@@ -148,6 +148,8 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
 	}
 
 	// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
+	_alt_rate_ref = setpoint.alt_rate;
+
 	if (PX4_ISFINITE(setpoint.alt_rate)) {
 		_alt_rate_ref = setpoint.alt_rate;
 
@@ -317,9 +319,17 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In
 float TECSControl::_calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param &param) const
 {
 	float altitude_rate_output;
-	altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
-			       param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate + setpoint.altitude_rate_setpoint;
-	altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
+
+	if (PX4_ISFINITE(input.altitude_rate_sp)) {
+		// Control only altitude rate if a valid setpoint is specified
+		altitude_rate_output = input.altitude_rate_sp;
+		altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
+
+	} else {
+		altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
+				       param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate + setpoint.altitude_rate_setpoint;
+		altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
+	}
 
 	return altitude_rate_output;
 }
@@ -650,6 +660,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 
 	const TECSControl::Input control_input{ .altitude = altitude,
 						.altitude_rate = altitude_rate,
+						.altitude_rate_sp = 0.0f,
 						.tas = eas_to_tas * equivalent_airspeed,
 						.tas_rate = 0.0f};
 
@@ -724,6 +735,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 
 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
+							.altitude_rate_sp = hgt_rate_sp,
 							.tas = eas_to_tas * eas.speed,
 							.tas_rate = eas_to_tas * eas.speed_rate};
 

src/lib/tecs/TECS.hpp

@@ -260,6 +260,7 @@ public:
 	struct Input {
 		float altitude;		///< Current altitude amsl of the UAS [m].
 		float altitude_rate;	///< Current altitude rate of the UAS [m/s].
+		float altitude_rate_sp; ///< Current altitude rate setpoint [m/s]
 		float tas;		///< Current true airspeed of the UAS [m/s].
 		float tas_rate;		///< Current true airspeed rate of the UAS [m/s²].
 	};

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -41,7 +41,7 @@
  */
 
 #include "ekf.h"
-#include "python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h"
+#include "python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h"
 
 #include <mathlib/mathlib.h>
 #include <cstdlib>
@@ -59,17 +59,17 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
 
 		const float R_YAW = sq(fmaxf(gps_sample.yaw_acc, _params.gps_heading_noise));
 
-		float heading_innov;
+		float heading_pred;
 		float heading_innov_var;
 
 		{
 		Vector24f H;
-		sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
+		sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
 		}
 
 		gnss_yaw.observation = measured_hdg;
 		gnss_yaw.observation_variance = R_YAW;
-		gnss_yaw.innovation = heading_innov;
+		gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg);
 		gnss_yaw.innovation_variance = heading_innov_var;
 
 		gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw;
@@ -93,12 +93,12 @@ void Ekf::fuseGpsYaw()
 	Vector24f H;
 
 	{
-	float heading_innov;
+	float heading_pred;
 	float heading_innov_var;
 
 	// Note: we recompute innov and innov_var because it doesn't cost much more than just computing H
 	// making a separate function just for H uses more flash space without reducing CPU load significantly
-	sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation, gnss_yaw.observation_variance, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
 	}
 
 	const SparseVector24f<0,1,2,3> Hfusion(H);

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

@@ -387,11 +387,10 @@ def compute_flow_y_innov_var_and_h(
 
     return (innov_var, Hy.T)
 
-def compute_gnss_yaw_innon_innov_var_and_h(
+def compute_gnss_yaw_pred_innov_var_and_h(
         state: VState,
         P: MState,
         antenna_yaw_offset: sf.Scalar,
-        meas: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (sf.Scalar, sf.Scalar, VState):
@@ -411,9 +410,7 @@ def compute_gnss_yaw_innon_innov_var_and_h(
     H = sf.V1(meas_pred).jacobian(state)
     innov_var = (H * P * H.T + R)[0,0]
 
-    innov = meas_pred - meas
-
-    return (innov, innov_var, H.T)
+    return (meas_pred, innov_var, H.T)
 
 def predict_drag(
         state: VState,
@@ -524,7 +521,7 @@ generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["
 generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])
 generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
 generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
-generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"])
+generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
 generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])

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

@@ -1,103 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: compute_gnss_yaw_innon_innov_var_and_h
- *
- * Args:
- *     state: Matrix24_1
- *     P: Matrix24_24
- *     antenna_yaw_offset: Scalar
- *     meas: Scalar
- *     R: Scalar
- *     epsilon: Scalar
- *
- * Outputs:
- *     innov: Scalar
- *     innov_var: Scalar
- *     H: Matrix24_1
- */
-template <typename Scalar>
-void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                                     const matrix::Matrix<Scalar, 24, 24>& P,
-                                     const Scalar antenna_yaw_offset, const Scalar meas,
-                                     const Scalar R, const Scalar epsilon,
-                                     Scalar* const innov = nullptr,
-                                     Scalar* const innov_var = nullptr,
-                                     matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 106
-
-  // Input arrays
-
-  // Intermediate terms (28)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp3 = std::sin(antenna_yaw_offset);
-  const Scalar _tmp4 = state(0, 0) * state(3, 0);
-  const Scalar _tmp5 = state(1, 0) * state(2, 0);
-  const Scalar _tmp6 = std::cos(antenna_yaw_offset);
-  const Scalar _tmp7 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + 2 * _tmp6 * (_tmp4 + _tmp5);
-  const Scalar _tmp8 = 2 * _tmp3 * (-_tmp4 + _tmp5) + _tmp6 * (-_tmp0 + _tmp1 + _tmp2);
-  const Scalar _tmp9 = _tmp8 + epsilon * ((((_tmp8) > 0) - ((_tmp8) < 0)) + Scalar(0.5));
-  const Scalar _tmp10 = 2 * state(3, 0);
-  const Scalar _tmp11 = 2 * state(0, 0);
-  const Scalar _tmp12 = -_tmp10 * _tmp3 + _tmp11 * _tmp6;
-  const Scalar _tmp13 = Scalar(1.0) / (_tmp9);
-  const Scalar _tmp14 = _tmp10 * _tmp6;
-  const Scalar _tmp15 = _tmp11 * _tmp3;
-  const Scalar _tmp16 = std::pow(_tmp9, Scalar(2));
-  const Scalar _tmp17 = _tmp7 / _tmp16;
-  const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp7, Scalar(2)));
-  const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15));
-  const Scalar _tmp20 = 2 * state(1, 0);
-  const Scalar _tmp21 = 2 * state(2, 0);
-  const Scalar _tmp22 = _tmp20 * _tmp6 + _tmp21 * _tmp3;
-  const Scalar _tmp23 = _tmp20 * _tmp3;
-  const Scalar _tmp24 = _tmp21 * _tmp6;
-  const Scalar _tmp25 = _tmp18 * (_tmp13 * (-_tmp23 + _tmp24) - _tmp17 * _tmp22);
-  const Scalar _tmp26 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15));
-  const Scalar _tmp27 = _tmp18 * (_tmp13 * _tmp22 - _tmp17 * (_tmp23 - _tmp24));
-
-  // Output terms (3)
-  if (innov != nullptr) {
-    Scalar& _innov = (*innov);
-
-    _innov = -meas + std::atan2(_tmp7, _tmp9);
-  }
-
-  if (innov_var != nullptr) {
-    Scalar& _innov_var = (*innov_var);
-
-    _innov_var =
-        R + _tmp19 * (P(0, 3) * _tmp26 + P(1, 3) * _tmp25 + P(2, 3) * _tmp27 + P(3, 3) * _tmp19) +
-        _tmp25 * (P(0, 1) * _tmp26 + P(1, 1) * _tmp25 + P(2, 1) * _tmp27 + P(3, 1) * _tmp19) +
-        _tmp26 * (P(0, 0) * _tmp26 + P(1, 0) * _tmp25 + P(2, 0) * _tmp27 + P(3, 0) * _tmp19) +
-        _tmp27 * (P(0, 2) * _tmp26 + P(1, 2) * _tmp25 + P(2, 2) * _tmp27 + P(3, 2) * _tmp19);
-  }
-
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
-
-    _h.setZero();
-
-    _h(0, 0) = _tmp26;
-    _h(1, 0) = _tmp25;
-    _h(2, 0) = _tmp27;
-    _h(3, 0) = _tmp19;
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

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

@@ -0,0 +1,99 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_gnss_yaw_pred_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     antenna_yaw_offset: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     meas_pred: Scalar
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                                    const matrix::Matrix<Scalar, 24, 24>& P,
+                                    const Scalar antenna_yaw_offset, const Scalar R,
+                                    const Scalar epsilon, Scalar* const meas_pred = nullptr,
+                                    Scalar* const innov_var = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 101
+
+  // Input arrays
+
+  // Intermediate terms (26)
+  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp3 = std::sin(antenna_yaw_offset);
+  const Scalar _tmp4 = state(0, 0) * state(3, 0);
+  const Scalar _tmp5 = state(1, 0) * state(2, 0);
+  const Scalar _tmp6 = std::cos(antenna_yaw_offset);
+  const Scalar _tmp7 = 2 * _tmp6;
+  const Scalar _tmp8 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + _tmp7 * (_tmp4 + _tmp5);
+  const Scalar _tmp9 = 2 * _tmp3;
+  const Scalar _tmp10 = _tmp6 * (-_tmp0 + _tmp1 + _tmp2) + _tmp9 * (-_tmp4 + _tmp5);
+  const Scalar _tmp11 = _tmp10 + epsilon * ((((_tmp10) > 0) - ((_tmp10) < 0)) + Scalar(0.5));
+  const Scalar _tmp12 = _tmp7 * state(0, 0) - _tmp9 * state(3, 0);
+  const Scalar _tmp13 = Scalar(1.0) / (_tmp11);
+  const Scalar _tmp14 = _tmp7 * state(3, 0);
+  const Scalar _tmp15 = _tmp9 * state(0, 0);
+  const Scalar _tmp16 = std::pow(_tmp11, Scalar(2));
+  const Scalar _tmp17 = _tmp8 / _tmp16;
+  const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp8, Scalar(2)));
+  const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15));
+  const Scalar _tmp20 = _tmp7 * state(1, 0) + _tmp9 * state(2, 0);
+  const Scalar _tmp21 = _tmp9 * state(1, 0);
+  const Scalar _tmp22 = _tmp7 * state(2, 0);
+  const Scalar _tmp23 = _tmp18 * (_tmp13 * (-_tmp21 + _tmp22) - _tmp17 * _tmp20);
+  const Scalar _tmp24 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15));
+  const Scalar _tmp25 = _tmp18 * (_tmp13 * _tmp20 - _tmp17 * (_tmp21 - _tmp22));
+
+  // Output terms (3)
+  if (meas_pred != nullptr) {
+    Scalar& _meas_pred = (*meas_pred);
+
+    _meas_pred = std::atan2(_tmp8, _tmp11);
+  }
+
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var =
+        R + _tmp19 * (P(0, 3) * _tmp24 + P(1, 3) * _tmp23 + P(2, 3) * _tmp25 + P(3, 3) * _tmp19) +
+        _tmp23 * (P(0, 1) * _tmp24 + P(1, 1) * _tmp23 + P(2, 1) * _tmp25 + P(3, 1) * _tmp19) +
+        _tmp24 * (P(0, 0) * _tmp24 + P(1, 0) * _tmp23 + P(2, 0) * _tmp25 + P(3, 0) * _tmp19) +
+        _tmp25 * (P(0, 2) * _tmp24 + P(1, 2) * _tmp23 + P(2, 2) * _tmp25 + P(3, 2) * _tmp19);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = _tmp24;
+    _h(1, 0) = _tmp23;
+    _h(2, 0) = _tmp25;
+    _h(3, 0) = _tmp19;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/test/test_EKF_gnss_yaw_generated.cpp

@@ -35,7 +35,7 @@
 #include "EKF/ekf.h"
 #include "test_helper/comparison_helper.h"
 
-#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h"
+#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h"
 
 using namespace matrix;
 
@@ -140,11 +140,11 @@ TEST(GnssYawFusionGenerated, SympyVsSymforce)
 	Vector24f K_sympy;
 	sympyGnssYawInnovVarHAndK(q(0), q(1), q(2), q(3), P, yaw_offset, R_YAW, innov_var_sympy, H_sympy, K_sympy);
 
-	float innov_symforce;
+	float meas_pred_symforce;
 	float innov_var_symforce;
 	Vector24f H_symforce;
-	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov_symforce,
-					     &innov_var_symforce, &H_symforce);
+	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred_symforce,
+					    &innov_var_symforce, &H_symforce);
 
 	// K isn't generated from symbolic anymore to save flash space
 	Vector24f K_symforce = P * H_symforce / innov_var_symforce;
@@ -177,11 +177,11 @@ TEST(GnssYawFusionGenerated, SingularityPitch90)
 	SquareMatrix24f P = createRandomCovarianceMatrix24f();
 	const float R_YAW = sq(0.3f);
 
-	float innov;
+	float meas_pred;
 	float innov_var;
 	Vector24f H;
-	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov,
-					     &innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
+					    &innov_var, &H);
 	Vector24f K = P * H / innov_var;
 
 	// THEN: the arctan is singular, the attitude isn't observable, so the innovation variance
@@ -205,11 +205,11 @@ TEST(GnssYawFusionGenerated, SingularityRoll90)
 	SquareMatrix24f P = createRandomCovarianceMatrix24f();
 	const float R_YAW = sq(0.3f);
 
-	float innov;
+	float meas_pred;
 	float innov_var;
 	Vector24f H;
-	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov,
-					     &innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
+					    &innov_var, &H);
 	Vector24f K = P * H / innov_var;
 
 	// THEN: the arctan is singular, the attitude isn't observable, so the innovation variance

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -1119,7 +1119,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   false,
+				   pos_sp_curr.vz);
 }
 
 void