Fix heightrate feedforard for fixdwings

cubeorangeplus: add check for SMPS support
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>
2026-06-02 13:00:04 +08:00 · 2023-03-27 13:13:24 +02:00 · 2023-03-13 22:54:41 -04:00 · 2023-03-13 22:53:56 -04:00 · 2023-03-13 22:53:56 -04:00 · 2023-03-13 22:53:56 -04:00
53 changed files with 1816 additions and 437 deletions
@@ -170,7 +170,7 @@
            ]
        },
        {
-            "label": "ign gazebo",
+            "label": "gazebo",
            "type": "shell",
            "options": {
                "cwd": "${workspaceFolder}",
@@ -178,7 +178,7 @@
                    "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gz/models",
                }
            },
-            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
+            "command": "gz sim -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
            "isBackground": true,
            "presentation": {
                "echo": true,
@@ -191,7 +191,7 @@
                "close": false
            },
            "problemMatcher": [],
-            "dependsOn":["ign gazebo kill"]
+            "dependsOn":["gazebo kill"]
        },
        {
            "label": "gazebo-classic kill",
@@ -211,9 +211,9 @@
            "dependsOn":["px4_sitl_cleanup"]
        },
        {
-            "label": "ign gazebo kill",
+            "label": "gazebo kill",
            "type": "shell",
-            "command": "pkill -9 -f 'ign gazebo' || true",
+            "command": "pkill -9 -f 'gz sim' || true",
            "presentation": {
                "echo": true,
                "reveal": "never",
@@ -1,8 +1,6 @@

 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p

-# shellcheck disable=SC2154
-mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
 # shellcheck disable=SC2154
 mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
 mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
@@ -8,6 +8,14 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"

 	echo "INFO  [init] SIH simulator"

+	if [ -n "${PX4_HOME_LAT}" ]; then
+		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
+	fi
+
+	if [ -n "${PX4_HOME_LON}" ]; then
+		param set SIH_LOC_LON0 ${PX4_HOME_LON}
+	fi
+
 	if simulator_sih start; then

 		if param compare -s SENS_EN_BAROSIM 1
@@ -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>
@@ -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>

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

@@ -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"
@@ -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
 	}),
 };
@@ -22,13 +22,10 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050

-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_ACC_COMP 0
-param set-default ATT_W_ACC 0.4000
-param set-default ATT_W_GYRO_BIAS 0.0000
-
+# use EKF2 without mag
 param set-default SYS_HAS_MAG 0
+# and enable gravity fusion
+param set-default EKF2_IMU_CONTROL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -41,11 +38,5 @@ param set-default SYS_DM_BACKEND 1
 # Ignore that there is no SD card
 param set-default COM_ARM_SDCARD 0

-# Store missions in RAM
-param set-default SYS_DM_BACKEND 1
-
-# Ignore that there is no SD card
-param set-default COM_ARM_SDCARD 0
-
 # Don't try to log onto SD card
 param set-default SDLOG_MODE -1
@@ -22,13 +22,10 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050

-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_ACC_COMP 0
-param set-default ATT_W_ACC 0.4000
-param set-default ATT_W_GYRO_BIAS 0.0000
-
+# use EKF2 without mag
 param set-default SYS_HAS_MAG 0
+# and enable gravity fusion
+param set-default EKF2_IMU_CONTROL 7

 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -17,6 +17,7 @@ uint32 mode_req_offboard_signal
 uint32 mode_req_home_position
 uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
 uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
+uint32 mode_req_manual_control
 uint32 mode_req_other             # other requirements, not covered above (for external modes)


