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-05-27 00:40: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
145 changed files with 2675 additions and 2767 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",
@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Gazebo x500 vision
+#
+# @type Quadrotor
+#
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_vision}
@@ -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
@@ -74,6 +74,7 @@ px4_add_romfs_files(
 	4002_gz_x500_depth
 	4003_gz_rc_cessna
 	4004_gz_standard_vtol
+	4005_gz_x500_vision

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -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
@@ -122,16 +122,12 @@ param set-default VT_TRANS_MIN_TM 15
 param set-default VT_B_TRANS_DUR 8
 param set-default VT_FWD_THRUST_SC 4
 param set-default VT_F_TRANS_DUR 1
-param set-default VT_B_REV_OUT 0.5
 param set-default VT_B_TRANS_THR 0.7
 param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4

 param set-default COM_RC_OVERRIDE 0

-
-
-
 param set-default CA_AIRFRAME 2

 param set-default CA_ROTOR_COUNT 5
@@ -54,14 +54,6 @@ param set-default CBRK_AIRSPD_CHK 162128
 # Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
 param set-default GND_MAX_ANG 3.1415

-param set-default RBCLW_BAUD 8
-param set-default RBCLW_COUNTS_REV 1200
-param set-default RBCLW_ADDRESS 128
-# 104 corresponds to Telem 4
-param set-default RBCLW_SER_CFG 104
-# Start this driver after setting parameters, because the driver uses some of those parameters.
-# roboclaw start /dev/ttyS3
-
 # Set geometry & output configration
 param set-default CA_AIRFRAME 6
 param set-default CA_R_REV 3
@@ -102,7 +102,7 @@ class ModuleDocumentation(object):
    def _handle_usage_param_int(self, args):
        assert(len(args) == 6) # option_char, default_val, min_val, max_val, description, is_optional
        option_char = self._get_option_char(args[0])
-        default_val = int(args[1], 0)
+        default_val = self._get_int(args[1])
        description = self._get_string(args[4])
        if self._is_bool_true(args[5]):
            self._usage_string += "     [-%s <val>]  %s\n" % (option_char, description)
@@ -214,6 +214,9 @@ class ModuleDocumentation(object):
            f = f[:-1]
        return float(f)

+    def _get_int(self, argument):
+        return int(eval(argument))
+
    def _is_string(self, argument):
        return len(argument) > 0 and argument[0] == '"'

@@ -307,6 +310,8 @@ class SourceParser(object):
            r'//.*?$|/\*.*?\*/|\'(?:\\.|[^\\\'])*\'|"(?:\\.|[^\\"])*"',
            re.DOTALL | re.MULTILINE)

