drivers/adc/board_adc: count number of times USB has been connected (disconnected -> connected)

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

@@ -209,6 +209,7 @@
           <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">
@@ -637,6 +638,7 @@
           <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">
@@ -653,6 +655,7 @@
           <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">
@@ -669,6 +672,7 @@
           <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">
@@ -803,7 +807,7 @@
       <sub_topic>servo_3</sub_topic>
       <p_gain>10.0</p_gain>
     </plugin>
-    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+    <plugin filename="ignition-gazebo-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,6 +208,7 @@
           <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'>
@@ -271,6 +272,7 @@
           <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'>
@@ -334,6 +336,7 @@
           <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'>
@@ -397,6 +400,7 @@
           <spring_reference>0</spring_reference>
           <spring_stiffness>0</spring_stiffness>
         </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
       </axis>
     </joint>
 
@@ -462,6 +466,7 @@
           <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,6 +302,7 @@
           <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">
@@ -374,6 +375,7 @@
           <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">
@@ -446,6 +448,7 @@
           <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">
@@ -518,9 +521,10 @@
           <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-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+   <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::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>x500</uri>
+      <uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</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='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'>
+    <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'>
       <render_engine>ogre2</render_engine>
     </plugin>
     <gui fullscreen='false'>
       <plugin name='3D View' filename='GzScene3D'>
-        <gz-gui>
+        <ignition-gui>
           <title>3D View</title>
           <property type='bool' key='showTitleBar'>0</property>
           <property type='string' key='state'>docked</property>
-        </gz-gui>
+        </ignition-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'>
-        <gz-gui>
+        <ignition-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>
-        </gz-gui>
+        </ignition-gui>
         <play_pause>1</play_pause>
         <step>1</step>
         <start_paused>1</start_paused>
       </plugin>
       <plugin name='World stats' filename='WorldStats'>
-        <gz-gui>
+        <ignition-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>
-        </gz-gui>
+        </ignition-gui>
         <sim_time>1</sim_time>
         <real_time>1</real_time>
         <real_time_factor>1</real_time_factor>

boards/cubepilot/cubeorangeplus/default.px4board

@@ -19,7 +19,6 @@ 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
@@ -50,6 +49,7 @@ 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,6 +66,7 @@ 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,27 +4,12 @@
 #------------------------------------------------------------------------------
 board_adc start
 
-# 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
+# SPI4
 ms5611 -s -b 4 start
+icm42688p -s -b 4 -R 10 start
+icm20948 -s -b 4 -R 10 -M start
 
-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
-
+# SPI1
 ms5611 -s -b 1 start
+icm20649 -s -b 1 start
 

boards/cubepilot/cubeorangeplus/src/board_config.h

@@ -44,16 +44,6 @@
 #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,7 +38,6 @@
 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
 	}),
 
@@ -49,7 +48,6 @@ 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
 	}),
 };

msg/SystemPower.msg

@@ -1,15 +1,20 @@
-uint64 timestamp		# time since system start (microseconds)
-float32 voltage5v_v		# peripheral 5V rail voltage
-float32[4] sensors3v3		# Sensors 3V3 rail voltage
-uint8 sensors3v3_valid		# Sensors 3V3 rail voltage was read (bitfield).
-uint8 usb_connected		# USB is connected when 1
-uint8 brick_valid		# brick bits power is good when bit 1
-uint8 usb_valid 		# USB is valid when 1
-uint8 servo_valid		# servo power is good when 1
-uint8 periph_5v_oc		# peripheral overcurrent when 1
-uint8 hipower_5v_oc		# high power peripheral overcurrent when 1
-uint8 comp_5v_valid		# 5V to companion valid
-uint8 can1_gps1_5v_valid	# 5V for CAN1/GPS1 valid
+uint64 timestamp                # time since system start (microseconds)
+
+float32 voltage5v_v             # peripheral 5V rail voltage
+float32[4] sensors3v3           # Sensors 3V3 rail voltage
+uint8 sensors3v3_valid          # Sensors 3V3 rail voltage was read (bitfield).
+
+bool usb_connected		# USB is connected when true
+uint8 usb_connected_count       # number of times USB has been connected
+
+uint8 brick_valid		# brick bits power is good (bitfield).
+bool usb_valid                  # USB is valid when true
+
+bool servo_valid                # servo power is good when true
+bool periph_5v_oc               # peripheral overcurrent when true
+bool hipower_5v_oc              # high power peripheral overcurrent when true
+bool comp_5v_valid              # 5V to companion valid
+bool can1_gps1_5v_valid         # 5V for CAN1/GPS1 valid
 
 uint8 BRICK1_VALID_SHIFTS=0
 uint8 BRICK1_VALID_MASK=1

src/drivers/adc/board_adc/ADC.cpp