@@ -347,12 +347,12 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 					data_offset += sizeof(uint64_t);

 				} else if (strcmp(c_type, "float") == 0) {
-					if (!dont_print) { PX4_INFO_RAW("%.4f", (double) * (float *)(data_ptr + data_offset)); }
+					if (!dont_print) { PX4_INFO_RAW("%.5f", (double) * (float *)(data_ptr + data_offset)); }

 					data_offset += sizeof(float);

 				} else if (strcmp(c_type, "double") == 0) {
-					if (!dont_print) { PX4_INFO_RAW("%.4f", *(double *)(data_ptr + data_offset)); }
+					if (!dont_print) { PX4_INFO_RAW("%.6f", *(double *)(data_ptr + data_offset)); }

 					data_offset += sizeof(double);

@@ -14,11 +14,11 @@
            "environment": [
                {
                    "name": "PX4_SIM_MODEL",
-                    "value": "${input:PX4_GZ_MODEL}"
+                    "value": "gz_${input:PX4_GZ_MODEL}"
                }
            ],
            "externalConsole": false,
-            "postDebugTask": "ign gazebo kill",
+            "postDebugTask": "gazebo kill",
            "linux": {
                "MIMode": "gdb",
                "externalConsole": false,
@@ -222,6 +222,9 @@
            "description": "GZ vehicle model",
            "options": [
              "x500",
+              "x500_depth",
+              "rc_cessna",
+              "standard_vtol",
            ],
            "default": "x500"
        }
@@ -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
@@ -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
+	)
@@ -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;
+}
@@ -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
+	};
+};
@@ -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
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_INVENSENSE_ICM45686
+	bool "icm45686"
+	default n
+	---help---
+		Enable support for icm45686
@@ -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;
+}
@@ -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};

@@ -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²].
 	};
@@ -246,6 +246,8 @@ public:
 	void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
 				const events::LogLevels &log_levels, const char *message);

+	void clearArmingBits(NavModes modes);
+
 	/**
 	 * Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
 	 * mode is being run.
@@ -302,8 +304,6 @@ private:

 	NavModes reportedModes(NavModes required_modes);

-	void clearArmingBits(NavModes modes);
-
 	NavModes getModeGroup(uint8_t nav_state) const;

 	friend class HealthAndArmingChecks;
@@ -143,6 +143,26 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_home_position);
 	}

+	if (reporter.failsafeFlags().manual_control_signal_lost && reporter.failsafeFlags().mode_req_manual_control != 0) {
+		const bool rc_disabled = (_param_com_rc_in_mode.get() == 4);
+		NavModes nav_modes = rc_disabled ? (NavModes)reporter.failsafeFlags().mode_req_manual_control : NavModes::None;
+		events::LogLevel log_level = rc_disabled ? events::Log::Error : events::Log::Warning;
+
+		/* EVENT
+		 * @description
+		 * Connect and enable stick input or use autonomous mode.
+		 * <profile name="dev">
+		 * Sticks can be enabled via <param>COM_RC_IN_MODE</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(nav_modes,
+					    health_component_t::remote_control,
+					    events::ID("check_modes_manual_control"),
+					    log_level, "No manual control input");
+		reporter.clearArmingBits((NavModes)reporter.failsafeFlags().mode_req_manual_control);
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_manual_control);
+	}
+
 	if (reporter.failsafeFlags().mode_req_other != 0) {
 		// Here we expect there is already an event reported for the failing check (this is for external modes)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other);
@@ -49,6 +49,7 @@ private:
 	void checkArmingRequirement(const Context &context, Report &reporter);

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req
+					(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req,
+					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode
 				       );
 };
@@ -38,65 +38,40 @@ using namespace time_literals;
 void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter)
 {
 	// RC
-	bool rc_is_optional = true;
+	manual_control_setpoint_s manual_control_setpoint;

-	if (_param_com_rc_in_mode.get() == 4) { // RC disabled
-		reporter.failsafeFlags().manual_control_signal_lost = false;
+	if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+		manual_control_setpoint = {};
+		reporter.failsafeFlags().manual_control_signal_lost = true;
+	}
+
+	// Check if RC is valid
+	if (!manual_control_setpoint.valid
+	    || hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
+
+		if (!reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
+
+			events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
+				     "Manual control lost");
+		}
+
+		reporter.failsafeFlags().manual_control_signal_lost = true;

 	} else {
+		reporter.setIsPresent(health_component_t::remote_control);

-		manual_control_setpoint_s manual_control_setpoint;
-
-		if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
-			manual_control_setpoint = {};
-			reporter.failsafeFlags().manual_control_signal_lost = true;
+		if (reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
+			float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
+			events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
+					    "Manual control regained after {1:.1} s", elapsed);
 		}

-		// Check if RC is valid
-		if (!manual_control_setpoint.valid
-		    || hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
-
-			if (!reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
-
-				events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
-					     "Manual control lost");
-			}
-
-			reporter.failsafeFlags().manual_control_signal_lost = true;
-
-		} else {
-			reporter.setIsPresent(health_component_t::remote_control);
-
-			if (reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
-				float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
-				events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
-						    "Manual control regained after {1:.1} s", elapsed);
-			}
-
-			reporter.failsafeFlags().manual_control_signal_lost = false;
-			_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
-		}
-
-
-		if (reporter.failsafeFlags().manual_control_signal_lost) {
-
-			NavModes affected_modes = rc_is_optional ? NavModes::None : NavModes::All;
-			events::LogLevel log_level = rc_is_optional ? events::Log::Info : events::Log::Error;
-			/* EVENT
-			 * @description
-			 * <profile name="dev">
-			 * This check can be configured via <param>COM_RC_IN_MODE</param> parameter.
-			 * </profile>
-			 */
-			reporter.armingCheckFailure(affected_modes, health_component_t::remote_control, events::ID("check_rc_dl_no_rc"),
-						    log_level, "No manual control input");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: No manual control input\t");
-			}
-		}
+		reporter.failsafeFlags().manual_control_signal_lost = false;
+		_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
 	}