+        self._define_pattern = re.compile(r'#define\s+(\w+?)[^\S\r\n]+(.+?)\s*?\n')
+
    def Parse(self, scope, contents):
        """
        Incrementally parse program contents and append all found documentations
@@ -316,6 +321,9 @@ class SourceParser(object):
        # remove comments from source
        contents = self._comment_remover(contents)

+        # replace preprocessor defines defined in file directly
+        contents = self._define_replacer(contents)
+
        extracted_function_calls = [] # list of tuples: (FUNC_NAME, list(ARGS))

        start_index = 0
@@ -379,6 +387,15 @@ class SourceParser(object):
                return s
        return re.sub(self._comment_remove_pattern, replacer, text)

+    def _define_replacer(self, text):
+        """ check for C preprocesor #define in text and replace with argument"""
+        text = re.sub(r"\\\s*?\n"," ",text)
+        define_iter = self._define_pattern.finditer(text)
+        for define_pattern in define_iter:
+            text = re.sub(r"\b" +re.escape(str(define_pattern.groups()[0])) + r"\b", re.escape(str(define_pattern.groups()[1])), text)
+        return text
+
+
    def _do_consistency_check(self, contents, scope, module_doc):
        """
        check the documentation for consistency with the code (arguments to
@@ -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>
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>x500-vision</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Jaeyoung Lim</name>
+    <email>jalim@ethz.ch</email>
+  </author>
+  <description>Model of the X500 with a odometry/external vision input.</description>
+</model>
@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500-vision'>
+    <include merge='true'>
+      <uri>x500</uri>
+    </include>
+      <plugin
+        filename="gz-sim-odometry-publisher-system"
+        name="gz::sim::systems::OdometryPublisher">
+        <dimensions>3</dimensions>
+      </plugin>
+  </model>
+</sdf>
@@ -5,22 +5,22 @@
      <real_time_factor>1.0</real_time_factor>
      <real_time_update_rate>250</real_time_update_rate>
    </physics>
-    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
-    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
-    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
-    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
-    <plugin name='ignition::gazebo::systems::Imu' filename='ignition-gazebo-imu-system'/>
-    <plugin name='ignition::gazebo::systems::AirPressure' filename='ignition-gazebo-air-pressure-system'/>
-    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
+    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
+    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
+    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
+    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
+    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
+    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
+    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
      <render_engine>ogre2</render_engine>
    </plugin>
    <gui fullscreen='false'>
      <plugin name='3D View' filename='GzScene3D'>
-        <ignition-gui>
+        <gz-gui>
          <title>3D View</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='string' key='state'>docked</property>
-        </ignition-gui>
+        </gz-gui>
        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>0.5984631152222222 0.5984631152222222 0.5984631152222222</ambient_light>
@@ -28,7 +28,7 @@
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
      </plugin>
      <plugin name='World control' filename='WorldControl'>
-        <ignition-gui>
+        <gz-gui>
          <title>World control</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -40,13 +40,13 @@
            <line own='left' target='left'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </ignition-gui>
+        </gz-gui>
        <play_pause>1</play_pause>
        <step>1</step>
        <start_paused>1</start_paused>
      </plugin>
      <plugin name='World stats' filename='WorldStats'>
-        <ignition-gui>
+        <gz-gui>
          <title>World stats</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -58,7 +58,7 @@
            <line own='right' target='right'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </ignition-gui>
+        </gz-gui>
        <sim_time>1</sim_time>
        <real_time>1</real_time>
        <real_time_factor>1</real_time_factor>
@@ -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
@@ -12,6 +12,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_MUORB_SLPI=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_COMMANDER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
@@ -5,13 +5,8 @@ uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
 uint8 INDEX_YAW = 2
 uint8 INDEX_THROTTLE = 3
-uint8 INDEX_FLAPS = 4
-uint8 INDEX_SPOILERS = 5
-uint8 INDEX_AIRBRAKES = 6
-uint8 INDEX_LANDING_GEAR = 7
 uint8 INDEX_GIMBAL_SHUTTER = 3
 uint8 INDEX_CAMERA_ZOOM = 4
-uint8 INDEX_COLLECTIVE_TILT = 8

 uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
@@ -5,4 +5,4 @@ uint32 noutputs				# valid outputs
 float32[16] output				# output data, in natural output units

 # actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])
-# TOPICS actuator_outputs actuator_outputs_sim
+# TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug
@@ -128,6 +128,7 @@ set(msg_files
 	MountOrientation.msg
 	ModeCompleted.msg
 	NavigatorMissionItem.msg
+	NormalizedUnsignedSetpoint.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg
 	OffboardControlMode.msg
@@ -178,6 +179,7 @@ set(msg_files
 	TaskStackInfo.msg
 	TecsStatus.msg
 	TelemetryStatus.msg
+	TiltrotorExtraControls.msg
 	TimesyncStatus.msg
 	TrajectoryBezier.msg
 	TrajectorySetpoint.msg
@@ -218,7 +220,6 @@ set(msg_files
 	VehicleTrajectoryBezier.msg
 	VehicleTrajectoryWaypoint.msg
 	VtolVehicleStatus.msg
-	WheelEncoders.msg
 	Wind.msg
 	YawEstimatorStatus.msg
 )
@@ -5,12 +5,14 @@ float32 esc_voltage					# Voltage measured from current ESC [V] - if supported
 float32 esc_current					# Current measured from current ESC [A] - if supported
 float32 esc_temperature					# Temperature measured from current ESC [degC] - if supported
 uint8 esc_address					# Address of current ESC (in most cases 1-8 / must be set by driver)
+uint8 esc_cmdcount					# Counter of number of commands

 uint8 esc_state					# State of ESC - depend on Vendor

 uint8 actuator_function				# actuator output function (one of Motor1...MotorN)

 uint16 failures					# Bitmask to indicate the internal ESC faults
+int8 esc_power					# Applied power 0-100 in % (negative values reserved)

 uint8 FAILURE_OVER_CURRENT = 0 			# (1 << 0)
 uint8 FAILURE_OVER_VOLTAGE = 1 			# (1 << 1)
@@ -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)


@@ -26,7 +26,7 @@ float32 pitch    # move forward, negative pitch rotation, nose down
 float32 yaw      #               positive yaw rotation,   clockwise when seen top down
 float32 throttle # move up,      positive thrust,         -1 is minimum available 0% or -100% +1 is 100% thrust

-float32 flaps			 # flap position
+float32 flaps			 # position of flaps switch/knob/lever [-1, 1]

 float32 aux1
 float32 aux2
@@ -0,0 +1,5 @@
+uint64 timestamp        		# time since system start (microseconds)
+
+float32 normalized_setpoint          	# [0, 1]
+
+# TOPICS flaps_setpoint spoilers_setpoint
@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 collective_tilt_normalized_setpoint	# Collective tilt angle of motors of tiltrotor, 0: vertical, 1: horizontal [0, 1]
+float32 collective_thrust_normalized_setpoint 	# Collective thrust setpoint [0, 1]
@@ -17,14 +17,4 @@ bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)

 bool fw_control_yaw_wheel	# control heading with steering wheel (used for auto takeoff on runway)

-uint8 apply_flaps       	# flap config specifier
-uint8 FLAPS_OFF = 0     	# no flaps
-uint8 FLAPS_LAND = 1    	# landing config flaps
-uint8 FLAPS_TAKEOFF = 2 	# take-off config flaps
-
-uint8 apply_spoilers		# spoiler config specifier
-uint8 SPOILERS_OFF = 0     	# no spoilers
-uint8 SPOILERS_LAND = 1    	# landing config spoiler
-uint8 SPOILERS_DESCEND = 2 	# descend config spoiler
-
 # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
@@ -1,5 +0,0 @@
-uint64 timestamp			# time since system start (microseconds)
-
-int64 encoder_position   # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation
-int32 speed              # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse
-uint32 pulses_per_rev    # Number of pulses per revolution for each wheel
@@ -99,7 +99,10 @@ __END_DECLS

 #define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR

+// Qurt doesn't have an SD card for storage
+#ifndef __PX4_QURT
 #define PX4_STORAGEDIR PX4_ROOTFSDIR
+#endif

 /****************************************************************************
 * Defines for POSIX and ROS
@@ -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"
        }
@@ -105,7 +105,8 @@ static int create_dirs();
 static int run_startup_script(const std::string &commands_file, const std::string &absolute_binary_path, int instance);
 static std::string get_absolute_binary_path(const std::string &argv0);
 static void wait_to_exit();
-static bool is_server_running(int instance, bool server);
+static int get_server_running(int instance, bool *is_running);
+static int set_server_running(int instance);
 static void print_usage();
 static bool dir_exists(const std::string &path);
 static bool file_exists(const std::string &name);
@@ -124,6 +125,7 @@ int main(int argc, char **argv)
 {
 	bool is_client = false;
 	bool pxh_off = false;
+	bool server_is_running = false;

 	/* Symlinks point to all commands that can be used as a client with a prefix. */
 	const char prefix[] = PX4_SHELL_COMMAND_PREFIX;
@@ -131,6 +133,9 @@ int main(int argc, char **argv)

 	std::string absolute_binary_path; // full path to the px4 binary being executed

+	int ret = PX4_OK;
+	int instance = 0;
+
 	if (argc > 0) {
 		/* The executed binary name could start with a path, so strip it away */
 		const std::string full_binary_name = argv[0];
@@ -146,8 +151,6 @@ int main(int argc, char **argv)
 	}

 	if (is_client) {
-		int instance = 0;
-
 		if (argc >= 3 && strcmp(argv[1], "--instance") == 0) {
 			instance = strtoul(argv[2], nullptr, 10);
 			/* update argv so that "--instance <instance>" is not visible anymore */
@@ -160,15 +163,16 @@ int main(int argc, char **argv)

 		PX4_DEBUG("instance: %i", instance);

-		if (!is_server_running(instance, false)) {
-			if (errno) {
-				PX4_ERR("Failed to communicate with daemon: %s", strerror(errno));
+		ret = get_server_running(instance, &server_is_running);

-			} else {
-				PX4_ERR("PX4 daemon not running yet");
-			}
+		if (ret != PX4_OK) {
+			PX4_ERR("PX4 client failed to get server status");
+			return ret;
+		}

-			return -1;
+		if (!server_is_running) {
+			PX4_ERR("PX4 server not running");
+			return PX4_ERROR;
 		}

 		/* Remove the path and prefix. */
@@ -202,7 +206,6 @@ int main(int argc, char **argv)

 		bool working_directory_default = false;

-		int instance = 0;
 		bool instance_provided = false;

 		int myoptind = 1;
@@ -292,20 +295,27 @@ int main(int argc, char **argv)
 				PX4_INFO("working directory %s", working_directory.c_str());
 			}

-			int ret = change_directory(working_directory);
+			ret = change_directory(working_directory);

 			if (ret != PX4_OK) {
 				return ret;
 			}
 		}

-		if (is_server_running(instance, true)) {
-			// allow running multiple instances, but the server is only started for the first
-			PX4_INFO("PX4 daemon already running for instance %i (%s)", instance, strerror(errno));
-			return -1;
+		ret = get_server_running(instance, &server_is_running);
+
+		if (ret != PX4_OK) {
+			PX4_ERR("Failed to get server status");
+			return ret;
 		}

-		int ret = create_symlinks_if_needed(data_path);
+		if (server_is_running) {
+			// allow running multiple instances, but the server is only started for the first
+			PX4_INFO("PX4 server already running for instance %i", instance);
+			return PX4_ERROR;
+		}
+
+		ret = create_symlinks_if_needed(data_path);

 		if (ret != PX4_OK) {
 			return ret;
@@ -343,6 +353,13 @@ int main(int argc, char **argv)
 		px4::init_once();
 		px4::init(argc, argv, "px4");

+		// Don't set this up until PX4 is up and running
+		ret = set_server_running(instance);
+
+		if (ret != PX4_OK) {
+			return ret;
+		}
+
 		ret = run_startup_script(commands_file, absolute_binary_path, instance);

 		if (ret == 0) {
@@ -618,39 +635,73 @@ void print_usage()
 	printf("        e.g.: px4-commander status\n");
 }

-bool is_server_running(int instance, bool server)
+int get_server_running(int instance, bool *is_server_running)
 {
 	const std::string file_lock_path = std::string(LOCK_FILE_PATH) + '-' + std::to_string(instance);
 	int fd = open(file_lock_path.c_str(), O_RDWR | O_CREAT, 0666);

 	if (fd < 0) {
-		PX4_ERR("is_server_running: failed to create lock file: %s, reason=%s", file_lock_path.c_str(), strerror(errno));
-		return false;
+		PX4_ERR("%s: failed to create lock file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
+		return PX4_ERROR;
 	}

-	bool result = false;
+	int status = PX4_OK;
+	struct flock lock;
+	memset(&lock, 0, sizeof(struct flock));

-	// Server is running if the file is already locked.
-	if (flock(fd, LOCK_EX | LOCK_NB) < 0) {
-		if (errno == EWOULDBLOCK) {
-			// a server is running!
-			result = true;
+	// Exclusive write lock, cover the entire file (regardless of size)
+	lock.l_type = F_WRLCK;
+	lock.l_whence = SEEK_SET;
+
+	if (fcntl(fd, F_GETLK, &lock) < 0) {
+		PX4_ERR("%s: failed to get check for lock on file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
+		status = PX4_ERROR;
+
+	} else {
+		// F_GETLK will set l_type to F_UNLCK if no one had a lock on the file. Otherwise,
+		// it means that the server is running and has a lock on the file
+		if (lock.l_type != F_UNLCK) {
+			*is_server_running = true;

 		} else {
-			PX4_ERR("is_server_running: failed to get lock on file: %s, reason=%s", file_lock_path.c_str(), strerror(errno));
-			result = false;
+			*is_server_running = false;
 		}
 	}

-	if (result || !server) {
+	close(fd);
+
+	return status;
+}
+
+int set_server_running(int instance)
+{
+	const std::string file_lock_path = std::string(LOCK_FILE_PATH) + '-' + std::to_string(instance);
+	int fd = open(file_lock_path.c_str(), O_RDWR | O_CREAT, 0666);
+
+	if (fd < 0) {
+		PX4_ERR("%s: failed to create lock file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
+		return PX4_ERROR;
+	}
+
+	int status = PX4_OK;
+
+	struct flock lock;
+	memset(&lock, 0, sizeof(struct flock));
+
+	// Exclusive lock, cover the entire file (regardless of size).
+	lock.l_type = F_WRLCK;
+	lock.l_whence = SEEK_SET;
+
+	if (fcntl(fd, F_SETLK, &lock) < 0) {
+		PX4_ERR("%s: failed to set lock on file: %s, reason=%s", __func__, file_lock_path.c_str(), strerror(errno));
+		status = PX4_ERROR;
 		close(fd);
 	}

-	// note: server leaks the file handle once, on purpose, in order to keep the lock on the file until the process terminates.
+	// note: server leaks the file handle, on purpose, in order to keep the lock on the file until the process terminates.
 	// In this case we return false so the server code path continues now that we have the lock.

-	errno = 0;
-	return result;
+	return status;
 }

 bool file_exists(const std::string &name)
@@ -166,11 +166,12 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma
 				return -1;

 			} else {
-				//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
+				strcpy(taskmap[task_index].argv_storage[i], argv[i]);
 				taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i];
 			}

 		} else {
+			// Must add NULL at end of argv
 			taskmap[task_index].argv[i] = nullptr;
 			break;
 		}
@@ -420,13 +421,13 @@ int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
 	return 0;
 }

-int px4_prctl(int option, const char *arg2, pthread_t pid)
+int px4_prctl(int option, const char *arg2, px4_task_t pid)
 {
 	int rv = -1;
 	pthread_mutex_lock(&task_mutex);

 	for (int i = 0; i < PX4_MAX_TASKS; i++) {
-		if (taskmap[i].isused && taskmap[i].tid == pid) {
+		if (taskmap[i].isused && taskmap[i].tid == (pthread_t) pid) {
 			rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
 			return rv;
 		}
@@ -65,12 +65,13 @@ ModalIo::ModalIo() :
 		_esc_status.esc[i].esc_address     = 0;
 		_esc_status.esc[i].esc_rpm         = 0;
 		_esc_status.esc[i].esc_state       = 0;
-		//_esc_status.esc[i].esc_cmdcount    = 0;
+		_esc_status.esc[i].esc_cmdcount    = 0;
 		_esc_status.esc[i].esc_voltage     = 0;
 		_esc_status.esc[i].esc_current     = 0;
 		_esc_status.esc[i].esc_temperature = 0;
 		_esc_status.esc[i].esc_errorcount  = 0;
 		_esc_status.esc[i].failures        = 0;
+		_esc_status.esc[i].esc_power       = 0;
 	}

 	qc_esc_packet_init(&_fb_packet);
@@ -152,6 +153,8 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map)
 	param_get(param_find("MODAL_IO_RPM_MIN"), &params->rpm_min);
 	param_get(param_find("MODAL_IO_RPM_MAX"), &params->rpm_max);

+	param_get(param_find("MODAL_IO_VLOG"),    &params->verbose_logging);
+
 	if (params->rpm_min >= params->rpm_max) {
 		PX4_ERR("Invalid parameter MODAL_IO_RPM_MIN.  Please verify parameters.");
 		params->rpm_min = 0;
@@ -336,9 +339,9 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
 					_esc_status.esc[id].esc_address  = motor_idx + 1; //remapped motor ID
 					_esc_status.esc[id].timestamp    = tnow;
 					_esc_status.esc[id].esc_rpm      = fb.rpm;
-					//_esc_status.esc[id].esc_power    = fb.power;
+					_esc_status.esc[id].esc_power    = fb.power;
 					_esc_status.esc[id].esc_state    = fb.id_state & 0x0F;
-					//_esc_status.esc[id].esc_cmdcount = fb.cmd_counter;
+					_esc_status.esc[id].esc_cmdcount = fb.cmd_counter;
 					_esc_status.esc[id].esc_voltage  = _esc_chans[id].voltage;
 					_esc_status.esc[id].esc_current  = _esc_chans[id].current;
 					_esc_status.esc[id].failures     = 0; //not implemented
@@ -585,7 +588,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 	}

 	if (!strcmp(verb, "reset")) {
-		if (esc_id < 4) {
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
 			PX4_INFO("Reset ESC: %i", esc_id);
 			cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
@@ -597,7 +600,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "version")) {
-		if (esc_id < 4) {
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
 			PX4_INFO("Request version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
@@ -610,7 +613,7 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "version-ext")) {
-		if (esc_id < 4) {
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
 			PX4_INFO("Request extended version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
@@ -623,14 +626,14 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	} else if (!strcmp(verb, "tone")) {
-		if (0 < esc_id && esc_id < 16) {
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
 			PX4_INFO("Request tone for ESC mask: %i", esc_id);
 			cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
 			return get_instance()->send_cmd_thread_safe(&cmd);

 		} else {
-			print_usage("Invalid ESC mask, use 1-15");
+			print_usage("Invalid ESC ID, use 0-3");
 			return 0;
 		}

@@ -652,42 +655,20 @@ int ModalIo::custom_command(int argc, char *argv[])
 		}

 	}  else if (!strcmp(verb, "rpm")) {
-		if (0 < esc_id && esc_id < 16) {
-			PX4_INFO("Request RPM for ESC bit mask: %i - RPM: %i", esc_id, rate);
-			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS];
-			int16_t outputs[MODAL_IO_OUTPUT_CHANNELS];
-			outputs[0] = (esc_id & 1) ? rate : 0;
-			outputs[1] = (esc_id & 2) ? rate : 0;
-			outputs[2] = (esc_id & 4) ? rate : 0;
-			outputs[3] = (esc_id & 8) ? rate : 0;
-
-			//the motor mapping is.. if I want to spin Motor 1 (1-4) then i need to provide non-zero rpm for motor map[m-1]
-
-			modal_io_params_t params;
-			ch_assign_t map[MODAL_IO_OUTPUT_CHANNELS];
-			get_instance()->load_params(&params, (ch_assign_t *)&map);
-
-			uint8_t id_fb_raw = 0;
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
+			PX4_INFO("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate);
+			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
 			uint8_t id_fb = 0;

-			if (esc_id & 1) { id_fb_raw = 0; }
+			if (esc_id == 0xFF) {
+				rate_req[0] = rate;
+				rate_req[1] = rate;
+				rate_req[2] = rate;
+				rate_req[3] = rate;

-			else if (esc_id & 2) { id_fb_raw = 1; }
-
-			else if (esc_id & 4) { id_fb_raw = 2; }
-
-			else if (esc_id & 8) { id_fb_raw = 3; }
-
-			for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
-				int motor_idx = map[i].number - 1; // user defined mapping is 1-4, array is 0-3
-
-				if (motor_idx >= 0 && motor_idx < MODAL_IO_OUTPUT_CHANNELS) {
-					rate_req[i] = outputs[motor_idx] * map[i].direction;
-				}
-
-				if (motor_idx == id_fb_raw) {
-					id_fb = i;
-				}
+			} else {
+				rate_req[esc_id] = rate;
+				id_fb = esc_id;
 			}

 			cmd.len = qc_esc_create_rpm_packet4_fb(rate_req[0],
@@ -708,53 +689,31 @@ int ModalIo::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;

-			PX4_INFO("ESC map: %d %d %d %d", map[0].number, map[1].number, map[2].number, map[3].number);
-			PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb);
+			PX4_INFO("feedback id debug: %i", id_fb);
 			PX4_INFO("Sending UART ESC RPM command %i", rate);

 			return get_instance()->send_cmd_thread_safe(&cmd);

 		} else {
-			print_usage("Invalid ESC mask, use 1-15");
+			print_usage("Invalid ESC ID, use 0-3");
 			return 0;
 		}

 	} else if (!strcmp(verb, "pwm")) {
-		if (0 < esc_id && esc_id < 16) {
-			PX4_INFO("Request PWM for ESC mask: %i - PWM: %i", esc_id, rate);
-			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS];
-			int16_t outputs[MODAL_IO_OUTPUT_CHANNELS];
-			outputs[0] = (esc_id & 1) ? rate : 0;
-			outputs[1] = (esc_id & 2) ? rate : 0;
-			outputs[2] = (esc_id & 4) ? rate : 0;
-			outputs[3] = (esc_id & 8) ? rate : 0;
-
-			modal_io_params_t params;
-			ch_assign_t map[MODAL_IO_OUTPUT_CHANNELS];
-			get_instance()->load_params(&params, (ch_assign_t *)&map);
-
-			uint8_t id_fb_raw = 0;
+		if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
+			PX4_INFO("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate);
+			int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
 			uint8_t id_fb = 0;

-			if (esc_id & 1) { id_fb_raw = 0; }
+			if (esc_id == 0xFF) {
+				rate_req[0] = rate;
+				rate_req[1] = rate;
+				rate_req[2] = rate;
+				rate_req[3] = rate;

-			else if (esc_id & 2) { id_fb_raw = 1; }
-
-			else if (esc_id & 4) { id_fb_raw = 2; }
-
-			else if (esc_id & 8) { id_fb_raw = 3; }
-
-			for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
-				int motor_idx = map[i].number - 1; // user defined mapping is 1-4, array is 0-3
-
-				if (motor_idx >= 0 && motor_idx < MODAL_IO_OUTPUT_CHANNELS) {
-					rate_req[i] = outputs[motor_idx] * map[i].direction;
-					PX4_INFO("rate_req[%d]=%d", i, rate_req[i]);
-				}
-
-				if (motor_idx == id_fb_raw) {
-					id_fb = i;
-				}
+			} else {
+				rate_req[esc_id] = rate;
+				id_fb = esc_id;
 			}

 			cmd.len = qc_esc_create_pwm_packet4_fb(rate_req[0],
@@ -775,11 +734,9 @@ int ModalIo::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;

-			PX4_INFO("ESC map: %d %d %d %d", map[0].number, map[1].number, map[2].number, map[3].number);
-			PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb);
+			PX4_INFO("feedback id debug: %i", id_fb);
 			PX4_INFO("Sending UART ESC power command %i", rate);

-
 			return get_instance()->send_cmd_thread_safe(&cmd);

 		} else {
@@ -1157,8 +1114,7 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	//check_for_esc_timeout();

 	// publish the actual command that we sent and the feedback received
-	/*
-	if (MODALAI_PUBLISH_ESC_STATUS) {
+	if (_parameters.verbose_logging) {
 		actuator_outputs_s actuator_outputs{};
 		actuator_outputs.noutputs = num_outputs;

@@ -1169,9 +1125,10 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 		actuator_outputs.timestamp = hrt_absolute_time();

 		_outputs_debug_pub.publish(actuator_outputs);
-		_esc_status_pub.publish(_esc_status);
+
 	}
-	*/
+
+	_esc_status_pub.publish(_esc_status);

 	perf_count(_output_update_perf);

@@ -1363,7 +1320,9 @@ void ModalIo::Run()
 					}

 					if (_current_cmd.response) {
-						read_response(&_current_cmd);
+						if (read_response(&_current_cmd) == 0) {
+							_esc_status_pub.publish(_esc_status);
+						}
 					}

 				} else {
@@ -1433,19 +1392,19 @@ $ todo
 	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);

 	PRINT_MODULE_USAGE_COMMAND_DESCR("rpm", "Closed-Loop RPM test control request");
-	PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);
 	PRINT_MODULE_USAGE_PARAM_INT('r', 0, -32768, 32768, "RPM, -32,768 to 32,768", false);
 	PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false);
 	PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false);

 	PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request");
-	PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);
 	PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false);
 	PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false);
 	PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false);

 	PRINT_MODULE_USAGE_COMMAND_DESCR("tone", "Send tone generation request to ESC");
-	PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false);
 	PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 255, "Period of sound, inverse frequency, 0-255", false);
 	PRINT_MODULE_USAGE_PARAM_INT('d', 0, 0, 255, "Duration of the sound, 0-255, 1LSB = 13ms", false);
 	PRINT_MODULE_USAGE_PARAM_INT('v', 0, 0, 100, "Power (volume) of sound, 0-100", false);
@@ -143,6 +143,7 @@ private:
 		int32_t		function_map[MODAL_IO_OUTPUT_CHANNELS] {0, 0, 0, 0};
 		int32_t		motor_map[MODAL_IO_OUTPUT_CHANNELS] {1, 2, 3, 4};
 		int32_t		direction_map[MODAL_IO_OUTPUT_CHANNELS] {1, 1, 1, 1};
+		int32_t		verbose_logging{0};
 	} modal_io_params_t;

 	struct EscChan {
@@ -188,7 +189,7 @@ private:
 	uORB::Subscription 	_actuator_test_sub{ORB_ID(actuator_test)};
 	uORB::Subscription	_led_update_sub{ORB_ID(led_control)};

-	//uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
+	uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
 	uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};

 	modal_io_params_t	_parameters;
@@ -201,3 +201,16 @@ PARAM_DEFINE_INT32(MODAL_IO_T_EXPO, 35);
 * @increment 0.001
 */
 PARAM_DEFINE_FLOAT(MODAL_IO_T_COSP, 0.990);
+
+/**
+ * UART ESC verbose logging
+ *
+ * @reboot_required true
+ *
+ * @group MODAL IO
+ * @value 0 - Disabled
+ * @value 1 - Enabled
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(MODAL_IO_VLOG, 0);
@@ -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
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   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
@@ -31,13 +31,18 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__roboclaw
-	MAIN roboclaw
+	MODULE drivers__imu__invensense__icm45686
+	MAIN icm45686
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+		#-DDEBUG_BUILD
 	SRCS
-		roboclaw_main.cpp
-		RoboClaw.cpp
-	MODULE_CONFIG
-		module.yaml
+		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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,25 +31,62 @@
 *
 ****************************************************************************/

-/**
- * @file tailsitter_params.c
- * Parameters for vtol attitude controller.
- *
- * @author Roman Bapst <bapstroman@gmail.com>
- * @author David Vorsin     <davidvorsin@gmail.com>
- */
+#include "ICM45686.hpp"

-/**
- * Duration of front transition phase 2
- *
- * Time in seconds it should take for the rotors to rotate forward completely from the point
- * when the plane has picked up enough airspeed and is ready to go into fixed wind mode.
- *
- * @unit s
- * @min 0.1
- * @max 5.0
- * @increment 0.01
- * @decimal 3
- * @group VTOL Attitude Control
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>

-PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/
+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;
+}
@@ -50,7 +50,7 @@
 #include <uORB/topics/qshell_retval.h>
 #include <drivers/drv_hrt.h>

-#define MAX_ARGS 8 // max number of whitespace separated args after app name
+#define MAX_ARGS 16 // max number of whitespace separated args after app name

 px4::AppState QShell::appState;

@@ -1,5 +0,0 @@
-menuconfig DRIVERS_ROBOCLAW
-	bool "roboclaw"
-	default n
-	---help---
-		Enable support for roboclaw
@@ -1,610 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 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 RoboClaw.cpp
- *
- * RoboClaw Motor Driver
- *
- * references:
- * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
- *
- */
-
-#include "RoboClaw.hpp"
-#include <poll.h>
-#include <stdio.h>
-#include <math.h>
-#include <string.h>
-#include <fcntl.h>
-#include <termios.h>
-
-#include <systemlib/err.h>
-#include <systemlib/mavlink_log.h>
-
-#include <uORB/Publication.hpp>
-#include <drivers/drv_hrt.h>
-#include <math.h>
-
-// The RoboClaw has a serial communication timeout of 10ms.
-// Add a little extra to account for timing inaccuracy
-#define TIMEOUT_US 10500
-
-// If a timeout occurs during serial communication, it will immediately try again this many times
-#define TIMEOUT_RETRIES 1
-
-// If a timeout occurs while disarmed, it will try again this many times. This should be a higher number,
-// because stopping when disarmed is pretty important.
-#define STOP_RETRIES 10
-
-// Number of bytes returned by the Roboclaw when sending command 78, read both encoders
-#define ENCODER_MESSAGE_SIZE 10
-
-// Number of bytes for commands 18 and 19, read speeds.
-#define ENCODER_SPEED_MESSAGE_SIZE 7
-
-bool RoboClaw::taskShouldExit = false;
-
-RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam):
-	_uart(0),
-	_uart_set(),
-	_uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US},
-	_actuatorsSub(-1),
-	_lastEncoderCount{0, 0},
-	_encoderCounts{0, 0},
-	_motorSpeeds{0, 0}
-
-{
-	_param_handles.actuator_write_period_ms = 	param_find("RBCLW_WRITE_PER");
-	_param_handles.encoder_read_period_ms = 	param_find("RBCLW_READ_PER");
-	_param_handles.counts_per_rev = 			param_find("RBCLW_COUNTS_REV");
-	_param_handles.serial_baud_rate = 			param_find(baudRateParam);
-	_param_handles.address = 					param_find("RBCLW_ADDRESS");
-
-	_parameters_update();
-
-	// start serial port
-	_uart = open(deviceName, O_RDWR | O_NOCTTY);
-
-	if (_uart < 0) { err(1, "could not open %s", deviceName); }
-
-	int ret = 0;
-	struct termios uart_config {};
-	ret = tcgetattr(_uart, &uart_config);
-
-	if (ret < 0) { err(1, "failed to get attr"); }
-
-	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
-	ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate);
-
-	if (ret < 0) { err(1, "failed to set input speed"); }
-
-	ret = cfsetospeed(&uart_config, _parameters.serial_baud_rate);
-
-	if (ret < 0) { err(1, "failed to set output speed"); }
-
-	ret = tcsetattr(_uart, TCSANOW, &uart_config);
-
-	if (ret < 0) { err(1, "failed to set attr"); }
-
-	FD_ZERO(&_uart_set);
-
-	// setup default settings, reset encoders
-	resetEncoders();
-}
-
-RoboClaw::~RoboClaw()
-{
-	setMotorDutyCycle(MOTOR_1, 0.0);
-	setMotorDutyCycle(MOTOR_2, 0.0);
-	close(_uart);
-}
-
-void RoboClaw::taskMain()
-{
-	// Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not.
-	uint8_t rbuff[4];
-	int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true);
-
-	if (err_code <= 0) {
-		PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver.");
-		return;
-	}
-
-	// This main loop performs two different tasks, asynchronously:
-	// - Send actuator_controls_0 to the Roboclaw as soon as they are available
-	// - Read the encoder values at a constant rate
-	// To do this, the timeout on the poll() function is used.
-	// waitTime is the amount of time left (int microseconds) until the next time I should read from the encoders.
-	// It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before
-	// I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return
-	// immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment)
-	uint64_t encoderTaskLastRun = 0;
-	int waitTime = 0;
-
-	uint64_t actuatorsLastWritten = 0;
-
-	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
-	orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
-
-	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
-	_paramSub = orb_subscribe(ORB_ID(parameter_update));
-
-	pollfd fds[3];
-	fds[0].fd = _paramSub;
-	fds[0].events = POLLIN;
-	fds[1].fd = _actuatorsSub;
-	fds[1].events = POLLIN;
-	fds[2].fd = _armedSub;
-	fds[2].events = POLLIN;
-
-	memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg));
-	_wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev;
-	_wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev;
-
-	while (!taskShouldExit) {
-
-		int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000);
-
-		bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms;
-
-		if (fds[0].revents & POLLIN) {
-			orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate);
-			_parameters_update();
-		}
-
-		// No timeout, update on either the actuator controls or the armed state
-		if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout)) {
-			orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls);
-			orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed);
-
-			int drive_ret = 0, turn_ret = 0;
-
-			const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown
-					      || _actuatorArmed.force_failsafe || actuators_timeout;
-
-			if (disarmed) {
-				// If disarmed, I want to be certain that the stop command gets through.
-				int tries = 0;
-
-				while (tries < STOP_RETRIES && ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0)) {
-					PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
-					tries++;
-					px4_usleep(TIMEOUT_US);
-				}
-
-			} else {
-				drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]);
-				turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]);
-
-				if (drive_ret <= 0 || turn_ret <= 0) {
-					PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
-				}
-			}
-
-			actuatorsLastWritten = hrt_absolute_time();
-
-		} else {
-			// A timeout occurred, which means that it's time to update the encoders
-			encoderTaskLastRun = hrt_absolute_time();
-
-			if (readEncoder() > 0) {
-
-				for (int i = 0; i < 2; i++) {
-					_wheelEncoderMsg[i].timestamp = encoderTaskLastRun;
-
-					_wheelEncoderMsg[i].encoder_position = _encoderCounts[i];
-					_wheelEncoderMsg[i].speed = _motorSpeeds[i];
-
-					_wheelEncodersAdv[i].publish(_wheelEncoderMsg[i]);
-				}
-
-			} else {
-				PX4_ERR("Error reading encoders");
-			}
-		}
-
-		waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
-		waitTime = waitTime < 0 ? 0 : waitTime;
-	}
-
-	orb_unsubscribe(_actuatorsSub);
-	orb_unsubscribe(_armedSub);
-	orb_unsubscribe(_paramSub);
-}
-
-int RoboClaw::readEncoder()
-{
-
-	uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE];
-	// I am saving space by overlapping the two separate motor speeds, so that the final buffer will look like:
-	// [<speed 1> <speed 2> <status 2> <checksum 2>]
-	// And I just ignore all of the statuses and checksums. (The _transaction() function internally handles the
-	// checksum)
-	uint8_t rbuff_speed[ENCODER_SPEED_MESSAGE_SIZE + 4];
-
-	bool success = false;
-
-	for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) {
-		success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false,
-				       true) == ENCODER_MESSAGE_SIZE;
-		success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false,
-						  true) == ENCODER_SPEED_MESSAGE_SIZE;
-		success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false,
-						  true) == ENCODER_SPEED_MESSAGE_SIZE;
-	}
-
-	if (!success) {
-		PX4_ERR("Error reading encoders");
-		return -1;
-	}
-
-	uint32_t count;
-	uint32_t speed;
-	uint8_t *count_bytes;
-	uint8_t *speed_bytes;
-
-	for (int motor = 0; motor <= 1; motor++) {
-		count = 0;
-		speed = 0;
-		count_bytes = &rbuff_pos[motor * 4];
-		speed_bytes = &rbuff_speed[motor * 4];
-
-		// Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the
-		// endianness of the system this code is running on.
-		for (int byte = 0; byte < 4; byte++) {
-			count = (count << 8) + count_bytes[byte];
-			speed = (speed << 8) + speed_bytes[byte];
-		}
-
-		// The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting
-		// at 0 and moving backward. The Roboclaw has overflow flags for this, but in my testing, they don't seem
-		// to work. This code detects overflow manually.
-		// These diffs are the difference between the count I just read from the Roboclaw and the last
-		// count that was read from the roboclaw for this motor. fwd_diff assumes that the wheel moved forward,
-		// and rev_diff assumes it moved backward. If the motor actually moved forward, then rev_diff will be close
-		// to 2^32 (UINT32_MAX). If the motor actually moved backward, then fwd_diff will be close to 2^32.
-		// To detect and account for overflow, I just assume that the smaller diff is correct.
-		// Strictly speaking, if the wheel rotated more than 2^31 encoder counts since the last time I checked, this
-		// will be wrong. But that's 1.7 million revolutions, so it probably won't come up.
-		uint32_t fwd_diff = count - _lastEncoderCount[motor];
-		uint32_t rev_diff = _lastEncoderCount[motor] - count;
-		// At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe.
-		int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff);
-		_encoderCounts[motor] += diff;
-		_lastEncoderCount[motor] = count;
-
-		_motorSpeeds[motor] = speed;
-	}
-
-	return 1;
-}
-
-void RoboClaw::printStatus(char *string, size_t n)
-{
-	snprintf(string, n, "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
-		 double(getMotorPosition(MOTOR_1)),
-		 double(getMotorSpeed(MOTOR_1)),
-		 double(getMotorPosition(MOTOR_2)),
-		 double(getMotorSpeed(MOTOR_2)));
-}
-
-float RoboClaw::getMotorPosition(e_motor motor)
-{
-	if (motor == MOTOR_1) {
-		return _encoderCounts[0];
-
-	} else if (motor == MOTOR_2) {
-		return _encoderCounts[1];
-
-	} else {
-		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
-		return NAN;
-	}
-}
-
-float RoboClaw::getMotorSpeed(e_motor motor)
-{
-	if (motor == MOTOR_1) {
-		return _motorSpeeds[0];
-
-	} else if (motor == MOTOR_2) {
-		return _motorSpeeds[1];
-
-	} else {
-		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
-		return NAN;
-	}
-}
-
-int RoboClaw::setMotorSpeed(e_motor motor, float value)
-{
-	e_command command;
-
-	// send command
-	if (motor == MOTOR_1) {
-		if (value > 0) {
-			command = CMD_DRIVE_FWD_1;
-
-		} else {
-			command = CMD_DRIVE_REV_1;
-		}
-
-	} else if (motor == MOTOR_2) {
-		if (value > 0) {
-			command = CMD_DRIVE_FWD_2;
-
-		} else {
-			command = CMD_DRIVE_REV_2;
-		}
-
-	} else {
-		return -1;
-	}
-
-	return _sendUnsigned7Bit(command, value);
-}
-
-int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
-{
-
-	e_command command;
-
-	// send command
-	if (motor == MOTOR_1) {
-		command = CMD_SIGNED_DUTYCYCLE_1;
-
-	} else if (motor == MOTOR_2) {
-		command = CMD_SIGNED_DUTYCYCLE_2;
-
-	} else {
-		return -1;
-	}
-
-	return _sendSigned16Bit(command, value);
-}
-
-int RoboClaw::drive(float value)
-{
-	e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX;
-	return _sendUnsigned7Bit(command, value);
-}
-
-int RoboClaw::turn(float value)
-{
-	e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT;
-	return _sendUnsigned7Bit(command, value);
-}
-
-int RoboClaw::resetEncoders()
-{
-	return _sendNothing(CMD_RESET_ENCODERS);
-}
-
-int RoboClaw::_sendUnsigned7Bit(e_command command, float data)
-{
-	data = fabs(data);
-
-	if (data > 1.0f) {
-		data = 1.0f;
-	}
-
-	auto byte = (uint8_t)(data * INT8_MAX);
-	uint8_t recv_byte;
-	return _transaction(command, &byte, 1, &recv_byte, 1);
-}
-
-int RoboClaw::_sendSigned16Bit(e_command command, float data)
-{
-	if (data > 1.0f) {
-		data = 1.0f;
-
-	} else if (data < -1.0f) {
-		data = -1.0f;
-	}
-
-	auto buff = (uint16_t)(data * INT16_MAX);
-	uint8_t recv_buff;
-	return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 1);
-}
-
-int RoboClaw::_sendNothing(e_command command)
-{
-	uint8_t recv_buff;
-	return _transaction(command, nullptr, 0, &recv_buff, 1);
-}
-
-uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
-{
-	uint16_t crc = init;
-
-	for (size_t byte = 0; byte < n; byte++) {
-		crc = crc ^ (((uint16_t) buf[byte]) << 8);
-
-		for (uint8_t bit = 0; bit < 8; bit++) {
-			if (crc & 0x8000) {
-				crc = (crc << 1) ^ 0x1021;
-
-			} else {
-				crc = crc << 1;
-			}
-		}
-	}
-
-	return crc;
-}
-
-int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
-			   uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum)
-{
-	int err_code = 0;
-
-	// WRITE
-
-	tcflush(_uart, TCIOFLUSH); // flush  buffers
-	uint8_t buf[wbytes + 4];
-	buf[0] = (uint8_t) _parameters.address;
-	buf[1] = cmd;
-
-	if (wbuff) {
-		memcpy(&buf[2], wbuff, wbytes);
-	}
-
-	wbytes = wbytes + (send_checksum ? 4 : 2);
-
-	if (send_checksum) {
-		uint16_t sum = _calcCRC(buf, wbytes - 2);
-		buf[wbytes - 2] = (sum >> 8) & 0xFF;
-		buf[wbytes - 1] = sum & 0xFFu;
-	}
-
-	int count = write(_uart, buf, wbytes);
-
-	if (count < (int) wbytes) { // Did not successfully send all bytes.
-		PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes);
-		return -1;
-	}
-
-	// READ
-
-	FD_ZERO(&_uart_set);
-	FD_SET(_uart, &_uart_set);
-
-	uint8_t *rbuff_curr = rbuff;
-	size_t bytes_read = 0;
-
-	// select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many
-	// bytes are available. I need to keep reading until I get the number of bytes I expect.
-	while (bytes_read < rbytes) {
-		// select(...) may change this timeout struct (because it is not const). So I reset it every time.
-		_uart_timeout.tv_sec = 0;
-		_uart_timeout.tv_usec = TIMEOUT_US;
-		err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout);
-
-		// An error code of 0 means that select timed out, which is how the Roboclaw indicates an error.
-		if (err_code <= 0) {
-			return err_code;
-		}
-
-		err_code = read(_uart, rbuff_curr, rbytes - bytes_read);
-
-		if (err_code <= 0) {
-			return err_code;
-
-		} else {
-			bytes_read += err_code;
-			rbuff_curr += err_code;
-		}
-	}
-
-	//TODO: Clean up this mess of IFs and returns
-
-	if (recv_checksum) {
-		if (bytes_read < 2) {
-			return -1;
-		}
-
-		// The checksum sent back by the roboclaw is calculated based on the address and command bytes as well
-		// as the data returned.
-		uint16_t checksum_calc = _calcCRC(buf, 2);
-		checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc);
-		uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1];
-
-		if (checksum_calc == checksum_recv) {
-			return bytes_read;
-
-		} else {
-			return -10;
-		}
-
-	} else {
-		if (bytes_read == 1 && rbuff[0] == 0xFF) {
-			return 1;
-
-		} else {
-			return -11;
-		}
-	}
-}
-
-void RoboClaw::_parameters_update()
-{
-	param_get(_param_handles.counts_per_rev, &_parameters.counts_per_rev);
-	param_get(_param_handles.encoder_read_period_ms, &_parameters.encoder_read_period_ms);
-	param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms);
-	param_get(_param_handles.address, &_parameters.address);
-
-	if (_actuatorsSub > 0) {
-		orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
-	}
-
-	int32_t baudRate;
-	param_get(_param_handles.serial_baud_rate, &baudRate);
-
-	switch (baudRate) {
-	case 2400:
-		_parameters.serial_baud_rate = B2400;
-		break;
-
-	case 9600:
-		_parameters.serial_baud_rate = B9600;
-		break;
-
-	case 19200:
-		_parameters.serial_baud_rate = B19200;
-		break;
-
-	case 38400:
-		_parameters.serial_baud_rate = B38400;
-		break;
-
-	case 57600:
-		_parameters.serial_baud_rate = B57600;
-		break;
-
-	case 115200:
-		_parameters.serial_baud_rate = B115200;
-		break;
-
-	case 230400:
-		_parameters.serial_baud_rate = B230400;
-		break;
-
-	case 460800:
-		_parameters.serial_baud_rate = B460800;
-		break;
-
-	default:
-		_parameters.serial_baud_rate = B2400;
-	}
-}
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 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 RoboClas.hpp
- *
- * RoboClaw Motor Driver
- *
- * references:
- * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
- *
- */
-
-#pragma once
-
-#include <poll.h>
-#include <stdio.h>
-#include <termios.h>
-#include <lib/parameters/param.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/wheel_encoders.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <drivers/device/i2c.h>
-#include <sys/select.h>
-#include <sys/time.h>
-#include <pthread.h>
-
-/**
- * This is a driver for the RoboClaw motor controller
- */
-class RoboClaw
-{
-public:
-
-	void taskMain();
-	static bool taskShouldExit;
-
-	/** control channels */
-	enum e_channel {
-		CH_VOLTAGE_LEFT = 0,
-		CH_VOLTAGE_RIGHT
-	};
-
-	/**  motors */
-	enum e_motor {
-		MOTOR_1 = 0,
-		MOTOR_2
-	};
-
-	/**
-	 * constructor
-	 * @param deviceName the name of the
-	 * 	serial port e.g. "/dev/ttyS2"
-	 * @param address the adddress  of the motor
-	 * 	(selectable on roboclaw)
-	 * @param baudRateParam Name of the parameter that holds the baud rate of this serial port
-	 */
-	RoboClaw(const char *deviceName, const char *baudRateParam);
-
-	/**
-	 * deconstructor
-	 */
-	virtual ~RoboClaw();
-
-	/**
-	 * @return position of a motor, rev
-	 */
-	float getMotorPosition(e_motor motor);
-
-	/**
-	 * @return speed of a motor, rev/sec
-	 */
-	float getMotorSpeed(e_motor motor);
-
-	/**
-	 * set the speed of a motor, rev/sec
-	 */
-	int setMotorSpeed(e_motor motor, float value);
-
-	/**
-	 * set the duty cycle of a motor
-	 */
-	int setMotorDutyCycle(e_motor motor, float value);
-
-	/**
-	 * Drive both motors. +1 = full forward, -1 = full backward
-	 */
-	int drive(float value);
-
-	/**
-	 * Turn. +1 = full right, -1 = full left
-	 */
-	int turn(float value);
-
-	/**
-	 * reset the encoders
-	 * @return status
-	 */
-	int resetEncoders();
-
-	/**
-	 * read data from serial
-	 */
-	int readEncoder();
-
-	/**
-	 * print status
-	 */
-	void printStatus(char *string, size_t n);
-
-private:
-
-	// commands
-	// We just list the commands we want from the manual here.
-	enum e_command {
-
-		// simple
-		CMD_DRIVE_FWD_1 = 0,
-		CMD_DRIVE_REV_1 = 1,
-		CMD_DRIVE_FWD_2 = 4,
-		CMD_DRIVE_REV_2 = 5,
-
-		CMD_DRIVE_FWD_MIX = 8,
-		CMD_DRIVE_REV_MIX = 9,
-		CMD_TURN_RIGHT = 10,
-		CMD_TURN_LEFT = 11,
-
-		// encoder commands
-		CMD_READ_ENCODER_1 = 16,
-		CMD_READ_ENCODER_2 = 17,
-		CMD_READ_SPEED_1 = 18,
-		CMD_READ_SPEED_2 = 19,
-		CMD_RESET_ENCODERS = 20,
-		CMD_READ_BOTH_ENCODERS = 78,
-		CMD_READ_BOTH_SPEEDS = 79,
-
-		// advanced motor control
-		CMD_READ_SPEED_HIRES_1 = 30,
-		CMD_READ_SPEED_HIRES_2 = 31,
-		CMD_SIGNED_DUTYCYCLE_1 = 32,
-		CMD_SIGNED_DUTYCYCLE_2 = 33,
-
-		CMD_READ_STATUS = 90
-	};
-
-	struct {
-		speed_t serial_baud_rate;
-		int32_t counts_per_rev;
-		int32_t encoder_read_period_ms;
-		int32_t actuator_write_period_ms;
-		int32_t address;
-	} _parameters{};
-
-	struct {
-		param_t serial_baud_rate;
-		param_t counts_per_rev;
-		param_t encoder_read_period_ms;
-		param_t actuator_write_period_ms;
-		param_t address;
-	} _param_handles{};
-
-	int _uart;
-	fd_set _uart_set;
-	struct timeval _uart_timeout;
-
-	/** actuator controls subscription */
-	int _actuatorsSub{-1};
-	actuator_controls_s _actuatorControls;
-
-	int _armedSub{-1};
-	actuator_armed_s _actuatorArmed;
-
-	int _paramSub{-1};
-	parameter_update_s _paramUpdate;
-
-	uORB::PublicationMulti<wheel_encoders_s> _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)};
-	wheel_encoders_s _wheelEncoderMsg[2];
-
-	uint32_t _lastEncoderCount[2] {0, 0};
-	int64_t _encoderCounts[2] {0, 0};
-	int32_t _motorSpeeds[2] {0, 0};
-
-	void _parameters_update();
-
-	static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
-	int _sendUnsigned7Bit(e_command command, float data);
-	int _sendSigned16Bit(e_command command, float data);
-	int _sendNothing(e_command);
-
-	/**
-	 * Perform a round-trip write and read.
-	 *
-	 * NOTE: This function is not thread-safe.
-	 *
-	 * @param cmd Command to send to the Roboclaw
-	 * @param wbuff Write buffer. Must not contain command, address, or checksum. For most commands, this will be
-	 *   one or two bytes. Can be null iff wbytes == 0.
-	 * @param wbytes Number of bytes to write. Can be 0.
-	 * @param rbuff Read buffer. Will be filled with the entire response, including a checksum if the Roboclaw sends
-	 *   a checksum for this command.
-	 * @param rbytes Maximum number of bytes to read.
-	 * @param send_checksum If true, then the checksum will be calculated and sent to the Roboclaw.
-	 *   This is an option because some Roboclaw commands expect no checksum.
-	 * @param recv_checksum If true, then this function will calculate the checksum of the returned data and compare
-	 *   it to the checksum received. If they are not equal, OR if fewer than 2 bytes were received, then an
-	 *   error is returned.
-	 *   If false, then this function will expect to read exactly one byte, 0xFF, and will return an error otherwise.
-	 * @return If successful, then the number of bytes read from the Roboclaw is returned. If there is a timeout
-	 *   reading from the Roboclaw, then 0 is returned. If there is an IO error, then a negative value is returned.
-	 */
-	int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
-			 uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false);
-};
@@ -1,6 +0,0 @@
-module_name: Roboclaw Driver
-serial_config:
-  - command: roboclaw start ${SERIAL_DEV} ${BAUD_PARAM}
-    port_config_param:
-      name: RBCLW_SER_CFG
-      group: Roboclaw
@@ -1,207 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 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 roboclaw_main.cpp
- *
- * RoboClaw Motor Driver
- *
- * references:
- * http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
- *
- */
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/log.h>
-#include <px4_platform_common/module.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <parameters/param.h>
-
-
-#include "RoboClaw.hpp"
-
-static bool thread_running = false;     /**< Deamon status flag */
-px4_task_t deamon_task;
-
-/**
- * Deamon management function.
- */
-extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int roboclaw_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage();
-
-static void usage()
-{
-	PRINT_MODULE_USAGE_NAME("roboclaw", "driver");
-
-	PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
-### Description
-
-This driver communicates over UART with the [Roboclaw motor driver](http://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf).
-It performs two tasks:
-
- Control the motors based on the `actuator_controls_0` UOrb topic.
- Read the wheel encoders and publish the raw data in the `wheel_encoders` UOrb topic
-
-In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and
-your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4,
-use the `UART & I2C B` port, which corresponds to `/dev/ttyS3`.
-
-### Implementation
-
-The main loop of this module (Located in `RoboClaw.cpp::task_main()`) performs 2 tasks:
-
- 1. Write `actuator_controls_0` messages to the Roboclaw as they become available
- 2. Read encoder data from the Roboclaw at a constant, fixed rate.
-
-Because of the latency of UART, this driver does not write every single `actuator_controls_0` message to the Roboclaw
-immediately. Instead, it is rate limited based on the parameter `RBCLW_WRITE_PER`.
-
-On startup, this driver will attempt to read the status of the Roboclaw to verify that it is connected. If this fails,
-the driver terminates immediately.
-
-### Examples
-
-The command to start this driver is:
-
-```
-$ roboclaw start <device> <baud>
-```
-
- `<device>` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`.
- `<baud>` is the baud rate.
-
-All available commands are:
-
- `$ roboclaw start <device> <baud>`
- `$ roboclaw status`
- `$ roboclaw stop`
-	)DESCR_STR");
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int roboclaw_main(int argc, char *argv[])
-{
-
-	if (argc < 4) {
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("roboclaw already running\n");
-			/* this is not an error */
-			return 0;
-		}
-
-		RoboClaw::taskShouldExit = false;
-		deamon_task = px4_task_spawn_cmd("roboclaw",
-						 SCHED_DEFAULT,
-						 SCHED_PRIORITY_MAX - 10,
-						 2000,
-						 roboclaw_thread_main,
-						 (char *const *)argv);
-		return 0;
-
-	} else if (!strcmp(argv[1], "stop")) {
-
-		RoboClaw::taskShouldExit = true;
-		return 0;
-
-	} else if (!strcmp(argv[1], "status")) {
-
-		if (thread_running) {
-			printf("\troboclaw app is running\n");
-
-		} else {
-			printf("\troboclaw app not started\n");
-		}
-
-		return 0;
-	}
-
-	usage();
-	return 1;
-}
-
-int roboclaw_thread_main(int argc, char *argv[])
-{
-	printf("[roboclaw] starting\n");
-
-	// skip parent process args
-	argc -= 2;
-	argv += 2;
-
-	if (argc < 2) {
-		printf("usage: roboclaw start <device> <baud>\n");
-		return -1;
-	}
-
-	const char *deviceName = argv[1];
-	const char *baudRate = argv[2];
-
-	// start
-	RoboClaw roboclaw(deviceName, baudRate);
-
-	thread_running = true;
-
-	roboclaw.taskMain();
-
-	// exit
-	printf("[roboclaw] exiting.\n");
-	thread_running = false;
-	return 0;
-}
@@ -1,114 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2019 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 roboclaw_params.c
- *
- * Parameters defined by the Roboclaw driver.
- *
- * The Roboclaw will need to be configured to match these parameters. For information about configuring the
- * Roboclaw, see http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
- *
- * @author Timothy Scott <timothy@auterion.com>
- */
-
-
-/**
- * Uart write period
- *
- * How long to wait, in Milliseconds, between writing actuator controls over Uart to the Roboclaw
- * @unit ms
- * @min 1
- * @max 1000
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_WRITE_PER, 10);
-
-/**
- * Encoder read period
- *
- * How long to wait, in Milliseconds, between reading wheel encoder values over Uart from the Roboclaw
- * @unit ms
- * @min 1
- * @max 1000
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_READ_PER, 10);
-
-/**
- * Encoder counts per revolution
- *
- * Number of encoder counts for one revolution. The roboclaw treats analog encoders (potentiometers) as having 2047
- * counts per rev. The default value of 1200 corresponds to the default configuration of the Aion R1 rover.
- * @min 1
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200);
-
-/**
- * Address of the Roboclaw
- *
- * The Roboclaw can be configured to have an address from 0x80 to 0x87, inclusive. It must be configured to match
- * this parameter.
- * @min 128
- * @max 135
- * @value 128 0x80
- * @value 129 0x81
- * @value 130 0x82
- * @value 131 0x83
- * @value 132 0x84
- * @value 133 0x85
- * @value 134 0x86
- * @value 135 0x87
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128);
-
-/**
- * Roboclaw serial baud rate
- *
- * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate.
- * @min 2400
- * @max 460800
- * @value 2400 2400 baud
- * @value 9600 9600 baud
- * @value 19200 19200 baud
- * @value 38400 38400 baud
- * @value 57600 57600 baud
- * @value 115200 115200 baud
- * @value 230400 230400 baud
- * @value 460800 460800 baud
- * @group Roboclaw driver
- * @reboot_required true
- */
-PARAM_DEFINE_INT32(RBCLW_BAUD, 2400);
@@ -46,38 +46,12 @@

 #include <float.h>

-using matrix::Vector2d;
 using matrix::Vector2f;
-using matrix::wrap_pi;
-
-void ECL_L1_Pos_Controller::update_roll_setpoint()
-{
-	float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
-	roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);
-
-	if (_dt > 0.0f && _roll_slew_rate > 0.0f) {
-		// slew rate limiting active
-		roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt);
-	}
-
-	if (PX4_ISFINITE(roll_new)) {
-		_roll_setpoint = roll_new;
-	}
-
-}
-
-float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
-{
-	/* following [2], switching on L1 distance */
-	return math::min(wp_radius, _L1_distance);
-}

 void
 ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
 		const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
 {
-	_has_guidance_updated = true;
-
 	/* this follows the logic presented in [1] */
 	float eta = 0.0f;

@@ -202,181 +176,6 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
 	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);

-	/* flying to waypoints, not circling them */
-	_circle_mode = false;
-
-	/* the bearing angle, in NED frame */
-	_bearing_error = eta;
-
-	update_roll_setpoint();
-}
-
-void
-ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
-				       const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector)
-{
-	_has_guidance_updated = true;
-
-	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
-
-	/* the complete guidance logic in this section was proposed by [2] */
-
-	/* calculate the gains for the PD loop (circle tracking) */
-	float omega = (2.0f * M_PI_F / _L1_period);
-	float K_crosstrack = omega * omega;
-	float K_velocity = 2.0f * _L1_damping * omega;
-
-	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
-	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
-
-	/* calculate the L1 length required for the desired period */
-	_L1_distance = _L1_ratio * ground_speed;
-
-	/* calculate the vector from waypoint A to current position */
-	Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
-
-	Vector2f vector_A_to_airplane_unit;
-
-	/* prevent NaN when normalizing */
-	if (vector_A_to_airplane.length() > FLT_EPSILON) {
-		/* store the normalized vector from waypoint A to current position */
-		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
-
-	} else {
-		vector_A_to_airplane_unit = vector_A_to_airplane;
-	}
-
-	/* update bearing to next waypoint */
-	_target_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
-
-	/* calculate eta angle towards the loiter center */
-
-	/* velocity across / orthogonal to line from waypoint to current position */
-	float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
-	/* velocity along line from waypoint to current position */
-	float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
-	float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
-	/* limit eta to 90 degrees */
-	eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
-
-	/* calculate the lateral acceleration to capture the center point */
-	float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
-
-	/* for PD control: Calculate radial position and velocity errors */
-
-	/* radial velocity error */
-	float xtrack_vel_circle = -ltrack_vel_center;
-	/* radial distance from the loiter circle (not center) */
-	float xtrack_err_circle = vector_A_to_airplane.length() - radius;
-
-	/* cross track error for feedback */
-	_crosstrack_error = xtrack_err_circle;
-
-	/* calculate PD update to circle waypoint */
-	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
-
-	/* calculate velocity on circle / along tangent */
-	float tangent_vel = xtrack_vel_center * loiter_direction_multiplier;
-
-	/* prevent PD output from turning the wrong way when in circle mode */
-	const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
-
-	if (tangent_vel < -l1_op_tan_vel && _circle_mode) {
-		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
-	}
-
-	/* calculate centripetal acceleration setpoint */
-	float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius),
-			(radius + xtrack_err_circle));
-
-	/* add PD control on circle and centripetal acceleration for total circle command */
-	float lateral_accel_sp_circle = loiter_direction_multiplier * (lateral_accel_sp_circle_pd +
-					lateral_accel_sp_circle_centripetal);
-
-	/*
-	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
-	 * the commands switch over. Only fly towards waypoint if outside the circle.
-	 */
-
-	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && !loiter_direction_counter_clockwise
-	     && xtrack_err_circle > 0.0f)
-	    ||
-	    (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && xtrack_err_circle > 0.0f)) {
-		_lateral_accel = lateral_accel_sp_center;
-		_circle_mode = false;
-		/* angle between requested and current velocity vector */
-		_bearing_error = eta;
-		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
-
-	} else {
-		_lateral_accel = lateral_accel_sp_circle;
-		_circle_mode = true;
-		_bearing_error = 0.0f;
-		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
-	}
-
-	update_roll_setpoint();
-}
-
-void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading,
-		const Vector2f &ground_speed_vector)
-{
-	_has_guidance_updated = true;
-
-	/* the complete guidance logic in this section was proposed by [2] */
-
-	/*
-	 * As the commanded heading is the only reference
-	 * (and no crosstrack correction occurs),
-	 * target and navigation bearing become the same
-	 */
-	_target_bearing = _nav_bearing = wrap_pi(navigation_heading);
-
-	float eta = wrap_pi(_target_bearing - wrap_pi(current_heading));
-
-	/* consequently the bearing error is exactly eta: */
-	_bearing_error = eta;
-
-	/* ground speed is the length of the ground speed vector */
-	float ground_speed = ground_speed_vector.length();
-
-	/* adjust L1 distance to keep constant frequency */
-	_L1_distance = ground_speed / _heading_omega;
-	float omega_vel = ground_speed * _heading_omega;
-
-	/* not circling a waypoint */
-	_circle_mode = false;
-
-	/* navigating heading means by definition no crosstrack error */
-	_crosstrack_error = 0;
-
-	/* limit eta to 90 degrees */
-	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
-	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
-
-	update_roll_setpoint();
-}
-
-void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
-{
-	_has_guidance_updated = true;
-
-	/* the logic in this section is trivial, but originally proposed by [2] */
-
-	/* reset all heading / error measures resulting in zero roll */
-	_target_bearing = current_heading;
-	_nav_bearing = current_heading;
-	_bearing_error = 0;
-	_crosstrack_error = 0;
-	_lateral_accel = 0;
-
-	/* not circling a waypoint when flying level */
-	_circle_mode = false;
-
-	update_roll_setpoint();
 }

 void ECL_L1_Pos_Controller::set_l1_period(float period)
@@ -83,14 +83,6 @@ public:
 	 */
 	float nav_lateral_acceleration_demand() { return _lateral_accel; }

-	/**
-	 * Heading error.
-	 *
-	 * The heading error is either compared to the current track
-	 * or to the tangent of the current loiter radius.
-	 */
-	float bearing_error() { return _bearing_error; }
-
 	/**
 	 * Bearing from aircraft to current target.
 	 *
@@ -98,13 +90,6 @@ public:
 	 */
 	float target_bearing() { return _target_bearing; }

-	/**
-	 * Get roll angle setpoint for fixed wing.
-	 *
-	 * @return Roll angle (in NED frame)
-	 */
-	float get_roll_setpoint() { return _roll_setpoint; }
-
 	/**
 	 * Get the current crosstrack error.
 	 *
@@ -112,27 +97,6 @@ public:
 	 */
 	float crosstrack_error() { return _crosstrack_error; }

-	/**
-	 * Returns true if the loiter waypoint has been reached
-	 */
-	bool reached_loiter_target() { return _circle_mode; }
-
-	/**
-	 * Returns true if following a circle (loiter)
-	 */
-	bool circle_mode() { return _circle_mode; }
-
-	/**
-	 * Get the switch distance
-	 *
-	 * This is the distance at which the system will
-	 * switch to the next waypoint. This depends on the
-	 * period and damping
-	 *
-	 * @param waypoint_switch_radius The switching radius the waypoint has set.
-	 */
-	float switch_distance(float waypoint_switch_radius);
-
 	/**
 	 * Navigate between two waypoints
 	 *
@@ -145,35 +109,6 @@ public:
 	 */
 	void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
 				const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
-	/**
-	 * Navigate on an orbit around a loiter waypoint.
-	 *
-	 * This allow orbits smaller than the L1 length,
-	 * this modification was introduced in [2].
-	 *
-	 * @return sets _lateral_accel setpoint
-	 */
-	void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
-			     const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector);
-
-	/**
-	 * Navigate on a fixed bearing.
-	 *
-	 * This only holds a certain direction and does not perform cross
-	 * track correction. Helpful for semi-autonomous modes. Introduced
-	 * by [2].
-	 *
-	 * @return sets _lateral_accel setpoint
-	 */
-	void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed);
-
-	/**
-	 * Keep the wings level.
-	 *
-	 * This is typically needed for maximum-lift-demand situations,
-	 * such as takeoff or near stall. Introduced in [2].
-	 */
-	void navigate_level_flight(float current_heading);

 	/**
 	 * Set the L1 period.
@@ -187,32 +122,11 @@ public:
 	 */
 	void set_l1_damping(float damping);

-	/**
-	 * Set the maximum roll angle output in radians
-	 */
-	void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
-
-	/**
-	 * Set roll angle slew rate. Set to zero to deactivate.
-	 */
-	void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; }
-
-	/**
-	 * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
-	 */
-	void set_dt(float dt) { _dt = dt;}
-
-	void reset_has_guidance_updated() { _has_guidance_updated = false; }
-
-	bool has_guidance_updated() { return _has_guidance_updated; }
-
 private:

 	float _lateral_accel{0.0f};		///< Lateral acceleration setpoint in m/s^2
 	float _L1_distance{20.0f};		///< L1 lead distance, defined by period and damping
-	bool _circle_mode{false};		///< flag for loiter mode
 	float _nav_bearing{0.0f};		///< bearing to L1 reference point
-	float _bearing_error{0.0f};		///< bearing error
 	float _crosstrack_error{0.0f};	///< crosstrack error in meters
 	float _target_bearing{0.0f};		///< the heading setpoint

@@ -221,21 +135,6 @@ private:
 	float _L1_ratio{5.0f};		///< L1 ratio for navigation
 	float _K_L1{2.0f};			///< L1 control gain for _L1_damping
 	float _heading_omega{1.0f};		///< Normalized frequency
-
-	float _roll_lim_rad{math::radians(30.0f)};  ///<maximum roll angle in radians
-	float _roll_setpoint{0.0f};	///< current roll angle setpoint in radians
-	float _roll_slew_rate{0.0f};	///< roll angle setpoint slew rate limit in rad/s
-	float _dt{0};				///< control loop time in seconds
-
-	bool _has_guidance_updated =
-		false;	///< this flag is set to true by any of the guidance methods. This flag has to be manually reset using has_guidance_updated_reset()
-
-	/**
-	 * Update roll angle setpoint. This will also apply slew rate limits if set.
-	 *
-	 */
-	void update_roll_setpoint();
-
 };


@@ -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²].
 	};
@@ -231,8 +231,10 @@ private:
 	hrt_abstime _last_disarmed_timestamp{0};
 	hrt_abstime _overload_start{0};		///< time when CPU overload started

-	hrt_abstime _led_armed_state_toggle{0};
-	hrt_abstime _led_overload_toggle{0};
+#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
+	hrt_abstime _led_armed_state_toggle {0};
+#endif
+	hrt_abstime _led_overload_toggle {0};

 	hrt_abstime _last_health_and_arming_check{0};

@@ -40,6 +40,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failsafe_flags.h>
 #include <systemlib/mavlink_log.h>
+#include <drivers/drv_hrt.h>

 #include <stdint.h>
 #include <limits.h>
@@ -245,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.
@@ -301,8 +304,6 @@ private:

 	NavModes reportedModes(NavModes required_modes);

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

 	friend class HealthAndArmingChecks;
@@ -68,7 +68,6 @@
 #include "checks/vtolCheck.hpp"
 #include "checks/offboardCheck.hpp"

-
 class HealthAndArmingChecks : public ModuleParams
 {
 public:
@@ -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
 				       )
@@ -38,12 +38,14 @@
 #ifdef __PX4_DARWIN
 #include <sys/param.h>
 #include <sys/mount.h>
-#else
+#elif defined(PX4_STORAGEDIR)
 #include <sys/statfs.h>
 #endif

 void SdCardChecks::checkAndReport(const Context &context, Report &reporter)
 {
+#ifdef PX4_STORAGEDIR
+
 	if (_param_com_arm_sdcard.get() > 0) {

 		struct statfs statfs_buf;
@@ -125,4 +127,5 @@ void SdCardChecks::checkAndReport(const Context &context, Report &reporter)
 	}

 #endif /* __PX4_NUTTX */
+#endif /* PX4_STORAGEDIR */
 }
@@ -44,11 +44,13 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;

 private:
-	bool _sdcard_detected{false};
+#ifdef PX4_STORAGEDIR
+	bool _sdcard_detected {false};

 #ifdef __PX4_NUTTX
 	bool _hardfault_checked_once {false};
-	bool _hardfault_file_present{false};
+	bool _hardfault_file_present {false};
+#endif
 #endif

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
@@ -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);
 }

@@ -112,7 +112,8 @@ public:

 		case Action::Terminate: return "Terminate";

-		case Action::Count: return "(invalid)";
+		case Action::Count:
+		default: return "(invalid)";
 		}
 	}

@@ -182,6 +182,13 @@ public:
 	 */
 	virtual const char *name() const = 0;

+	/**
+	 * Callback from the control allocation, allowing to manipulate the setpoint.
+	 * Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers).
+	 *
+	 * @param actuator_sp input & output setpoint
+	 */
+	virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {}

 	/**
 	 * Callback from the control allocation, allowing to manipulate the setpoint.
@@ -52,8 +52,15 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
 		_param_handles[i].torque[2] = param_find(buffer);
 		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
 		_param_handles[i].trim = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_FLAP", i);
+		_param_handles[i].scale_flap = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_SPOIL", i);
+		_param_handles[i].scale_spoiler = param_find(buffer);
 	}

+	_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
+	_spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate);
+
 	_count_handle = param_find("CA_SV_CS_COUNT");
 	updateParams();
 }
@@ -81,6 +88,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 		}

 		param_get(_param_handles[i].trim, &_params[i].trim);
+		param_get(_param_handles[i].scale_flap, &_params[i].scale_flap);
+		param_get(_param_handles[i].scale_spoiler, &_params[i].scale_spoiler);

 		// TODO: enforce limits (note that tailsitter uses different limits)?
 		switch (_params[i].type) {
@@ -109,12 +118,12 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 		case Type::RightVTail:
 			break;

-		case Type::LeftFlaps:
-		case Type::RightFlaps:
+		case Type::LeftFlap:
+		case Type::RightFlap:
 			torque.setZero();
 			break;

-		case Type::Airbrakes:
+		case Type::Airbrake:
 			torque.setZero();
 			break;

@@ -134,6 +143,10 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 			torque.setZero();
 			break;

+		case Type::LeftSpoiler:
+		case Type::RightSpoiler:
+			torque.setZero();
+			break;
 		}
 	}
 }
@@ -151,32 +164,25 @@ bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configura
 	return true;
 }

-void ActuatorEffectivenessControlSurfaces::applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control,
-		float wheel_control, int first_actuator_idx,
-		ActuatorVector &actuator_sp) const
+void ActuatorEffectivenessControlSurfaces::applyFlaps(float flaps_control, int first_actuator_idx, float dt,
+		ActuatorVector &actuator_sp)
 {
+	_flaps_setpoint_with_slewrate.update(flaps_control, dt);
+
 	for (int i = 0; i < _count; ++i) {
-		switch (_params[i].type) {
-		// TODO: check sign
-		case ActuatorEffectivenessControlSurfaces::Type::LeftFlaps:
-			actuator_sp(i + first_actuator_idx) += flaps_control;
-			break;
-
-		case ActuatorEffectivenessControlSurfaces::Type::RightFlaps:
-			actuator_sp(i + first_actuator_idx) -= flaps_control;
-			break;
-
-		case ActuatorEffectivenessControlSurfaces::Type::Airbrakes:
-			actuator_sp(i + first_actuator_idx) += airbrakes_control;
-			break;
-
-		case ActuatorEffectivenessControlSurfaces::Type::SteeringWheel:
-			actuator_sp(i + first_actuator_idx) += wheel_control;
-			break;
-
-		default:
-			break;
-		}
+		// map [0, 1] to [-1, 1]
+		// TODO: this currently only works for dedicated flaps, not flaperons
+		actuator_sp(i + first_actuator_idx) += (_flaps_setpoint_with_slewrate.getState() * 2.f - 1.f) * _params[i].scale_flap;
+	}
+}
+
+void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control, int first_actuator_idx, float dt,
+		ActuatorVector &actuator_sp)
+{
+	_spoilers_setpoint_with_slewrate.update(spoilers_control, dt);
+
+	for (int i = 0; i < _count; ++i) {
+		// TODO: this currently only works for spoilerons, not dedicated spoilers
+		actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler;
 	}
-
 }
@@ -36,6 +36,10 @@
 #include "ActuatorEffectiveness.hpp"

 #include <px4_platform_common/module_params.h>
+#include <lib/slew_rate/SlewRate.hpp>
+
+static constexpr float kFlapSlewRate = 0.5f; // slew rate for normalized flaps setpoint [1/s]
+static constexpr float kSpoilersSlewRate = 0.5f; // slew rate for normalized spoilers setpoint [1/s]

 class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness
 {
@@ -53,14 +57,16 @@ public:
 		RightElevon = 6,
 		LeftVTail = 7,
 		RightVTail = 8,
-		LeftFlaps = 9,
-		RightFlaps = 10,
-		Airbrakes = 11,
+		LeftFlap = 9,
+		RightFlap = 10,
+		Airbrake = 11,
 		Custom = 12,
 		LeftATail = 13,
 		RightATail = 14,
 		SingleChannelAileron = 15,
 		SteeringWheel = 16,
+		LeftSpoiler = 17,
+		RightSpoiler = 18,
 	};

 	struct Params {
@@ -68,6 +74,8 @@ public:

 		matrix::Vector3f torque;
 		float trim;
+		float scale_flap;
+		float scale_spoiler;
 	};

 	ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
@@ -81,8 +89,8 @@ public:

 	const Params &config(int idx) const { return _params[idx]; }

-	void applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control, float wheel_control, int first_actuator_idx,
-				      ActuatorVector &actuator_sp) const;
+	void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
+	void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);

 private:
 	void updateParams() override;
@@ -92,10 +100,15 @@ private:

 		param_t torque[3];
 		param_t trim;
+		param_t scale_flap;
+		param_t scale_spoiler;
 	};
 	ParamHandles _param_handles[MAX_COUNT];
 	param_t _count_handle;

 	Params _params[MAX_COUNT] {};
 	int _count{0};
+
+	SlewRate<float> _flaps_setpoint_with_slewrate;
+	SlewRate<float> _spoilers_setpoint_with_slewrate;
 };
@@ -61,18 +61,20 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	return (rotors_added_successfully && surfaces_added_successfully);
 }

-void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
 {
 	// apply flaps
-	actuator_controls_s actuator_controls_0;
+	normalized_unsigned_setpoint_s flaps_setpoint;

-	if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
-		const float flaps_control = actuator_controls_0.control[actuator_controls_s::INDEX_FLAPS];
-		const float airbrakes_control = actuator_controls_0.control[actuator_controls_s::INDEX_AIRBRAKES];
-		const float steering_wheel_control = actuator_controls_0.control[actuator_controls_s::INDEX_YAW];
-		_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
-				_first_control_surface_idx, actuator_sp);
+	if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+		_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+	}
+
+	// apply spoilers
+	normalized_unsigned_setpoint_s spoilers_setpoint;
+
+	if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+		_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
 	}
 }
@@ -37,7 +37,7 @@
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"

-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>

 class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness
 {
@@ -49,15 +49,14 @@ public:

 	const char *name() const override { return "Fixed Wing"; }

-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;

 private:
 	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;

-	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};

 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 };
@@ -63,20 +63,22 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 	return (mc_rotors_added_successfully && surfaces_added_successfully);
 }

-void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
 {
-	// apply flaps
 	if (matrix_index == 1) {
-		actuator_controls_s actuator_controls_1;
+		// apply flaps
+		normalized_unsigned_setpoint_s flaps_setpoint;

-		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
-			const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
-			const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
-			_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
-					_first_control_surface_idx, actuator_sp);
+		if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+			_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+
+		// apply spoilers
+		normalized_unsigned_setpoint_s spoilers_setpoint;
+
+		if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
 		}
 	}
 }
@@ -45,7 +45,8 @@
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"

-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+

 class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness
 {
@@ -72,9 +73,7 @@ public:
 		normalize[1] = false;
 	}

-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;

 	void setFlightPhase(const FlightPhase &flight_phase) override;

@@ -89,6 +88,7 @@ private:

 	int _first_control_surface_idx{0}; ///< applies to matrix 1

-	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};

 };
@@ -68,6 +68,27 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
 	return (mc_rotors_added_successfully && surfaces_added_successfully);
 }

+void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
+{
+	if (matrix_index == 1) {
+		// apply flaps
+		normalized_unsigned_setpoint_s flaps_setpoint;
+
+		if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+			_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+
+		// apply spoilers
+		normalized_unsigned_setpoint_s spoilers_setpoint;
+
+		if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+	}
+
+}
+
 void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
 {
 	if (_flight_phase == flight_phase) {
@@ -43,7 +43,8 @@
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"

-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+
 #include <uORB/Subscription.hpp>

 class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness
@@ -69,6 +70,8 @@ public:
 		normalize[1] = false;
 	}

+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
+
 	void setFlightPhase(const FlightPhase &flight_phase) override;

 	const char *name() const override { return "VTOL Tailsitter"; }
@@ -82,5 +85,6 @@ protected:

 	int _first_control_surface_idx{0}; ///< applies to matrix 1

-	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 };
@@ -91,29 +91,38 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }

+void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
+{
+	if (matrix_index == 1) {
+		// apply flaps
+		normalized_unsigned_setpoint_s flaps_setpoint;
+
+		if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+			_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+
+		// apply spoilers
+		normalized_unsigned_setpoint_s spoilers_setpoint;
+
+		if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+	}
+
+}
+
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
-	// apply flaps
-	if (matrix_index == 1) {
-		actuator_controls_s actuator_controls_1;
-
-		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
-			const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
-			const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
-			_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
-					_first_control_surface_idx, actuator_sp);
-		}
-	}
-
 	// apply tilt
 	if (matrix_index == 0) {
-		actuator_controls_s actuator_controls_1;

-		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			float control_collective_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
+		tiltrotor_extra_controls_s tiltrotor_extra_controls;
+
+		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
+			float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;

 			// set control_collective_tilt to exactly -1 or 1 if close to these end points
 			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
@@ -133,16 +142,12 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
 			}
-		}

-		// in FW directly use throttle sp
-		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			// in FW directly use throttle sp
+			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {

-			actuator_controls_s actuator_controls_0;
-
-			if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
 				for (int i = 0; i < _first_tilt_idx; ++i) {
-					actuator_sp(i) = actuator_controls_0.control[actuator_controls_s::INDEX_THROTTLE];
+					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
 				}
 			}
 		}
@@ -46,7 +46,9 @@
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 #include "ActuatorEffectivenessTilts.hpp"

-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+#include <uORB/topics/tiltrotor_extra_controls.h>
+
 #include <uORB/Subscription.hpp>

 class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness
@@ -74,6 +76,8 @@ public:

 	void setFlightPhase(const FlightPhase &flight_phase) override;

+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
+
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
@@ -98,8 +102,8 @@ protected:

 	float _last_collective_tilt_control{NAN};

-	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
-	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};

 	struct YawTiltSaturationFlags {
 		bool tilt_yaw_pos;
@@ -107,4 +111,6 @@ protected:
 	};

 	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
+
+	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
 };
@@ -52,4 +52,5 @@ px4_add_module(
 		ActuatorEffectiveness
 		ControlAllocation
 		px4_work_queue
+		SlewRate
 )
@@ -419,6 +419,7 @@ ControlAllocator::Run()

 			// Do allocation
 			_control_allocation[i]->allocate();
+			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
 								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());

@@ -266,12 +266,14 @@ parameters:
                8: Right V-Tail
                9: Left Flap
                10: Right Flap
-                11: Airbrakes
+                11: Airbrake
                12: Custom
                13: Left A-tail
                14: Right A-tail
                15: Single Channel Aileron
                16: Steering Wheel
+                17: Left Spoiler
+                18: Right Spoiler
            num_instances: *max_num_servos
            instance_start: 0
            default: 0
@@ -302,6 +304,7 @@ parameters:
            num_instances: *max_num_servos
            instance_start: 0
            default: 0.0
+
        CA_SV_CS${i}_TRIM:
            description:
                short: Control Surface ${i} trim
@@ -314,6 +317,28 @@ parameters:
            instance_start: 0
            default: 0.0

+        CA_SV_CS${i}_FLAP:
+            description:
+                short: Control Surface ${i} configuration as flap
+            type: float
+            decimal: 2
+            min: -1.0
+            max: 1.0
+            num_instances: *max_num_servos
+            instance_start: 0
+            default: 0
+
+        CA_SV_CS${i}_SPOIL:
+            description:
+                short: Control Surface ${i} configuration as spoiler
+            type: float
+            decimal: 2
+            min: -1.0
+            max: 1.0
+            num_instances: *max_num_servos
+            instance_start: 0
+            default: 0
+
        # Tilts
        CA_SV_TL_COUNT:
            description:
@@ -534,7 +559,7 @@ mixer:

    rules:
      - select_identifier: 'servo-type' # restrict torque based on servo type
-        apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw']
+        apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler']
        items:
        # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive.
        # By default the scale is set to 1/N, where N is the amount of actuators with an effect on
@@ -543,54 +568,117 @@ mixer:
              - { 'disabled': True, 'default': 0.0 } # roll
              - { 'disabled': True, 'default': 0.0 } # pitch
              - { 'disabled': True, 'default': 0.0 } # yaw
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            1: # Left Aileron
              - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll
              - { 'hidden': True, 'default': 0.0 } # pitch
              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            2: # Right Aileron
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll
              - { 'hidden': True, 'default': 0.0 } # pitch
              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            3: # Elevator
              - { 'hidden': True, 'default': 0.0 } # roll
              - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch
              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            4: # Rudder
              - { 'hidden': True, 'default': 0.0 } # roll
              - { 'hidden': True, 'default': 0.0 } # pitch
              - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            5: # Left Elevon
              - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            6: # Right Elevon
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            7: # Left V Tail
              - { 'hidden': True, 'default': 0.0 } # roll
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            8: # Right V Tail
              - { 'hidden': True, 'default': 0.0 } # roll
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
              - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
+            9: # Left Flap
+              - { 'hidden': True, 'default': 0.0 } # roll
+              - { 'hidden': True, 'default': 0.0 } # pitch
+              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
+            10: # Right Flap
+              - { 'hidden': True, 'default': 0.0 } # roll
+              - { 'hidden': True, 'default': 0.0 } # pitch
+              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
+            11: # Airbrake
+              - { 'hidden': True, 'default': 0.0 } # roll
+              - { 'hidden': True, 'default': 0.0 } # pitch
+              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
+            12: # Custom
+              - { 'hidden': False, 'default': 0.0 } # roll
+              - { 'hidden': False, 'default': 0.0 } # pitch
+              - { 'hidden': False, 'default': 0.0 } # yaw
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            13: # Left A Tail
              - { 'hidden': True, 'default': 0.0 } # roll
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
              - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            14: # Right A Tail
              - { 'hidden': True, 'default': 0.0 } # roll
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch
              - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            15: # Single Channel Aileron
              - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll
              - { 'hidden': True, 'default': 0.0 } # pitch
              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
            16: # Steering Wheel
              - { 'hidden': True, 'default': 0.0 } # roll
              - { 'hidden': True, 'default': 0.0 } # pitch
              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler
+            17: # Left Spoiler
+              - { 'hidden': True, 'default': 0.0 } # roll
+              - { 'hidden': True, 'default': 0.0 } # pitch
+              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler
+            18: # Right Spoiler
+              - { 'hidden': True, 'default': 0.0 } # roll
+              - { 'hidden': True, 'default': 0.0 } # pitch
+              - { 'hidden': True, 'default': 0.0 } # yaw
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap
+              - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler
+

      - select_identifier: 'servo-type-tailsitter' # restrict torque based on servo type for tailsitters
        apply_identifiers: ['servo-torque-roll-tailsitter', 'servo-torque-pitch-tailsitter', 'servo-torque-yaw-tailsitter']
@@ -662,6 +750,14 @@ mixer:
                            identifier: 'servo-torque-yaw'
                          - name: 'CA_SV_CS${i}_TRIM'
                            label: 'Trim'
+                          - name: 'CA_SV_CS${i}_FLAP'
+                            label: 'Flaps Scale'
+                            advanced: true
+                            identifier: 'servo-scale-flap'
+                          - name: 'CA_SV_CS${i}_SPOIL'
+                            label: 'Spoiler Scale'
+                            advanced: true
+                            identifier: 'servo-scale-spoiler'

            2: # Standard VTOL
                title: 'Standard VTOL'
@@ -706,6 +802,14 @@ mixer:
                            identifier: 'servo-torque-yaw'
                          - name: 'CA_SV_CS${i}_TRIM'
                            label: 'Trim'
+                          - name: 'CA_SV_CS${i}_FLAP'
+                            label: 'Flaps Scale'
+                            advanced: true
+                            identifier: 'servo-scale-flap'
+                          - name: 'CA_SV_CS${i}_SPOIL'
+                            label: 'Spoiler Scale'
+                            advanced: true
+                            identifier: 'servo-scale-spoiler'
                    parameters:
                      - label: 'Lock control surfaces in hover'
                        name: VT_ELEV_MC_LOCK
@@ -747,6 +851,14 @@ mixer:
                            identifier: 'servo-torque-yaw'
                          - name: 'CA_SV_CS${i}_TRIM'
                            label: 'Trim'
+                          - name: 'CA_SV_CS${i}_FLAP'
+                            label: 'Flaps Scale'
+                            advanced: true
+                            identifier: 'servo-scale-flap'
+                          - name: 'CA_SV_CS${i}_SPOIL'
+                            label: 'Spoiler Scale'
+                            advanced: true
+                            identifier: 'servo-scale-spoiler'
                    parameters:
                      - label: 'Lock control surfaces in hover'
                        name: VT_ELEV_MC_LOCK
@@ -800,6 +912,14 @@ mixer:
                            identifier: 'servo-torque-yaw-tailsitter'
                          - name: 'CA_SV_CS${i}_TRIM'
                            label: 'Trim'
+                          - name: 'CA_SV_CS${i}_FLAP'
+                            label: 'Flaps Scale'
+                            advanced: true
+                            identifier: 'servo-scale-flap'
+                          - name: 'CA_SV_CS${i}_SPOIL'
+                            label: 'Spoiler Scale'
+                            advanced: true
+                            identifier: 'servo-scale-spoiler'
                    parameters:
                      - label: 'Lock control surfaces in hover'
                        name: VT_ELEV_MC_LOCK
@@ -912,6 +1032,14 @@ mixer:
                            label: 'Yaw Scale'
                          - name: 'CA_SV_CS${i}_TRIM'
                            label: 'Trim'
+                          - name: 'CA_SV_CS${i}_FLAP'
+                            label: 'Flaps Scale'
+                            advanced: true
+                            identifier: 'servo-scale-flap'
+                          - name: 'CA_SV_CS${i}_SPOIL'
+                            label: 'Spoiler Scale'
+                            advanced: true
+                            identifier: 'servo-scale-spoiler'

            10: # Helicopter
                actuators:
@@ -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();
@@ -176,10 +176,16 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)

 					if (do_vel_pos_reset) {
 						ECL_WARN("GPS fusion timeout, resetting velocity and position");
+
+						// reset velocity
 						_information_events.flags.reset_vel_to_gps = true;
+						resetVelocityTo(velocity, vel_obs_var);
+						_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
+
+						// reset position
 						_information_events.flags.reset_pos_to_gps = true;
-						resetVelocityTo(gps_sample.vel, vel_obs_var);
-						resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
+						resetHorizontalPositionTo(position, pos_obs_var);
+						_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
 					}

 				} else {
@@ -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;

@@ -710,6 +710,18 @@ void EKF2::VerifyParams()
 				    "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get());
 	}

+	if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV)
+	    && !(_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS))) {
+		_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VPOS));
+		_param_ekf2_ev_ctrl.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "EV vertical position enabled by EKF2_HGT_REF\n");
+		/* EVENT
+		 * @description <param>EKF2_EV_CTRL</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_hgt_ref_ev"), events::Log::Warning,
+				    "EV vertical position enabled by EKF2_HGT_REF", _param_ekf2_ev_ctrl.get());
+	}
+
 	// EV EKF2_AID_MASK -> EKF2_EV_CTRL
 	if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)
 	    || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)
@@ -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
@@ -36,6 +36,13 @@

 #include "FlightTaskDescend.hpp"

+bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+	return ret;
+}
+
 bool FlightTaskDescend::update()
 {
 	bool ret = FlightTask::update();
@@ -49,6 +49,7 @@ public:
 	virtual ~FlightTaskDescend() = default;

 	bool update() override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;

 private:
 	Sticks _sticks{this};
@@ -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;
 }
--- a/Show More
+++ b/Show More
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
Konrad	7098970a38	Tools: extend documentation parser: - Add the possibility in the parser to replace the defines made in the current file with their argument (includes are not supported) - Add the possibility for the parser to parse int argument with bitwise shift operators	2023-03-06 18:34:01 -05:00
PX4 BuildBot	603d4b999b	Update submodule sitl_gazebo-classic to latest Mon Mar 6 12:38:18 UTC 2023 - sitl_gazebo-classic in PX4/Firmware (`7edce94b93`): https://github.com/PX4/PX4-SITL_gazebo-classic/commit/9343aaf4e275db48fce02dd25c5bd8273c2d583a - sitl_gazebo-classic current upstream: https://github.com/PX4/PX4-SITL_gazebo-classic/commit/e3722bf9132567e8dd08b7ed6df2986e21a6ec18 - Changes: https://github.com/PX4/PX4-SITL_gazebo-classic/compare/9343aaf4e275db48fce02dd25c5bd8273c2d583a...e3722bf9132567e8dd08b7ed6df2986e21a6ec18 e3722bf 2023-02-24 Frederic Taillandier - Allowing to override sniffer's modules ip (#963) 2221c95 2023-02-24 Frederic Taillandier - removing macos 1015 github actions (#962) 48e9b17 2023-02-24 frederic@auterion.com - removing debug 265198d 2023-02-24 frederic@auterion.com - fixing indentation de9bf14 2023-02-24 frederic@auterion.com - try fix build by updating the path to the right gazebo ec8641d 2022-08-24 Konrad - Revert "Enable multi IMU capability in gazebo mavlink interface" 487a789 2023-02-21 Jaeyoung Lim - Revert "Add multi magnetometer capability. Magnetometer plugin now derived from sensorplugin. Magnetometer topic dervied from naming. Updated all models with magnetometer submodel"	2023-03-06 18:33:00 -05:00
Silvan Fuhrer	2d92bd627a	FWRateController: always update manual_control_setpoint if in manual and FW Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 22:43:41 +01:00
Silvan Fuhrer	caee131e6a	FW Position Controller: mini fw_control_yaw_wheel refactoring Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 22:43:41 +01:00
Silvan Fuhrer	1e56d9c219	Rework flaps/spoilers logic - remove deprecated actuator_controls[INDEX_FLAPS/SPOILERS/AIRBRAKES] - use new topic normalized_unsigned_setpoint.msg (with instances flaps_setpoint and spoilers_setpoint) to pass into control allocation - remove flaps/spoiler related fields from attitude_setpoint topic - CA: add possibility to map flaps/spoilers to any control surface - move flaps/spoiler pitch trimming to CA (previously called DTRIM_FLAPS/SPOILER) - move manual flaps/spoiler handling from rate to attitude controller FW Position controller: change how negative switch readings are intepreted for flaps/spoilers (considered negative as 0). VTOL: Rework spoiler publishing in hover - pushlish spoiler_setpoint.msg in the VTOL module if in hover - also set spoilers to land configuration if in Descend mode Allocation: add slew rate limit of 0.5 to flaps/spoilers configuration change Instead of doing the flaps/spoilers slew rate limiting in the FW Position Controller (which then only is applied in Auto flight), do it consistently over all flight modes, so also for manual modes. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 22:43:41 +01:00
Silvan Fuhrer	16594bffa9	Rework landing gear logic - remove deprecated actuator_controls[INDEX_LANDING_GEAR] - remove dead code in mc rate controller that used to prevent it from being retracted on the ground (anyway had no effect as it only affected the actuator_control[LANDING_GEAR] which wasn't sent to the control allocation) - for VTOLs handle deployment/retraction of landing gear in AUTO as a MC (retract if more than 2m above ground, deploy if WP is a landing WP), plus additionally when transition flight task is called (ALTITUDE mode and higher) - for FW in AUTO: add logic in FW Position Controller, depending on waypoint type mainly - manual landing gear settings always come through (a manual command overrides a previous auto command, and vice-versa) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 22:43:41 +01:00
Silvan Fuhrer	3e884116c4	logged_topics: make landing_gear_wheel optional and increase interval to 100ms Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 22:43:41 +01:00
Silvan Fuhrer	4b54ddfe61	Remove INDEX_COLLECTIVE_TILT from actuator_controls and instead use new topic tiltrotor_extra_controls Tiltrotor_extra_controls also contains collective thrust beside collective tilt, as passing a 3D thrust setpoint vector beside the tilt is not feasible. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 22:43:41 +01:00
Eric Katzfey	21c7f8ad74	posix server: changed the method of checking and setting the server file lock (#21243 ) * Changed the method of checking and setting the server file lock on Posix to avoid conditions where the server can indicate that it is running but still hasn't finished it's initialization	2023-03-06 09:55:57 -05:00
Eric Katzfey	5cade89499	Improve logging for Modal IO ESC (#21188 ) - always publish esc_status - when enabled via MODAL_IO_VLOG param, enable actuator debug output - for modal_io commands, use ESC HW ID values instead of motor number for easier use - publish esc_status message for command line commands - Uncommented the code that fills in the cmdcount and power fields in the esc_status topic --------- Co-authored-by: Travis Bottalico <travis@modalai.com>	2023-03-06 09:51:22 -05:00
Eric Katzfey	daa302cdbe	Changes to allow the commander module to be built and run on Qurt (#21186 ) * Changed exclusion to rely on the definition of PX4_STORAGEDIR	2023-03-06 09:49:07 -05:00
Silvan Fuhrer	dc4926dc4d	remove WheelEncoders.msg Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 09:43:01 -05:00
Silvan Fuhrer	0633d0d826	drivers: remove RoboClaw Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 09:43:01 -05:00
Jaeyoung Lim	e5d5fcd315	Subscribe to vehicle odometry in GZ Bridge This PR subscribes to the vehicle odometry in gz bridge / Add x500_vision model Fix transforms F	2023-03-06 09:27:35 -05:00
Tahsincan Köse	8737099a33	commander: failsafe framework fix missing return in actionStr function (#21245 ) - there needs to be a default statement for the compiler to work when this function is called.	2023-03-06 09:21:20 -05:00
Beniamino Pozzan	b79578fa55	efk2: Force external vision vertical position if EKF2_HGT_REF=VISION Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-06 09:19:49 -05:00
Daniel Agar	3fac85369e	ekf2: gps control use adjusted velocity and position for reset	2023-03-06 09:03:39 -05:00
Silvan Fuhrer	95754876ed	Apply small suggestions from code review Further param description improvements. Co-authored-by: Matthias Grob <maetugr@gmail.com>	2023-03-06 14:00:02 +01:00
Silvan Fuhrer	ec38ec660c	FW controllers: make param description more concise Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 14:00:02 +01:00
Silvan Fuhrer	4be74befd2	VTOL: remove pusher reverse feature Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 14:00:02 +01:00
Silvan Fuhrer	c09bf66639	VTOL: make param descripion more concise Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 14:00:02 +01:00
Silvan Fuhrer	9a038281c5	RoverPositionController: remove some unused stuff Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 14:00:02 +01:00
Silvan Fuhrer	feec8b2036	L1: remove some functions that Rover doesn't need Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-06 14:00:02 +01:00