@@ -244,7 +244,17 @@ void ADC::update_system_power(hrt_abstime now)
 	// these are not ADC related, but it is convenient to
 	// publish these to the same topic
 
-	system_power.usb_connected = BOARD_ADC_USB_CONNECTED;
+	const bool usb_connected = BOARD_ADC_USB_CONNECTED;
+
+	if (!_usb_connected && usb_connected) {
+		_usb_connected_count++;
+	}
+
+	_usb_connected = usb_connected;
+
+	system_power.usb_connected = _usb_connected;
+	system_power.usb_connected_count = _usb_connected_count;
+
 	/* If provided used the Valid signal from HW*/
 #if defined(BOARD_ADC_USB_VALID)
 	system_power.usb_valid = BOARD_ADC_USB_VALID;

src/drivers/adc/board_adc/ADC.hpp

@@ -113,6 +113,9 @@ private:
 	uORB::Publication<adc_report_s>		_to_adc_report{ORB_ID(adc_report)};
 	uORB::Publication<system_power_s>	_to_system_power{ORB_ID(system_power)};
 
+	uint8_t _usb_connected_count{0};
+	bool _usb_connected{false};
+
 #ifdef BOARD_GPIO_VDD_5V_COMP_VALID
 	int _5v_comp_valid_fd {-1};
 #endif

src/drivers/drv_sensor.h

@@ -84,7 +84,6 @@
 #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

@@ -1,48 +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__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

@@ -1,752 +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 "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

@@ -1,165 +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.
- *
- ****************************************************************************/
-
-/**
- * @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

@@ -1,266 +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.
- *
- ****************************************************************************/
-
-/**
- * @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

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

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

@@ -1,92 +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 "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,8 +148,6 @@ 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;
 
@@ -319,17 +317,9 @@ 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;
-
-	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);
-	}
+	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;
 }
@@ -660,7 +650,6 @@ 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};
 
@@ -735,7 +724,6 @@ 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,7 +260,6 @@ 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_pred_innov_var_and_h.h"
+#include "python/ekf_derivation/generated/compute_gnss_yaw_innon_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_pred;
+		float heading_innov;
 		float heading_innov_var;
 
 		{
 		Vector24f H;
-		sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
+		sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
 		}
 
 		gnss_yaw.observation = measured_hdg;
 		gnss_yaw.observation_variance = R_YAW;
-		gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg);
+		gnss_yaw.innovation = heading_innov;
 		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_pred;
+	float heading_innov;
 	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::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
+	sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation, gnss_yaw.observation_variance, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
 	}
 
 	const SparseVector24f<0,1,2,3> Hfusion(H);

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

@@ -387,10 +387,11 @@ def compute_flow_y_innov_var_and_h(
 
     return (innov_var, Hy.T)
 
-def compute_gnss_yaw_pred_innov_var_and_h(
+def compute_gnss_yaw_innon_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):
@@ -410,7 +411,9 @@ def compute_gnss_yaw_pred_innov_var_and_h(
     H = sf.V1(meas_pred).jacobian(state)
     innov_var = (H * P * H.T + R)[0,0]
 
-    return (meas_pred, innov_var, H.T)
+    innov = meas_pred - meas
+
+    return (innov, innov_var, H.T)
 
 def predict_drag(
         state: VState,
@@ -521,7 +524,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_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
+generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"])
 generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])

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

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

@@ -1,99 +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_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_pred_innov_var_and_h.h"
+#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_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 meas_pred_symforce;
+	float innov_symforce;
 	float innov_var_symforce;
 	Vector24f H_symforce;
-	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred_symforce,
-					    &innov_var_symforce, &H_symforce);
+	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov_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 meas_pred;
+	float innov;
 	float innov_var;
 	Vector24f H;
-	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
-					    &innov_var, &H);
+	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov,
+					     &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 meas_pred;
+	float innov;
 	float innov_var;
 	Vector24f H;
-	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
-					    &innov_var, &H);
+	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov,
+					     &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,9 +1119,7 @@ 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(),
-				   false,
-				   pos_sp_curr.vz);
+				   _param_climbrate_target.get());
 }
 
 void

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -75,7 +75,7 @@ void LandingTargetEstimator::update()
 	/* predict */
 	if (_estimator_initialized) {
 		if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) {
-			PX4_INFO("Lost sight of Marker");
+			PX4_WARN("Timeout");
 			_estimator_initialized = false;
 
 		} else {
@@ -129,7 +129,7 @@ void LandingTargetEstimator::update()
 		if (!update_x || !update_y) {
 			if (!_faulty) {
 				_faulty = true;
-				PX4_INFO("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
+				PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
 			}
 
 		} else {
@@ -264,7 +264,7 @@ void LandingTargetEstimator::_update_topics()
 		}
 
 		if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) {
-			PX4_WARN("Marker position reading invalid!");
+			PX4_WARN("Position is corrupt!");
 			return;
 		}