+	// Manual control check is in modeCheck as mode requirement
+
 	// GCS connection
 	reporter.failsafeFlags().gcs_connection_lost = context.status().gcs_connection_lost;

@@ -52,7 +52,6 @@ private:
 	hrt_abstime	_last_valid_manual_control_setpoint{0};

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
 					(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
 					(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act
 				       )
@@ -56,26 +56,29 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	flags.mode_req_home_position = 0;
 	flags.mode_req_wind_and_flight_time_compliance = 0;
 	flags.mode_req_prevent_arming = 0;
+	flags.mode_req_manual_control = 0;
 	flags.mode_req_other = 0;

 	// NAVIGATION_STATE_MANUAL
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_MANUAL, flags.mode_req_manual_control);

 	// NAVIGATION_STATE_ALTCTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_attitude);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_manual_control);

 	// NAVIGATION_STATE_POSCTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position_relaxed);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_manual_control);

 	if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);
 	}

-	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_alt);
-
 	// NAVIGATION_STATE_AUTO_MISSION
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude);
@@ -104,6 +107,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)

 	// NAVIGATION_STATE_ACRO
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_manual_control);

 	// NAVIGATION_STATE_DESCEND
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_angular_velocity);
@@ -122,6 +126,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	// NAVIGATION_STATE_STAB
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_manual_control);

 	// NAVIGATION_STATE_AUTO_TAKEOFF
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_angular_velocity);
@@ -656,6 +656,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
 		(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
 		(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
 		(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
+		(!status_flags.manual_control_signal_lost || ((status_flags.mode_req_manual_control & mode_mask) == 0)) &&
 		((status_flags.mode_req_other & mode_mask) == 0);
 }

@@ -355,7 +355,7 @@ struct parameters {
 	int32_t mag_declination_source{7};      ///< bitmask used to control the handling of declination data
 	int32_t mag_fusion_type{0};             ///< integer used to specify the type of magnetometer fusion used
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
-	float mag_yaw_rate_gate{0.25f};         ///< yaw rate threshold used by mode select logic (rad/sec)
+	float mag_yaw_rate_gate{0.20f};         ///< yaw rate threshold used by mode select logic (rad/sec)

 	// GNSS heading fusion
 	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
@@ -769,20 +769,47 @@ private:
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();

-	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
+	void clearInhibitedStateKalmanGains(Vector24f &K) const
 	{
+		// gyro bias: states 10, 11, 12
 		for (unsigned i = 0; i < 3; i++) {
-			// gyro bias: states 10, 11, 12
 			if (_gyro_bias_inhibit[i]) {
-				K(10 + i) = 0.0f;
-			}
-
-			// accel bias: states 13, 14, 15
-			if (_accel_bias_inhibit[i]) {
-				K(13 + i) = 0.0f;
+				K(10 + i) = 0.f;
 			}
 		}

+		// accel bias: states 13, 14, 15
+		for (unsigned i = 0; i < 3; i++) {
+			if (_accel_bias_inhibit[i]) {
+				K(13 + i) = 0.f;
+			}
+		}
+
+		// mag I: states 16, 17, 18
+		if (!_control_status.flags.mag_3D) {
+			K(16) = 0.f;
+			K(17) = 0.f;
+			K(18) = 0.f;
+		}
+
+		// mag B: states 19, 20, 21
+		if (!_control_status.flags.mag_3D) {
+			K(19) = 0.f;
+			K(20) = 0.f;
+			K(21) = 0.f;
+		}
+
+		// wind: states 22, 23
+		if (!_control_status.flags.wind) {
+			K(22) = 0.f;
+			K(23) = 0.f;
+		}
+	}
+
+	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
+	{
+		clearInhibitedStateKalmanGains(K);
+
 		const Vector24f KS = K * innovation_variance;
 		SquareMatrix24f KHP;

@@ -863,11 +890,10 @@ private:
 	// control fusion of magnetometer observations
 	void controlMagFusion();

-	void checkHaglYawResetReq();
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }

-	void runOnGroundYawReset();
-	void runInAirYawReset();
+	bool magReset();
+	bool haglYawResetReq();

 	void selectMagAuto();
 	void check3DMagFusionSuitability();
@@ -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);
@@ -206,18 +206,18 @@ void Ekf::controlMagFusion()
 			break;
 		}

-		const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {

-		if ((!mag_enabled_previously && mag_enabled) || mag_sample.reset) {
-			_mag_yaw_reset_req = true;
-		}
+			if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) {

-		if (_control_status.flags.in_air) {
-			checkHaglYawResetReq();
-			runInAirYawReset();
+				if (magReset()) {
+					_mag_yaw_reset_req = false;

-		} else {
-			runOnGroundYawReset();
+				} else {
+					// mag reset failed, try again next time
+					_mag_yaw_reset_req = true;
+				}
+			}
 		}

 		if (!_control_status.flags.yaw_align) {
@@ -231,90 +231,84 @@ void Ekf::controlMagFusion()
 	}
 }

-void Ekf::checkHaglYawResetReq()
+bool Ekf::haglYawResetReq()
 {
 	// We need to reset the yaw angle after climbing away from the ground to enable
 	// recovery from ground level magnetic interference.
-	if (!_control_status.flags.mag_aligned_in_flight) {
+	if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
 		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
-		_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
+		return above_mag_anomalies;
 	}
+
+	return false;
 }

-void Ekf::runOnGroundYawReset()
-{
-	if (_mag_yaw_reset_req) {
-		const bool has_realigned_yaw = resetMagHeading();
-
-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
-		}
-	}
-}
-
-void Ekf::runInAirYawReset()
+bool Ekf::magReset()
 {
 	// prevent a reset being performed more than once on the same frame
 	if ((_flt_mag_align_start_time == _time_delayed_us)
 	    || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
-		return;
+		return false;
 	}

-	if (_mag_yaw_reset_req) {
-		bool has_realigned_yaw = false;
+	bool has_realigned_yaw = false;

-		// use yaw estimator if available
-		if (_control_status.flags.gps && isYawEmergencyEstimateAvailable()
-		    && (_mag_counter > 1) // mag LPF available
-		   ) {
+	// use yaw estimator if available
+	if (_control_status.flags.gps && isYawEmergencyEstimateAvailable()
+	    && (_mag_counter > 1) // mag LPF available
+	   ) {

-			resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
+		resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());

-			_information_events.flags.yaw_aligned_to_imu_gps = true;
+		_information_events.flags.yaw_aligned_to_imu_gps = true;

-			// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-			if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
-				// use predicted earth field to reset states
-				const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
-				_state.mag_I = mag_earth_pred;
+		// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
+		if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
+			// use predicted earth field to reset states
+			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+			_state.mag_I = mag_earth_pred;

-				const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-				_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
+			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+			_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);

-			} else {
-				// Use the last magnetometer measurements to reset the field states
-				// calculate initial earth magnetic field states
-				_state.mag_I = _R_to_earth * _mag_lpf.getState();
-				_state.mag_B.zero();
-			}
-
-			ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]",
-				(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
-				(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
-				);
-
-			resetMagCov();
-
-			has_realigned_yaw = true;
+		} else {
+			// Use the last magnetometer measurements to reset the field states
+			// calculate initial earth magnetic field states
+			_state.mag_I = _R_to_earth * _mag_lpf.getState();
+			_state.mag_B.zero();
 		}

-		if (!has_realigned_yaw) {
-			has_realigned_yaw = resetMagHeading();
-		}
+		ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]",
+			(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
+			(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
+			);

-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
+		resetMagCov();
+
+		has_realigned_yaw = true;
+	}
+
+	if (!has_realigned_yaw) {
+		has_realigned_yaw = resetMagHeading();
+	}
+
+	if (has_realigned_yaw) {
+		_control_status.flags.yaw_align = true;
+
+		if (_control_status.flags.in_air) {
 			_control_status.flags.mag_aligned_in_flight = true;

 			// record the time for the magnetic field alignment event
 			_flt_mag_align_start_time = _time_delayed_us;
 		}
+
+		return true;
 	}
+
+	return false;
 }

 void Ekf::selectMagAuto()
@@ -452,9 +446,11 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)

 void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
-	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
-	// before they are used to constrain heading drift
-	const bool update_all_states = ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)5e6);
+	// sanity check mag_B before they are used to constrain heading drift
+	const Vector3f mag_bias_var = P.slice<3, 3>(19, 19).diag();
+	const bool mag_bias_var_good = (mag_bias_var.min() > 0.f) && (mag_bias_var.max() < sq(0.02f));
+
+	const bool update_all_states = _control_status.flags.mag_aligned_in_flight && mag_bias_var_good;

 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state
@@ -258,7 +258,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 	// only calculate gains for states we are using
 	Vector24f Kfusion;

-	for (uint8_t row = 0; row <= 15; row++) {
+	for (uint8_t row = 0; row < _k_num_states; row++) {
 		for (uint8_t col = 0; col <= 3; col++) {
 			Kfusion(row) += P(row, col) * H_YAW(col);
 		}
@@ -266,16 +266,6 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 		Kfusion(row) *= heading_innov_var_inv;
 	}

-	if (_control_status.flags.wind) {
-		for (uint8_t row = 22; row <= 23; row++) {
-			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row, col) * H_YAW(col);
-			}
-
-			Kfusion(row) *= heading_innov_var_inv;
-		}
-	}
-
 	// define the innovation gate size
 	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);

@@ -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"])
@@ -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
@@ -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
@@ -200,17 +200,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 		Kfusion(row) = P(row, state_index) / innov_var;
 	}

-	for (unsigned i = 0; i < 3; i++) {
-		// gyro bias: states 10, 11, 12
-		if (_gyro_bias_inhibit[i]) {
-			Kfusion(10 + i) = 0.0f;
-		}
-
-		// accel bias: states 13, 14, 15
-		if (_accel_bias_inhibit[i]) {
-			Kfusion(13 + i) = 0.0f;
-		}
-	}
+	clearInhibitedStateKalmanGains(Kfusion);

 	SquareMatrix24f KHP;

@@ -532,7 +532,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f);
 * @unit rad/s
 * @decimal 2
 */
-PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.25f);
+PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.20f);

 /**
 * Gate size for barometric and GPS height fusion
@@ -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
@@ -52,9 +52,7 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
 		_stick_acceleration_xy.resetVelocity(_velocity.xy());
 	}

-	if (Vector2f(last_setpoint.acceleration).isAllFinite()) {
-		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
-	}
+	_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));

 	return ret;
 }
@@ -86,7 +86,6 @@ protected:
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
 					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
-					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
 					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
 					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below which to land with land speed */
 					(ParamFloat<px4::params::MPC_LAND_SPEED>)
@@ -57,13 +57,17 @@ void StickAccelerationXY::resetPosition(const matrix::Vector2f &position)

 void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
 {
-	_velocity_setpoint = velocity;
+	if (velocity.isAllFinite()) {
+		_velocity_setpoint = velocity;
+	}
 }

 void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration)
 {
-	_acceleration_slew_rate_x.setForcedValue(acceleration(0));
-	_acceleration_slew_rate_y.setForcedValue(acceleration(1));
+	if (acceleration.isAllFinite()) {
+		_acceleration_slew_rate_x.setForcedValue(acceleration(0));
+		_acceleration_slew_rate_y.setForcedValue(acceleration(1));
+	}
 }

 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
@@ -150,7 +154,7 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo

 	drag_coefficient *= _brake_boost_filter.getState();

-	// increase drag with sqareroot function when velocity is lower than 1m/s
+	// increase drag with squareroot function when velocity is lower than 1m/s
 	const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm());
 	return drag_coefficient.emult(velocity_with_sqrt_boost);
 }
@@ -84,7 +84,6 @@ private:
 		(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
 		(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
 		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
-		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
-		(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max
 	)
 };
@@ -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
@@ -75,7 +75,7 @@ void LandingTargetEstimator::update()
 	/* predict */
 	if (_estimator_initialized) {
 		if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) {
-			PX4_WARN("Timeout");
+			PX4_INFO("Lost sight of Marker");
 			_estimator_initialized = false;

 		} else {
@@ -129,7 +129,7 @@ void LandingTargetEstimator::update()
 		if (!update_x || !update_y) {
 			if (!_faulty) {
 				_faulty = true;
-				PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
+				PX4_INFO("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("Position is corrupt!");
+			PX4_WARN("Marker position reading invalid!");
 			return;
 		}

@@ -8,8 +8,15 @@ menuconfig MAVLINK_DIALECT
 depends on MODULES_MAVLINK
        string "Mavlink dialect"
        default "common"
-    help
-        Select the Mavlink dialect to generate and use.
+	---help---
+		Select the Mavlink dialect to generate and use.
+
+menuconfig MAVLINK_UAVCAN_PARAMETERS
+depends on MODULES_MAVLINK && DRIVERS_UAVCAN
+        bool "Mavlink UAVCAN parameter support"
+        default y
+	---help---
+		Expose UAVCAN parameters over Mavlink.

 menuconfig USER_MAVLINK
 	bool "mavlink running as userspace module"
@@ -77,6 +77,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				}
 			}

+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
 			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 				// publish list request to UAVCAN driver via uORB.
@@ -88,6 +90,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				_uavcan_parameter_request_pub.publish(req);
 			}

+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 			break;
 		}

@@ -133,6 +136,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				}
 			}

+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
 			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
@@ -158,6 +163,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				_uavcan_parameter_request_pub.publish(req);
 			}

+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 			break;
 		}

@@ -208,6 +214,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				}
 			}

+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
 			    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
@@ -224,6 +232,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				request_next_uavcan_parameter();
 			}

+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 			break;
 		}

@@ -328,18 +337,22 @@ MavlinkParametersManager::send()
 bool
 MavlinkParametersManager::send_params()
 {
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 	if (send_uavcan()) {
 		return true;
+	}

-	} else if (send_one()) {
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
+	if (send_one()) {
 		return true;

 	} else if (send_untransmitted()) {
 		return true;
-
-	} else {
-		return false;
 	}
+
+	return false;
 }

 bool
@@ -393,63 +406,6 @@ MavlinkParametersManager::send_untransmitted()
 	return sent_one;
 }

-bool
-MavlinkParametersManager::send_uavcan()
-{
-	/* Send parameter values received from the UAVCAN topic */
-	uavcan_parameter_value_s value{};
-
-	if (_uavcan_parameter_value_sub.update(&value)) {
-
-		// Check if we received a matching parameter, drop it from the list and request the next
-		if ((_uavcan_open_request_list != nullptr)
-		    && (value.param_index == _uavcan_open_request_list->req.param_index)
-		    && (value.node_id == _uavcan_open_request_list->req.node_id)) {
-
-			dequeue_uavcan_request();
-			request_next_uavcan_parameter();
-		}
-
-		mavlink_param_value_t msg{};
-		msg.param_count = value.param_count;
-		msg.param_index = value.param_index;
-#if defined(__GNUC__) && __GNUC__ >= 8
-#pragma GCC diagnostic ignored "-Wstringop-truncation"
-#endif
-		/*
-		 * coverity[buffer_size_warning : FALSE]
-		 *
-		 * The MAVLink spec does not require the string to be NUL-terminated if it
-		 * has length 16. In this case the receiving end needs to terminate it
-		 * when copying it.
-		 */
-		strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-#if defined(__GNUC__) && __GNUC__ >= 8
-#pragma GCC diagnostic pop
-#endif
-
-		if (value.param_type == MAV_PARAM_TYPE_REAL32) {
-			msg.param_type = MAVLINK_TYPE_FLOAT;
-			msg.param_value = value.real_value;
-
-		} else {
-			int32_t val = (int32_t)value.int_value;
-			memcpy(&msg.param_value, &val, sizeof(int32_t));
-			msg.param_type = MAVLINK_TYPE_INT32_T;
-		}
-
-		// Re-pack the message with the UAVCAN node ID
-		mavlink_message_t mavlink_packet{};
-		mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
-						    &msg);
-		_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
-
-		return true;
-	}
-
-	return false;
-}
-
 bool
 MavlinkParametersManager::send_one()
 {
@@ -591,6 +547,64 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 	return 0;
 }

+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
+bool MavlinkParametersManager::send_uavcan()
+{
+	/* Send parameter values received from the UAVCAN topic */
+	uavcan_parameter_value_s value{};
+
+	if (_uavcan_parameter_value_sub.update(&value)) {
+
+		// Check if we received a matching parameter, drop it from the list and request the next
+		if ((_uavcan_open_request_list != nullptr)
+		    && (value.param_index == _uavcan_open_request_list->req.param_index)
+		    && (value.node_id == _uavcan_open_request_list->req.node_id)) {
+
+			dequeue_uavcan_request();
+			request_next_uavcan_parameter();
+		}
+
+		mavlink_param_value_t msg{};
+		msg.param_count = value.param_count;
+		msg.param_index = value.param_index;
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic ignored "-Wstringop-truncation"
+#endif
+		/*
+		 * coverity[buffer_size_warning : FALSE]
+		 *
+		 * The MAVLink spec does not require the string to be NUL-terminated if it
+		 * has length 16. In this case the receiving end needs to terminate it
+		 * when copying it.
+		 */
+		strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic pop
+#endif
+
+		if (value.param_type == MAV_PARAM_TYPE_REAL32) {
+			msg.param_type = MAVLINK_TYPE_FLOAT;
+			msg.param_value = value.real_value;
+
+		} else {
+			int32_t val = (int32_t)value.int_value;
+			memcpy(&msg.param_value, &val, sizeof(int32_t));
+			msg.param_type = MAVLINK_TYPE_INT32_T;
+		}
+
+		// Re-pack the message with the UAVCAN node ID
+		mavlink_message_t mavlink_packet{};
+		mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
+						    &msg);
+		_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
+
+		return true;
+	}
+
+	return false;
+}
+
 void MavlinkParametersManager::request_next_uavcan_parameter()
 {
 	// Request a parameter if we are not already waiting on a response and if the list is not empty
@@ -643,3 +657,5 @@ void MavlinkParametersManager::dequeue_uavcan_request()
 		_uavcan_waiting_for_request_response = false;
 	}
 }
+
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
@@ -49,11 +49,14 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/rc_parameter_map.h>
-#include <uORB/topics/uavcan_parameter_request.h>
-#include <uORB/topics/uavcan_parameter_value.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>

+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+# include <uORB/topics/uavcan_parameter_request.h>
+# include <uORB/topics/uavcan_parameter_value.h>
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
 using namespace time_literals;

 class Mavlink;
@@ -91,11 +94,6 @@ protected:
 	 */
 	bool send_params();

-	/**
-	 * Send UAVCAN params
-	 */
-	bool send_uavcan();
-
 	/**
 	 * Send untransmitted params
 	 */
@@ -103,6 +101,12 @@ protected:

 	int send_param(param_t param, int component_id = -1);

+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+	/**
+	 * Send UAVCAN params
+	 */
+	bool send_uavcan();
+
 	// Item of a single-linked list to store requested uavcan parameters
 	struct _uavcan_open_request_list_item {
 		uavcan_parameter_request_s req;
@@ -128,9 +132,6 @@ protected:
 	bool _uavcan_waiting_for_request_response{false}; ///< We have reqested a parameter and wait for the response
 	uint16_t _uavcan_queued_request_items{0};	///< Number of stored parameter requests currently in the list

-	uORB::Publication<rc_parameter_map_s>	_rc_param_map_pub{ORB_ID(rc_parameter_map)};
-	rc_parameter_map_s _rc_param_map{};
-
 	uORB::Publication<uavcan_parameter_request_s> _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)};
 	// enforce ORB_ID(uavcan_parameter_request) constants that map to MAVLINK defines
 	static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ,
@@ -149,6 +150,10 @@ protected:
 		      "uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch");

 	uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
+	uORB::Publication<rc_parameter_map_s>	_rc_param_map_pub{ORB_ID(rc_parameter_map)};
+	rc_parameter_map_s _rc_param_map{};

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	hrt_abstime _param_update_time{0};
Author	SHA1	Message	Date
Jaeyoung Lim	8904f6f487	Fix heightrate feedforard for fixdwings	2023-03-27 13:13:24 +02:00
Julian Oes	7be3279675	cubeorangeplus: add check for SMPS support 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>	2023-03-13 22:54:41 -04:00
Julian Oes	36f430e385	cubeorangeplus: save some flash space We need to make space for drivers. Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-13 22:53:56 -04:00
Julian Oes	bee4fe9470	boards: sensor config for CubeOrange+ Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-13 22:53:56 -04:00
Julian Oes	63dc6b5bc9	ICM45686: fix clipping due to rotation It turns out that when you rotate by 45 degrees, as required on the CubeOrange+, then you can easily get into clipping because the vector components are constrained after the rotation. In order to avoid that, we have to avoid getting close to the int16 range and switch from 20 bit resolution to 16bit resolution earlier. Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-13 22:53:56 -04:00
Julian Oes	f4b48e685f	drivers: add ICM45686 Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-13 22:53:56 -04:00
Beniamino Pozzan	82dce9353c	gz models: fix deprecated warnings (#21285 ) Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-13 12:28:20 -07:00
bresch	fd33e60f78	ekf: fix GNSS yaw fusion wrapping	2023-03-13 10:46:34 +01:00
akkawimo	3bae99267b	fix(precland): Improved log messages (#21289 )	2023-03-13 08:39:31 +01:00
Daniel Agar	9be8f81d75	flight_mode_manager: StickAccelerationXY protect from NAN velocity reset Co-authored-by: Matthias Grob <maetugr@gmail.com>	2023-03-10 08:17:20 -05:00
Daniel Agar	435c799f57	uORB: print more decimal places for float32 and float64	2023-03-10 07:39:34 +01:00
Frederic Taillandier	91f6ab865c	ROMFS: fix shellcheck error in px4-rc.simulator (#21282 )	2023-03-10 07:37:45 +01:00
Matthias Grob	bd5838faf0	FlightTask: don't instaniate unused parameters	2023-03-09 17:40:55 +01:00
Tony Samaritano	eb4da990c3	init.d-posix/px4-rc.simulator: adds non-default LAT and LON as optional environment variables	2023-03-09 09:40:35 -05:00
Daniel Agar	b3cc945a5a	ekf2: merge runOnGroundYawReset() + runInAirYawReset() into unified magReset()	2023-03-09 09:08:27 -05:00
Daniel Agar	c1f244a6fd	ekf2: decrease EKF2_MAG_YAWLIM default 0.25 -> 0.2 rad/s (#21264 )	2023-03-09 09:07:54 -05:00
Daniel Agar	60b85c2e1a	mavlink: add kconfig option to disable UAVCAN parameter bridge - depends on DRIVERS_UAVCAN	2023-03-08 19:30:06 -05:00
frederictaillandier	eb86cb85b7	removing MOUNT_ORIENTATION on udp_gcs_port_local from typhoon	2023-03-09 12:43:47 +13:00
Daniel Agar	4dda5a97d8	ekf2: mag_3d check mag bias variance before allowed to update all states (orientation)	2023-03-08 15:12:48 -05:00
Julian Oes	ea20217c1b	kakuteh7v2/mini: EKF2 is already the default	2023-03-08 10:48:31 -05:00
Julian Oes	593b3d250d	kakuteh7mini: remove duplicate param defaults Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-08 10:48:31 -05:00
Julian Oes	ed49ed3903	kakuteh7v2/mini: use EKF2 without mag by default This switches from attitude_estimator_q to EKF2 which should now work without mag when the params are set to SYS_HAS_MAG = 0 and EKF2_IMU_CTRL = 7 to enable gravity fusion. Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-08 10:48:31 -05:00
Matthias Grob	132e9d2439	modeCheck: add warning when RC enabled but not present	2023-03-08 09:32:56 +01:00
Matthias Grob	898c0ae5a8	mode_requirements: refactor order of setting flags	2023-03-08 09:32:56 +01:00
Matthias Grob	7fa8dfe2d2	rcAndDataLinkCheck: always update manual control availability and remove duplicate manual control check possibly it needs to be readded to give warning about RC enabled but not present.	2023-03-08 09:32:56 +01:00
Matthias Grob	f498b90c41	mode_requirements: add manual control for manual modes	2023-03-08 09:32:56 +01:00
Beniamino Pozzan	636dfdec6a	VScode: fix tasks.json and launch_sitl.json after ign -> gazebo renaming PX4_SIM model need the simulator (gz_) prefix Fix post debug task Add x500_depth, rc_cessna, standard_vtol Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-07 21:28:39 -05:00
Daniel Agar	d45aeae1de	ekf2: add and share centralized method to clear inhibited state Kalman gains	2023-03-07 13:27:57 -05:00