WIP: adis16477 debug

mpu9250: try all I2C addresses if not manually specified
Double the allocated stack size of 64-bit NuttX built-in modules
2026-05-24 21:37:34 +08:00 · 2022-08-09 11:13:17 -04:00 · 2022-08-09 10:05:33 -04:00 · 2022-08-09 08:08:54 +02:00 · 2022-08-08 21:27:47 -04:00 · 2022-08-08 21:27:47 -04:00
159 changed files with 3631 additions and 1132 deletions
@@ -20,7 +20,7 @@ A clear and concise description of what you expected to happen.

 ## Log Files and Screenshots
 *Always* provide a link to the flight log file:
- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/master/en/getting_started/flight_reporting.html)).
+- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)).
 - Upload the log to the [PX4 Flight Review](http://logs.px4.io/)
 - Share the link to the log (Copy and paste the URL of the log)

@@ -8,7 +8,7 @@ First [fork and clone](https://help.github.com/articles/fork-a-repo) the project

 ### Create a feature branch

-*Always* branch off master for new features.
+*Always* branch off main for new features.

 ```
 git checkout -b mydescriptivebranchname
@@ -16,7 +16,7 @@ git checkout -b mydescriptivebranchname

 ### Edit and build the code

-The [developer guide](http://dev.px4.io/) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](https://dev.px4.io/master/en/contribute/code.html) when editing files.
+The [developer guide](https://docs.px4.io/main/en/development/development.html) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](https://docs.px4.io/main/en/contribute/code.html) when editing files.

 ### Commit your changes

@@ -7,7 +7,8 @@ pipeline {
    stage('Analysis') {
      when {
        anyOf {
-          branch 'master'
+          branch 'main'
+          branch 'master' // should be removed, but in case there is something going on...
          branch 'pr-jenkins' // for testing
        }
      }
@@ -204,20 +205,21 @@ pipeline {
            unstash 'msg_documentation'
            unstash 'uorb_graph'
            withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
-              sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_user_guide.git')
-              sh('cp airframes.md px4_user_guide/en/airframes/airframe_reference.md')
-              sh('cp parameters.md px4_user_guide/en/advanced_config/parameter_reference.md')
-              sh('cp -R modules/*.md px4_user_guide/en/modules/')
-              sh('cp -R graph_*.json px4_user_guide/.vuepress/public/en/middleware/')
-              sh('cp -R msg_docs/*.md px4_user_guide/en/msg_docs/')
-              sh('cd px4_user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true')
-              sh('cd px4_user_guide; git push origin master || true')
-              sh('rm -rf px4_user_guide')
+              sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/PX4-user_guide.git')
+              sh('cp airframes.md PX4-user_guide/en/airframes/airframe_reference.md')
+              sh('cp parameters.md PX4-user_guide/en/advanced_config/parameter_reference.md')
+              sh('cp -R modules/*.md PX4-user_guide/en/modules/')
+              sh('cp -R graph_*.json PX4-user_guide/.vuepress/public/en/middleware/')
+              sh('cp -R msg_docs/*.md PX4-user_guide/en/msg_docs/')
+              sh('cd PX4-user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true')
+              sh('cd PX4-user_guide; git push origin main || true')
+              sh('rm -rf PX4-user_guide')
            }
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@@ -245,7 +247,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@@ -278,7 +281,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@@ -307,7 +311,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@@ -350,7 +355,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@@ -373,7 +379,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@@ -10,8 +10,8 @@ This repository holds the [PX4](http://px4.io) flight control solution for drone

 PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out of the box.

-* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/master/LICENSE))
-* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
+* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/main/LICENSE))
+* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](https://px4.io/ecosystem/commercial-systems/)):
  * [Multicopters](https://docs.px4.io/main/en/frames_multicopter/)
  * [Fixed wing](https://docs.px4.io/main/en/frames_plane/)
  * [VTOL](https://docs.px4.io/main/en/frames_vtol/)
@@ -13,12 +13,9 @@ param set-default FW_LND_ANG 8

 param set-default FW_L1_PERIOD 12

-param set-default FW_MAN_P_MAX 30
-
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15

 param set-default FW_RR_FF 0.1
@@ -13,12 +13,9 @@ param set-default FW_LND_ANG 8

 param set-default FW_L1_PERIOD 12

-param set-default FW_MAN_P_MAX 30
-
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15

 param set-default FW_RR_FF 0.1
@@ -49,11 +49,9 @@ param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
@@ -44,11 +44,9 @@ param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_REV 96 # invert both elevons

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_I 0.2
 param set-default FW_PR_P 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_P 0.2
 param set-default FW_THR_TRIM 0.33
@@ -56,11 +56,9 @@ param set-default PWM_MAIN_FUNC10 202
 param set-default PWM_MAIN_FUNC11 203

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
@@ -8,11 +8,9 @@
 . ${R}etc/init.d/rc.vtol_defaults

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
@@ -38,7 +38,6 @@ param set-default FW_P_LIM_MAX 25
 param set-default FW_P_LIM_MIN -5
 param set-default FW_R_LIM 30

-param set-default FW_MAN_P_MAX 30.0
 param set-default FW_MAN_R_MAX 30.0

 param set-default FW_THR_TRIM 0.8
@@ -44,7 +44,6 @@ param set-default FW_P_LIM_MAX 25
 param set-default FW_P_LIM_MIN -5
 param set-default FW_R_LIM 30

-param set-default FW_MAN_P_MAX 30.0
 param set-default FW_MAN_R_MAX 30.0

 param set-default FW_THR_CRUISE 0.8
@@ -45,7 +45,6 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_P_ACC 3
 param set-default MPC_Z_VEL_P_ACC 12
 param set-default MPC_Z_VEL_I_ACC 3
-param set-default MPC_Z_VEL_MAX_DN 1.5

 param set-default NAV_ACC_RAD 5
 param set-default NAV_DLL_ACT 2
@@ -31,7 +31,6 @@ param set-default MC_YAWRATE_I 0.04
 param set-default MC_YAWRATE_MAX 40

 param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_LAND_SPEED 0.8
 param set-default MPC_YAWRAUTO_MAX 40

@@ -34,8 +34,6 @@ param set-default FW_ACRO_X_MAX   270
 param set-default FW_ACRO_Y_MAX   270
 param set-default FW_ACRO_Z_MAX   180
 param set-default FW_PSP_OFF  5
-param set-default FW_P_LIM_MAX    30
-param set-default FW_P_LIM_MIN    -30
 param set-default FW_RR_FF    0.33
 param set-default FW_RR_P 0.11

@@ -74,4 +72,4 @@ then
 	set PWM_OUT 1234
 else
 	set PWM_OUT 3456
-fi	
+fi
@@ -55,7 +55,6 @@ param set-default FW_T_SINK_MIN 1
 param set-default FW_T_VERT_ACC 6
 param set-default FW_THR_TRIM 0.70
 param set-default FW_THR_SLEW_MAX 1
-param set-default FW_MAN_P_MAX 30
 param set-default FW_P_LIM_MAX 15
 param set-default FW_P_LIM_MIN -25
 param set-default FW_P_RMAX_NEG 45
@@ -90,7 +89,6 @@ param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_LAND_SPEED 1.2
 param set-default MPC_TILTMAX_LND 35
 param set-default MPC_Z_VEL_MAX_UP 1.5
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_HOLD_MAX_XY 0.5
 param set-default MPC_HOLD_MAX_Z 0.5
 param set-default MPC_TKO_RAMP_T 0.8
@@ -31,7 +31,6 @@ param set-default FW_AIRSPD_MAX 30
 param set-default FW_AIRSPD_MIN 19
 param set-default FW_AIRSPD_TRIM 23
 param set-default FW_L1_R_SLEW_MAX 40
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PSP_OFF 3
 param set-default FW_P_LIM_MAX 18
 param set-default FW_P_LIM_MIN -25
@@ -71,7 +70,6 @@ param set-default MPC_VEL_MANUAL 3
 param set-default MPC_XY_CRUISE 3
 param set-default MPC_XY_VEL_MAX 3.5
 param set-default MPC_YAWRAUTO_MAX 40
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_Z_VEL_MAX_UP 2

 param set-default NAV_ACC_RAD 3
@@ -2,7 +2,7 @@
 #
 # @name Phantom FPV Flying Wing
 #
-# @url https://docs.px4.io/master/en/frames_plane/wing_wing_z84.html
+# @url https://docs.px4.io/main/en/frames_plane/wing_wing_z84.html
 #
 # @type Flying Wing
 # @class Plane
@@ -2,7 +2,7 @@
 #
 # @name Wing Wing (aka Z-84) Flying Wing
 #
-# @url https://docs.px4.io/master/en/frames_plane/wing_wing_z84.html
+# @url https://docs.px4.io/main/en/frames_plane/wing_wing_z84.html
 #
 # @type Flying Wing
 # @class Plane
@@ -28,7 +28,6 @@ param set-default FW_L1_PERIOD 15
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 8
 param set-default FW_P_LIM_MAX 20
-param set-default FW_P_LIM_MIN -30
 param set-default FW_R_LIM 45
 param set-default FW_PR_FF 0.45
 param set-default FW_PR_P 0.005
@@ -1,7 +1,7 @@
 #!/bin/sh
 #
 # @name Spedix S250AQ
-# @url https://docs.px4.io/master/en/frames_multicopter/spedix_s250_pixracer.html
+# @url https://docs.px4.io/main/en/frames_multicopter/spedix_s250_pixracer.html
 #
 # @type Quadrotor asymmetric
 # @class Copter
@@ -2,7 +2,7 @@
 #
 # @name HolyBro QAV250
 #
-# @url https://docs.px4.io/master/en/frames_multicopter/holybro_qav250_pixhawk4_mini.html
+# @url https://docs.px4.io/main/en/frames_multicopter/holybro_qav250_pixhawk4_mini.html
 #
 # @type Quadrotor x
 # @class Copter
@@ -21,16 +21,20 @@ param set-default HTE_VXY_THR 2.0
 param set-default MIS_DIST_WPS 5000

 param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_TKO_SPEED 1
-param set-default MPC_VEL_MANUAL 3
-param set-default MPC_XY_CRUISE 3
+param set-default MPC_VEL_MANUAL 5
+param set-default MPC_XY_CRUISE 5
 param set-default MPC_XY_ERR_MAX 5
-param set-default MPC_XY_VEL_MAX 4
-param set-default MPC_Z_VEL_MAX_DN 1.5
+param set-default MPC_XY_VEL_MAX 8
 param set-default MPC_JERK_MAX 4.5
 param set-default MPC_YAW_MODE 4

-param set-default NAV_ACC_RAD 3
+# reduce aggressiveness around roll and yaw axis,
+# as VTOLs usually have high intertia and lot af drag due to wings
+param set-default MC_ROLL_P 5
+param set-default MC_ROLLRATE_MAX 120
+param set-default MC_YAW_P 2
+param set-default MC_YAWRATE_MAX 120
+param set-default MPC_MAN_Y_MAX 90

 param set-default PWM_AUX_RATE 50
 param set-default PWM_MAIN_RATE 400
@@ -330,7 +330,7 @@ def get_mixers(yaml_config, output_functions, verbose):
        option = select_param + '==' + str(type_index)
        mixer_config = {
                'option': option,
-                'help-url': 'https://docs.px4.io/master/en/config/actuators.html',
+                'help-url': 'https://docs.px4.io/main/en/config/actuators.html',
            }
        for optional in ['type', 'title']:
            if optional in current_type:
@@ -8,7 +8,7 @@ class MarkdownTablesOutput():
        result = """# Airframes Reference

 :::note
-**This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/Tools/px4airframes/markdownout.py) from the source code** using the build command: `make airframe_metadata`.
+**This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/px4airframes/markdownout.py) from the source code** using the build command: `make airframe_metadata`.
 :::

 This page lists all supported airframes and types including the motor assignment and numbering.
@@ -69,7 +69,7 @@ The generated files will be written to the `modules` directory.
        result = ''
        for module in module_list:
            result += "## %s\n" % module.name()
-            result += "Source: [%s](https://github.com/PX4/PX4-Autopilot/tree/master/src/%s)\n\n" % (module.scope(), module.scope())
+            result += "Source: [%s](https://github.com/PX4/PX4-Autopilot/tree/main/src/%s)\n\n" % (module.scope(), module.scope())
            doc = module.documentation()
            if len(doc) > 0:
                result += "%s\n" % doc
@@ -12,7 +12,7 @@ class ModuleDocumentation(object):
    """

    # If you add categories or subcategories, they also need to be added to the
-    # TOC in https://github.com/PX4/Devguide/blob/master/en/SUMMARY.md
+    # TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
@@ -331,7 +331,7 @@ class uploader(object):

        except NotImplementedError:
            raise RuntimeError("Programing not supported for this version of silicon!\n"
-                               "See https://docs.px4.io/master/en/flight_controller/silicon_errata.html")
+                               "See https://docs.px4.io/main/en/flight_controller/silicon_errata.html")
        except RuntimeError:
            # timeout, no response yet
            return False
@@ -694,7 +694,7 @@ class OutputJSON(object):
            node['type'] = 'topic'
            node['color'] = topic_colors[topic]
            # url is opened when double-clicking on the node
-            node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/master/msg/'+topic_filename(topic)+'.msg'
+            node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/main/msg/'+topic_filename(topic)+'.msg'
            nodes.append(node)

        data['nodes'] = nodes
@@ -41,6 +41,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -56,6 +56,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
@@ -56,6 +56,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
@@ -55,6 +55,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
@@ -42,6 +42,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -59,6 +59,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
@@ -42,6 +42,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -56,6 +56,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
@@ -47,6 +47,7 @@ add_dependencies(drivers_board arch_board_hw_info)
 target_link_libraries(drivers_board
 	PRIVATE
 		arch_board_hw_info
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -57,6 +57,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
@@ -57,6 +57,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
@@ -57,6 +57,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
@@ -46,6 +46,7 @@ add_dependencies(drivers_board arch_board_hw_info)

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		arch_board_hw_info
 		drivers__led # drv_led_start
@@ -60,6 +60,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
@@ -43,6 +43,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -43,6 +43,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -55,6 +55,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
@@ -55,6 +55,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
@@ -55,6 +55,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
@@ -46,6 +46,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
 		drivers__led # drv_led_start
@@ -45,6 +45,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
 		drivers__led # drv_led_start
@@ -52,6 +52,7 @@ add_dependencies(drivers_board nuttx_context)
 target_link_libraries(drivers_board
 	PRIVATE
 		arch_board_hw_info
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -24,6 +24,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -9,12 +9,3 @@ param set-default BAT2_V_DIV 18.1

 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152
-
-# Mavlink ethernet (CFG 1000)
-param set-default MAV_2_CONFIG 1000
-param set-default MAV_2_BROADCAST 1
-param set-default MAV_2_MODE 0
-param set-default MAV_2_RADIO_CTL 0
-param set-default MAV_2_RATE 100000
-param set-default MAV_2_REMOTE_PRT 14550
-param set-default MAV_2_UDP_PRT 14550
@@ -60,6 +60,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
@@ -60,6 +60,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
@@ -46,6 +46,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
@@ -4,7 +4,7 @@ project(googletest-download NONE)

 include(ExternalProject)
 ExternalProject_Add(googletest
-	URL https://github.com/google/googletest/archive/8b6d3f9c4a774bef3081195d422993323b6bb2e0.zip
+	URL https://github.com/google/googletest/archive/58d77fa8070e8cec2dc1ed015d66b454c8d78850.zip # 1.12.1
 	SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
 	BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
 	CONFIGURE_COMMAND ""
@@ -310,7 +310,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	endif()

    if (NO_HELP)
-		add_definitions(-DCONSTRAINED_FLASH_NO_HELP="https://docs.px4.io/master/en/modules/modules_main.html")
+		add_definitions(-DCONSTRAINED_FLASH_NO_HELP="https://docs.px4.io/main/en/modules/modules_main.html")
 	endif()

 	if(CONSTRAINED_MEMORY)
@@ -202,6 +202,9 @@ function(px4_add_module)
 	set_target_properties(${MODULE} PROPERTIES STACK_MAX ${STACK_MAX})

 	if(${PX4_PLATFORM} STREQUAL "nuttx")
+		# double the allocated stacks for 64 bit nuttx targets
+		set(STACK_MAIN "${STACK_MAIN} * (__SIZEOF_POINTER__ >> 2)")
+
 		target_compile_options(${MODULE} PRIVATE -Wframe-larger-than=${STACK_MAX})
 	endif()

@@ -20,3 +20,4 @@ bool fused                   # true if the sample was successfully fused

 # TOPICS estimator_aid_source_1d
 # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed
+# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
@@ -20,3 +20,4 @@ bool[3] fused                # true if the sample was successfully fused

 # TOPICS estimator_aid_source_3d
 # TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
+# TOPICS estimator_aid_src_mag estimator_aid_src_aux_vel
@@ -65,9 +65,6 @@ bool reject_hor_vel                       #  0 - true if horizontal velocity obs
 bool reject_ver_vel                       #  1 - true if vertical velocity observations have been rejected
 bool reject_hor_pos                       #  2 - true if horizontal position observations have been rejected
 bool reject_ver_pos                       #  3 - true if vertical position observations have been rejected
-bool reject_mag_x                         #  4 - true if the X magnetometer observation has been rejected
-bool reject_mag_y                         #  5 - true if the Y magnetometer observation has been rejected
-bool reject_mag_z                         #  6 - true if the Z magnetometer observation has been rejected
 bool reject_yaw                           #  7 - true if the yaw observation has been rejected
 bool reject_airspeed                      #  8 - true if the airspeed observation has been rejected
 bool reject_sideslip                      #  9 - true if the synthetic sideslip observation has been rejected
@@ -354,7 +354,7 @@ def generate_agent(out_dir):
    # the '-typeros2' option in fastrtpsgen.
    # .. note:: This is only available in FastRTPSGen 1.0.4 and above
    gen_ros2_typename = ""
-    if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"):
+    if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'humble', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"):
        gen_ros2_typename = "-typeros2 "

    idl_files = []
@@ -38,7 +38,7 @@ if __name__ == "__main__":
        print("{:} -> {:}".format(msg_filename, output_file))

        #Format msg url
-        msg_url="[source file](https://github.com/PX4/PX4-Autopilot/blob/master/msg/%s)" % msg_file
+        msg_url="[source file](https://github.com/PX4/PX4-Autopilot/blob/main/msg/%s)" % msg_file

        msg_description = ""
        summary_description = ""
@@ -90,7 +90,7 @@ if __name__ == "__main__":
    readme_text="""# uORB Message Reference

 :::note
-This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/msg/tools/generate_msg_docs.py) from the source code.
+This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/msg/tools/generate_msg_docs.py) from the source code.
 :::

 This topic lists the UORB messages available in PX4 (some of which may be may be shared by the [PX4-ROS 2 Bridge](../ros/ros2_comm.md)).
@@ -62,7 +62,7 @@ uint8 DIST_BOTTOM_SENSOR_FLOW = 2	# (1 << 1) a flow sensor is used to estimate d
 float32 eph				# Standard deviation of horizontal position error, (metres)
 float32 epv				# Standard deviation of vertical position error, (metres)
 float32 evh				# Standard deviation of horizontal velocity error, (metres/sec)
-float32 evv				# Standard deviation of horizontal velocity error, (metres/sec)
+float32 evv				# Standard deviation of vertical velocity error, (metres/sec)

 bool dead_reckoning                     # True if this position is estimated through dead-reckoning

@@ -1,68 +1,31 @@
 # Vehicle odometry data. Fits ROS REP 147 for aerial vehicles
 uint64 timestamp		# time since system start (microseconds)
-
 uint64 timestamp_sample

-# Covariance matrix index constants
-uint8 COVARIANCE_MATRIX_X_VARIANCE=0
-uint8 COVARIANCE_MATRIX_Y_VARIANCE=6
-uint8 COVARIANCE_MATRIX_Z_VARIANCE=11
-uint8 COVARIANCE_MATRIX_ROLL_VARIANCE=15
-uint8 COVARIANCE_MATRIX_PITCH_VARIANCE=18
-uint8 COVARIANCE_MATRIX_YAW_VARIANCE=20
-uint8 COVARIANCE_MATRIX_VX_VARIANCE=0
-uint8 COVARIANCE_MATRIX_VY_VARIANCE=6
-uint8 COVARIANCE_MATRIX_VZ_VARIANCE=11
-uint8 COVARIANCE_MATRIX_ROLLRATE_VARIANCE=15
-uint8 COVARIANCE_MATRIX_PITCHRATE_VARIANCE=18
-uint8 COVARIANCE_MATRIX_YAWRATE_VARIANCE=20
+uint8 POSE_FRAME_UNKNOWN = 0
+uint8 POSE_FRAME_NED     = 1 # NED earth-fixed frame
+uint8 POSE_FRAME_FRD     = 2 # FRD world-fixed frame, arbitrary heading reference
+uint8 pose_frame            # Position and orientation frame of reference

-# Position and linear velocity frame of reference constants
-uint8 LOCAL_FRAME_NED=0         # NED earth-fixed frame
-uint8 LOCAL_FRAME_FRD=1         # FRD earth-fixed frame, arbitrary heading reference
-uint8 LOCAL_FRAME_OTHER=2       # Not aligned with the std frames of reference
-uint8 BODY_FRAME_FRD=3          # FRD body-fixed frame
+float32[3] position         # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown
+float32[4] q                # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown

-# Position and linear velocity local frame of reference
-uint8 local_frame
+uint8 VELOCITY_FRAME_UNKNOWN  = 0
+uint8 VELOCITY_FRAME_NED      = 1 # NED earth-fixed frame
+uint8 VELOCITY_FRAME_FRD      = 2 # FRD world-fixed frame, arbitrary heading reference
+uint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frame
+uint8 velocity_frame        # Reference frame of the velocity data

-# Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown
-float32 x			# North position
-float32 y			# East position
-float32 z			# Down position
+float32[3] velocity         # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown

-# Orientation quaternion. First value NaN if invalid/unknown
-float32[4] q			# Quaternion rotation from FRD body frame to reference frame
-float32[4] q_offset		# Quaternion rotation from odometry reference frame to navigation frame
+float32[3] angular_velocity # Angular velocity in body-fixed frame (rad/s). NaN if invalid/unknown

-# Row-major representation of 6x6 pose cross-covariance matrix URT.
-# NED earth-fixed frame.
-# Order: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
-# If position covariance invalid/unknown, first cell is NaN
-# If orientation covariance invalid/unknown, 16th cell is NaN
-float32[21] pose_covariance
-
-# Reference frame of the velocity data
-uint8 velocity_frame
-
-# Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
-float32 vx 			# North velocity
-float32 vy			# East velocity
-float32 vz			# Down velocity
-
-# Angular rate in body-fixed frame (rad/s). NaN if invalid/unknown
-float32 rollspeed		# Angular velocity about X body axis
-float32 pitchspeed		# Angular velocity about Y body axis
-float32 yawspeed		# Angular velocity about Z body axis
-
-# Row-major representation of 6x6 velocity cross-covariance matrix URT.
-# Linear velocity in NED earth-fixed frame. Angular velocity in body-fixed frame.
-# Order: vx, vy, vz, rotation rate about X axis, rotation rate about Y axis, rotation rate about Z axis
-# If linear velocity covariance invalid/unknown, first cell is NaN
-# If angular velocity covariance invalid/unknown, 16th cell is NaN
-float32[21] velocity_covariance
+float32[3] position_variance
+float32[3] orientation_variance
+float32[3] velocity_variance

 uint8 reset_counter
+int8 quality

 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
 # TOPICS estimator_odometry estimator_visual_odometry_aligned
@@ -33,7 +33,7 @@

 if("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU")
 	if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS_EQUAL 7)
-		message(FATAL_ERROR "GCC 7 or older no longer supported. https://docs.px4.io/master/en/dev_setup/dev_env.html")
+		message(FATAL_ERROR "GCC 7 or older no longer supported. https://docs.px4.io/main/en/dev_setup/dev_env.html")
 	endif()
 endif()

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(arch_watchdog_iwdg
+	iwdg.c
+)
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *       Author: David Sidrane <david.sidrane@nscdg.com>
+ *
+ * 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 <nuttx/config.h>
+#include "arm_internal.h"
+#include "arm_arch.h"
+#include "chip.h"
+
+#include "nvic.h"
+
+
+/****************************************************************************
+ * Name: watchdog_pet()
+ *
+ * Description:
+ *   This function resets the Independent watchdog (IWDG)
+ *
+ *
+ * Input Parameters:
+ *   none.
+ *
+ * Returned value:
+ *   none.
+ *
+ ****************************************************************************/
+
+void watchdog_pet(void)
+{
+}
+
+/****************************************************************************
+ * Name: watchdog_init()
+ *
+ * Description:
+ *   This function initialize the Independent watchdog (IWDG)
+ *
+ *
+ * Input Parameters:
+ *   none.
+ *
+ * Returned value:
+ *   none.
+ *
+ ****************************************************************************/
+
+void watchdog_init(void)
+{
+
+	watchdog_pet();
+}
@@ -103,7 +103,7 @@ Serial bus driver for the LeddarOne LiDAR.

 Most boards are configured to enable/start the driver on a specified UART using the SENS_LEDDAR1_CFG parameter.

-Setup/usage information: https://docs.px4.io/master/en/sensor/leddar_one.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/leddar_one.html

 ### Examples

@@ -175,21 +175,21 @@ int LightwareLaser::init()

 	case 4:
 		/* SF11/c (120m 20Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(120.0f);
 		_conversion_interval = 50000;
 		break;

 	case 5:
 		/* SF/LW20/b (50m 48-388Hz) */
-		_px4_rangefinder.set_min_distance(0.001f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(50.0f);
 		_conversion_interval = 20834;
 		break;

 	case 6:
 		/* SF/LW20/c (100m 48-388Hz) */
-		_px4_rangefinder.set_min_distance(0.001f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(100.0f);
 		_conversion_interval = 20834;
 		_type = Type::LW20c;
@@ -417,7 +417,7 @@ void LightwareLaser::print_usage()

 I2C bus driver for Lightware SFxx series LIDAR rangefinders: SF10/a, SF10/b, SF10/c, SF11/c, SF/LW20.

-Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/sfxx_lidar.html
 )DESCR_STR");

 	PRINT_MODULE_USAGE_NAME("lightware_laser_i2c", "driver");
@@ -106,7 +106,7 @@ LightwareLaserSerial::init()

 	case 5:
 		/* SF11/c (120m 20Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(120.0f);
 		_interval = 50000;
 		break;
@@ -104,7 +104,7 @@ Serial bus driver for the LightWare SF02/F, SF10/a, SF10/b, SF10/c, SF11/c Laser

 Most boards are configured to enable/start the driver on a specified UART using the SENS_SF0X_CFG parameter.

-Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/sfxx_lidar.html

 ### Examples

@@ -57,7 +57,7 @@ I2C bus driver for LidarLite rangefinders.

 The sensor/driver must be enabled using the parameter SENS_EN_LL40LS.

-Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/lidar_lite.html
 )DESCR_STR");

 	PRINT_MODULE_USAGE_NAME("ll40ls", "driver");
@@ -147,7 +147,7 @@ PWM driver for LidarLite rangefinders.

 The sensor/driver must be enabled using the parameter SENS_EN_LL40LS.

-Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/lidar_lite.html
 )DESCR_STR");

 	PRINT_MODULE_USAGE_NAME("ll40ls", "driver");
@@ -47,7 +47,7 @@ I2C bus driver for TeraRanger rangefinders.

 The sensor/driver must be enabled using the parameter SENS_EN_TRANGER.

-Setup/usage information: https://docs.px4.io/master/en/sensor/rangefinders.html#teraranger-rangefinders
+Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html#teraranger-rangefinders
 )DESCR_STR");
 	PRINT_MODULE_USAGE_NAME("teraranger", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
@@ -115,7 +115,7 @@ Serial bus driver for the Benewake TFmini LiDAR.

 Most boards are configured to enable/start the driver on a specified UART using the SENS_TFMINI_CFG parameter.

-Setup/usage information: https://docs.px4.io/master/en/sensor/tfmini.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/tfmini.html

 ### Examples

@@ -79,6 +79,7 @@
 #define DRV_IMU_DEVTYPE_ICM20948 0x28
 #define DRV_IMU_DEVTYPE_ICM42605 0x29
 #define DRV_IMU_DEVTYPE_ICM42670P 0x2A
+#define DRV_IMU_DEVTYPE_IIM42652 0x2B

 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32
@@ -62,6 +62,8 @@ ADIS16477::ADIS16477(const I2CSPIDriverConfig &config) :
 	_bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")),
 	_drdy_gpio(config.drdy_gpio)
 {
+	_debug_enabled = true;
+
 #ifdef GPIO_SPI1_RESET_ADIS16477
 	// Configure hardware reset line
 	px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16477);
@@ -35,6 +35,7 @@ px4_add_module(
 	MAIN adis16477
 	COMPILE_FLAGS
 		-Wno-cast-align # TODO: fix and enable
+		-DDEBUG_BUILD
 	SRCS
 		ADIS16477.cpp
 		adis16477_main.cpp
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__imu__invensense__iim42652
+	MAIN iim42652
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		iim42652_main.cpp
+		IIM42652.cpp
+		IIM42652.hpp
+		InvenSense_IIM42652_registers.hpp
+	DEPENDS
+		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
+	)
@@ -0,0 +1,800 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "IIM42652.hpp"
+
+using namespace time_literals;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+IIM42652::IIM42652(const I2CSPIDriverConfig &config) :
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio),
+	_px4_accel(get_device_id(), config.rotation),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (config.drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+}
+
+IIM42652::~IIM42652()
+{
+	perf_free(_bad_register_perf);
+	perf_free(_bad_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+	perf_free(_drdy_missed_perf);
+}
+
+int IIM42652::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool IIM42652::Reset()
+{
+	_state = STATE::RESET;
+	DataReadyInterruptDisable();
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void IIM42652::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void IIM42652::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+
+	perf_print_counter(_bad_register_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_missed_perf);
+}
+
+int IIM42652::probe()
+{
+	for (int i = 0; i < 3; i++) {
+		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+
+		if (whoami == WHOAMI) {
+			return PX4_OK;
+
+		} else {
+			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+
+			uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
+			int bank = reg_bank_sel >> 4;
+
+			if (bank >= 1 && bank <= 3) {
+				DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
+				// force bank selection and retry
+				SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
+			}
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+void IIM42652::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// DEVICE_CONFIG: Software reset configuration
+		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
+		_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::DEVICE_CONFIG) == 0x00)
+		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
+
+			// 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 start reading from FIFO
+			_state = STATE::FIFO_READ;
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(100_ms);
+
+			} else {
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();
+
+		} else {
+			// CONFIGURE not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Configure failed, resetting");
+				_state = STATE::RESET;
+
+			} else {
+				PX4_DEBUG("Configure failed, retrying");
+			}
+
+			ScheduleDelayed(100_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			if (samples == 0) {
+				// check current FIFO count
+				const uint16_t fifo_count = FIFOReadCount();
+
+				if (fifo_count >= FIFO::SIZE) {
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (fifo_count == 0) {
+					perf_count(_fifo_empty_perf);
+
+				} else {
+					// FIFO count (size in bytes)
+					samples = (fifo_count / sizeof(FIFO::DATA));
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
+					}
+
+					if (samples > FIFO_MAX_SAMPLES) {
+						// not technically an overflow, but more samples than we expected or can publish
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
+						samples = 0;
+					}
+				}
+			}
+
+			bool success = false;
+
+			if (samples >= 1) {
+				if (FIFORead(timestamp_sample, samples)) {
+					success = true;
+
+					if (_failure_count > 0) {
+						_failure_count--;
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+				    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+				   ) {
+					_last_config_check_timestamp = now;
+					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+					_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+					_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+			}
+		}
+
+		break;
+	}
+}
+
+void IIM42652::ConfigureSampleRate(int sample_rate)
+{
+	// round down to 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 IIM42652::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_CONFIG2) {
+			// FIFO_WM[7:0]  FIFO_CONFIG2
+			r.set_bits = fifo_watermark_threshold & 0xFF;
+
+		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
+			// FIFO_WM[11:8] FIFO_CONFIG3
+			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
+		}
+	}
+}
+
+void IIM42652::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
+{
+	if (bank != _last_register_bank || force) {
+		// select BANK_0
+		uint8_t cmd_bank_sel[2] {};
+		cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
+		cmd_bank_sel[1] = bank;
+		transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
+
+		_last_register_bank = bank;
+	}
+}
+
+bool IIM42652::Configure()
+{
+	// 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);
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	for (const auto &reg_cfg : _register_bank2_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;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank1_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	for (const auto &reg_cfg : _register_bank2_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	// 20-bits data format used
+	//  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
+	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+	_px4_gyro.set_range(math::radians(2000.f));
+
+	return success;
+}
+
+int IIM42652::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<IIM42652 *>(arg)->DataReady();
+	return 0;
+}
+
+void IIM42652::DataReady()
+{
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
+}
+
+bool IIM42652::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+}
+
+bool IIM42652::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+template <typename T>
+bool IIM42652::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_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+		success = false;
+	}
+
+	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+template <typename T>
+uint8_t IIM42652::RegisterRead(T reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+template <typename T>
+void IIM42652::RegisterWrite(T reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	SelectRegisterBank(reg);
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+template <typename T>
+void IIM42652::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 IIM42652::FIFOReadCount()
+{
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+}
+
+bool IIM42652::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+
+	if (fifo_count_bytes >= FIFO::SIZE) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+
+	if (fifo_count_samples == 0) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	// check FIFO header in every sample
+	uint8_t valid_samples = 0;
+
+	for (int i = 0; i < math::min(samples, fifo_count_samples); 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_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 IIM42652::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// SIGNAL_PATH_RESET: FIFO flush
+	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+
+	// reset while FIFO is disabled
+	_drdy_timestamp_sample.store(0);
+}
+
+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);
+}
+
+void IIM42652::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_accel_fifo_s accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = 0;
+	accel.dt = FIFO_SAMPLE_DT;
+
+	// 18-bits of accelerometer data
+	bool scale_20bit = false;
+
+	// first pass
+	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 ()
+		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+		// sample invalid if -524288
+		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+			// check if any values are going to exceed int16 limits
+			static constexpr int16_t max_accel = INT16_MAX;
+			static constexpr int16_t min_accel = INT16_MIN;
+
+			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;
+			}
+
+			// shift by 2 (2 least significant bits are always 0)
+			accel.x[accel.samples] = accel_x / 4;
+			accel.y[accel.samples] = accel_y / 4;
+			accel.z[accel.samples] = accel_z / 4;
+			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_X1, fifo[i].ACCEL_DATA_X0);
+			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
+			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
+
+			accel.x[i] = accel_x;
+			accel.y[i] = accel_y;
+			accel.z[i] = accel_z;
+		}
+
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+	}
+
+	// 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 IIM42652::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;
+	gyro.dt = FIFO_SAMPLE_DT;
+
+	// 20-bits of gyroscope data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+		// 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_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
+		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
+		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
+
+		// check if any values are going to exceed int16 limits
+		static constexpr int16_t max_gyro = INT16_MAX;
+		static constexpr int16_t min_gyro = INT16_MIN;
+
+		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 / 2;
+		gyro.y[gyro.samples] = gyro_y / 2;
+		gyro.z[gyro.samples] = gyro_z / 2;
+		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_X1, fifo[i].GYRO_DATA_X0);
+			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
+		}
+
+		_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+	}
+
+	// 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 IIM42652::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++) {
+		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
+
+		// 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_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+		if (PX4_ISFINITE(TEMP_degC)) {
+			_px4_accel.set_temperature(TEMP_degC);
+			_px4_gyro.set_temperature(TEMP_degC);
+			return true;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+		}
+	}
+
+	return false;
+}
@@ -0,0 +1,206 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file IIM42652.hpp
+ *
+ * Driver for the Invensense IIM42652 connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "InvenSense_IIM42652_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_IIM42652;
+
+class IIM42652 : public device::SPI, public I2CSPIDriver<IIM42652>
+{
+public:
+	IIM42652(const I2CSPIDriverConfig &config);
+	~IIM42652() 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};
+
+	// 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::INT_STATUS) | DIR_READ};
+		uint8_t INT_STATUS{0};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + 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};
+	};
+
+	struct register_bank1_config_t {
+		Register::BANK_1 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	struct register_bank2_config_t {
+		Register::BANK_2 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 SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
+	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
+	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
+	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	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, uint8_t samples);
+	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);
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
+	perf_counter_t _drdy_missed_perf{nullptr};
+
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+	int _failure_count{0};
+
+	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
+
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+	} _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+
+	uint8_t _checked_register_bank0{0};
+	static constexpr uint8_t size_register_bank0_cfg{13};
+	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 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_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
+		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
+		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+	};
+
+	uint8_t _checked_register_bank1{0};
+	static constexpr uint8_t size_register_bank1_cfg{1};
+	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS, 0 },
+	};
+
+	uint8_t _checked_register_bank2{0};
+	static constexpr uint8_t size_register_bank2_cfg{1};
+	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS, 0 },
+	};
+};
@@ -0,0 +1,322 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file InvenSense_IIM42652_registers.hpp
+ *
+ * Invensense IIM-42652 registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+namespace InvenSense_IIM42652
+{
+// 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 = 0x6F;
+
+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 {
+	DEVICE_CONFIG      = 0x11,
+
+	INT_CONFIG         = 0x14,
+
+	FIFO_CONFIG        = 0x16,
+
+	TEMP_DATA1         = 0x1D,
+	TEMP_DATA0         = 0x1E,
+
+	INT_STATUS         = 0x2D,
+	FIFO_COUNTH        = 0x2E,
+	FIFO_COUNTL        = 0x2F,
+	FIFO_DATA          = 0x30,
+
+	SIGNAL_PATH_RESET  = 0x4B,
+	INTF_CONFIG0       = 0x4C,
+	INTF_CONFIG1       = 0x4D,
+	PWR_MGMT0          = 0x4E,
+	GYRO_CONFIG0       = 0x4F,
+	ACCEL_CONFIG0      = 0x50,
+	GYRO_CONFIG1       = 0x51,
+	GYRO_ACCEL_CONFIG0 = 0x52,
+	ACCEL_CONFIG1      = 0x53,
+
+	FIFO_CONFIG1       = 0x5F,
+	FIFO_CONFIG2       = 0x60,
+	FIFO_CONFIG3       = 0x61,
+
+	INT_CONFIG0        = 0x63,
+
+	INT_SOURCE0        = 0x65,
+
+	SELF_TEST_CONFIG   = 0x70,
+
+	WHO_AM_I           = 0x75,
+	REG_BANK_SEL       = 0x76,
+};
+
+enum class BANK_1 : uint8_t {
+	GYRO_CONFIG_STATIC2 = 0x0B,
+
+	INTF_CONFIG5        = 0x7B,
+};
+
+enum class BANK_2 : uint8_t {
+	ACCEL_CONFIG_STATIC2 = 0x03,
+};
+
+};
+
+//---------------- BANK0 Register bits
+
+// DEVICE_CONFIG
+enum DEVICE_CONFIG_BIT : uint8_t {
+	SOFT_RESET_CONFIG = Bit0, //
+};
+
+// INT_CONFIG
+enum INT_CONFIG_BIT : uint8_t {
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
+};
+
+// FIFO_CONFIG
+enum FIFO_CONFIG_BIT : uint8_t {
+	// 7:6 FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
+};
+
+// INT_STATUS
+enum INT_STATUS_BIT : uint8_t {
+	RESET_DONE_INT = Bit4,
+	DATA_RDY_INT   = Bit3,
+	FIFO_THS_INT   = Bit2,
+	FIFO_FULL_INT  = Bit1,
+};
+
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	ABORT_AND_RESET = Bit3,
+	FIFO_FLUSH      = Bit1,
+};
+
+// 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
+};
+
+// GYRO_CONFIG0
+enum GYRO_CONFIG0_BIT : uint8_t {
+	// 7:5 GYRO_FS_SEL
+	GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
+	GYRO_FS_SEL_1000_DPS = Bit5,
+	GYRO_FS_SEL_500_DPS  = Bit6,
+	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
+	GYRO_FS_SEL_125_DPS  = Bit7,
+
+
+	// 3:0 GYRO_ODR
+	//  0001: 32kHz
+	GYRO_ODR_32KHZ_SET   = Bit0,
+	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	GYRO_ODR_16KHZ_SET   = Bit1,
+	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
+	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
+	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:5 ACCEL_FS_SEL
+	ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
+	ACCEL_FS_SEL_8G  = Bit5,
+	ACCEL_FS_SEL_4G  = Bit6,
+	ACCEL_FS_SEL_2G  = Bit6 | Bit5,
+
+
+	// 3:0 ACCEL_ODR
+	//  0001: 32kHz
+	ACCEL_ODR_32KHZ_SET   = Bit0,
+	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	ACCEL_ODR_16KHZ_SET   = Bit1,
+	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
+	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
+	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+};
+
+// GYRO_CONFIG1
+enum GYRO_CONFIG1_BIT : uint8_t {
+	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
+};
+
+// GYRO_ACCEL_CONFIG0
+enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:4 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
+
+	// 3:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
+};
+
+// ACCEL_CONFIG1
+enum ACCEL_CONFIG1_BIT : uint8_t {
+	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+};
+
+// FIFO_CONFIG1
+enum FIFO_CONFIG1_BIT : uint8_t {
+	FIFO_RESUME_PARTIAL_RD = Bit6,
+	FIFO_WM_GT_TH          = Bit5,
+	FIFO_HIRES_EN          = Bit4,
+	FIFO_TEMP_EN           = Bit2,
+	FIFO_GYRO_EN           = Bit1,
+	FIFO_ACCEL_EN          = Bit0,
+};
+
+// INT_CONFIG0
+enum INT_CONFIG0_BIT : uint8_t {
+	// 3:2 FIFO_THS_INT_CLEAR
+	CLEAR_ON_FIFO_READ = Bit3,
+};
+
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	UI_FSYNC_INT1_EN   = Bit6,
+	PLL_RDY_INT1_EN    = Bit5,
+	RESET_DONE_INT1_EN = Bit4,
+	UI_DRDY_INT1_EN    = Bit3,
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+	FIFO_FULL_INT1_EN  = Bit1,
+	UI_AGC_RDY_INT1_EN = Bit0,
+};
+
+// REG_BANK_SEL
+enum REG_BANK_SEL_BIT : uint8_t {
+	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
+	USER_BANK_1 = Bit4,        // 1: Select USER BANK 1.
+	USER_BANK_2 = Bit5,        // 2: Select USER BANK 2.
+	USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
+};
+
+
+//---------------- BANK1 Register bits
+
+// GYRO_CONFIG_STATIC2
+enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
+	GYRO_AAF_DIS = Bit1,
+	GYRO_NF_DIS  = Bit0,
+};
+
+
+//---------------- BANK2 Register bits
+
+// ACCEL_CONFIG_STATIC2
+enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
+	ACCEL_AAF_DIS = Bit0,
+};
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 2048;
+
+// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
+
+// Packet 4
+struct DATA {
+	uint8_t FIFO_Header;
+	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
+	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
+	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
+	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
+	uint8_t TEMP_DATA1;    // Temperature[15:8]
+	uint8_t TEMP_DATA0;    // Temperature[7:0]
+	uint8_t TimeStamp_h;   // TimeStamp[15:8]
+	uint8_t TimeStamp_l;   // TimeStamp[7:0]
+	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
+	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
+	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [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,
+	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_IIM42652
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_INVENSENSE_IIM42652
+	bool "iim42652"
+	default n
+	---help---
+		Enable support for iim42652
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -31,56 +31,57 @@
 *
 ****************************************************************************/

-#ifndef ATT_POS_MOCAP_HPP
-#define ATT_POS_MOCAP_HPP
+#include "IIM42652.hpp"

-#include <uORB/topics/vehicle_odometry.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>

-class MavlinkStreamAttPosMocap : public MavlinkStream
+void IIM42652::print_usage()
 {
-public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttPosMocap(mavlink); }
+	PRINT_MODULE_USAGE_NAME("iim42652", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}

-	static constexpr const char *get_name_static() { return "ATT_POS_MOCAP"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATT_POS_MOCAP; }
+extern "C" int iim42652_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = IIM42652;
+	BusCLIArguments cli{false, true};
+	cli.default_spi_frequency = SPI_SPEED;

-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
-
-	unsigned get_size() override
-	{
-		return _mocap_sub.advertised() ? MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-	}
-
-private:
-	explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink) {}
-
-	uORB::Subscription _mocap_sub{ORB_ID(vehicle_mocap_odometry)};
-
-	bool send() override
-	{
-		vehicle_odometry_s mocap;
-
-		if (_mocap_sub.update(&mocap)) {
-			mavlink_att_pos_mocap_t msg{};
-
-			msg.time_usec = mocap.timestamp_sample;
-			msg.q[0] = mocap.q[0];
-			msg.q[1] = mocap.q[1];
-			msg.q[2] = mocap.q[2];
-			msg.q[3] = mocap.q[3];
-			msg.x = mocap.x;
-			msg.y = mocap.y;
-			msg.z = mocap.z;
-			// msg.covariance =
-
-			mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg);
-
-			return true;
+	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+		switch (ch) {
+		case 'R':
+			cli.rotation = (enum Rotation)atoi(cli.optArg());
+			break;
 		}
-
-		return false;
 	}
-};

-#endif // ATT_POS_MOCAP_HPP
+	const char *verb = cli.optArg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_IIM42652);
+
+	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;
+}
@@ -54,7 +54,7 @@ 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 I2C_ADDRESS_DEFAULT = 0x69; // 0b110100X
+static constexpr uint8_t I2C_ADDRESSES[] {0b110'1000, 0b110'1001}; // 0b110'100x (0x68 or 0x69)
 static constexpr uint32_t I2C_SPEED = 400 * 1000;

 static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000;
@@ -42,7 +42,7 @@ void MPU9250_I2C::print_usage()
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x68);
 	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
@@ -53,7 +53,7 @@ extern "C" int mpu9250_i2c_main(int argc, char *argv[])
 	using ThisDriver = MPU9250_I2C;
 	BusCLIArguments cli{true, false};
 	cli.default_i2c_frequency = I2C_SPEED;
-	cli.i2c_address = I2C_ADDRESS_DEFAULT;
+	cli.i2c_address = 0;

 	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
 		switch (ch) {
@@ -73,7 +73,21 @@ extern "C" int mpu9250_i2c_main(int argc, char *argv[])
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_MPU9250);

 	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
+		if (cli.i2c_address != 0) {
+			return ThisDriver::module_start(cli, iterator);
+
+		} else {
+			// otherwise try all I2C addresses
+			for (auto &i2c_address : I2C_ADDRESSES) {
+				cli.i2c_address = i2c_address;
+
+				if (ThisDriver::module_start(cli, iterator) == PX4_OK) {
+					return PX4_OK;
+				}
+			}
+
+			return PX4_ERROR;
+		}
 	}

 	if (!strcmp(verb, "stop")) {
@@ -428,7 +428,7 @@ Serial bus driver for the ThoneFlow-3901U optical flow sensor.

 Most boards are configured to enable/start the driver on a specified UART using the SENS_TFLOW_CFG parameter.

-Setup/usage information: https://docs.px4.io/master/en/sensor/pmw3901.html#thone-thoneflow-3901u
+Setup/usage information: https://docs.px4.io/main/en/sensor/pmw3901.html#thone-thoneflow-3901u

 ### Examples

@@ -494,8 +494,57 @@ void PWMOut::Run()

 void PWMOut::update_params()
 {
+	uint32_t previously_set_functions = 0;
+
+	for (size_t i = 0; i < _num_outputs; i++) {
+		previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i;
+	}
+
 	updateParams();

+	// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
+	if (_mixing_output.useDynamicMixing() && !_first_param_update) {
+		for (size_t i = 0; i < _num_outputs; i++) {
+			if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
+				int32_t output_function;
+
+				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
+				    && output_function >= (int)OutputFunction::Servo1
+				    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+					int32_t val = 1500;
+					PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
+					param_set(_mixing_output.disarmedParamHandle(i), &val);
+
+					// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+					for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+
+						uint32_t channels = io_timer_get_group(timer);
+
+						if ((channels & (1u << i)) == 0) {
+							continue;
+						}
+
+						if ((channels & previously_set_functions) == 0) { // None of the channels was set
+							char param_name[17];
+							snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+
+							int32_t tim_config = 0;
+							param_t handle = param_find(param_name);
+
+							if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+								tim_config = 50;
+								PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+								param_set(handle, &tim_config);
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+
+	_first_param_update = false;
+
 	if (_mixing_output.useDynamicMixing()) {
 		return;
 	}
@@ -147,6 +147,7 @@ private:
 	bool		_pwm_on{false};
 	uint32_t	_pwm_mask{0};
 	bool		_pwm_initialized{false};
+	bool		_first_param_update{true};

 	unsigned	_num_disarmed_set{0};

@@ -179,6 +179,8 @@ private:
 	unsigned		_max_transfer{16};		///< Maximum number of I2C transfers supported by PX4IO

 	int			_class_instance{-1};
+	bool			_first_param_update{true};
+	uint32_t    		_group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {};

 	hrt_abstime		_poll_last{0};

@@ -477,6 +479,12 @@ int PX4IO::init()
 		return ret;
 	}

+	/* initialize _group_channels */
+	for (uint8_t group = PX4IO_P_SETUP_PWM_RATE_GROUP0; group <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++group) {
+		unsigned group_idx = group - PX4IO_P_SETUP_PWM_RATE_GROUP0;
+		_group_channels[group_idx] = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + group_idx);
+	}
+
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
 		_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
@@ -605,8 +613,6 @@ void PX4IO::Run()

 			_param_update_force = false;

-			ModuleParams::updateParams();
-
 			update_params();

 			/* Check if the flight termination circuit breaker has been updated */
@@ -699,14 +705,67 @@ void PX4IO::updateTimerRateGroups()

 void PX4IO::update_params()
 {
+	uint32_t previously_set_functions = 0;
+
+	for (size_t i = 0; i < _max_actuators; i++) {
+		previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i;
+	}
+
+	updateParams();
+
 	if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) {
+
+		// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
+		if (!_first_param_update) {
+			for (size_t i = 0; i < _max_actuators; i++) {
+				if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
+					int32_t output_function;
+
+					if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
+					    && output_function >= (int)OutputFunction::Servo1
+					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+						int32_t val = 1500;
+						PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
+						param_set(_mixing_output.disarmedParamHandle(i), &val);
+
+						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+						for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
+
+							uint32_t channels = _group_channels[timer];
+
+							if ((channels & (1u << i)) == 0) {
+								continue;
+							}
+
+							if ((channels & previously_set_functions) == 0) { // None of the channels was set
+								char param_name[17];
+								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+
+								int32_t tim_config = 0;
+								param_t handle = param_find(param_name);
+
+								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+									tim_config = 50;
+									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+									param_set(handle, &tim_config);
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+
 		// sync params to IO
 		updateTimerRateGroups();
 		updateFailsafe();
 		updateDisarmed();
+		_first_param_update = false;
 		return;
 	}

+	_first_param_update = false;
+
 	// skip update when armed or PWM disabled
 	if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) {
 		return;
@@ -61,6 +61,14 @@ bool GHSTTelemetry::update(const hrt_abstime &now)
 			success = send_battery_status();
 			break;

+		case 1U:
+			success = send_gps1_status();
+			break;
+
+		case 2U:
+			success = send_gps2_status();
+			break;
+
 		default:
 			success = false;
 			break;
@@ -93,3 +101,39 @@ bool GHSTTelemetry::send_battery_status()

 	return success;
 }
+
+bool GHSTTelemetry::send_gps1_status()
+{
+	sensor_gps_s vehicle_gps_position;
+
+	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+		return false;
+	}
+
+	int32_t latitude = vehicle_gps_position.lat;				// 1e-7 degrees
+	int32_t longitude = vehicle_gps_position.lon;				// 1e-7 degrees
+	uint16_t altitude = vehicle_gps_position.alt / 1000;			// mm -> m
+
+	return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
+}
+
+bool GHSTTelemetry::send_gps2_status()
+{
+	sensor_gps_s vehicle_gps_position;
+
+	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+		return false;
+	}
+
+	uint16_t ground_speed = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f);
+	uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f);
+	uint8_t num_sats = vehicle_gps_position.satellites_used;
+
+	// TBD: Can these be computed in a RC telemetry driver?
+	uint16_t home_dist = 0;
+	uint16_t home_dir = 0;
+	uint8_t flags = 0;
+
+	return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags);
+}
+
@@ -44,6 +44,7 @@

 #include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_gps.h>
 #include <drivers/drv_hrt.h>

 /**
@@ -69,14 +70,17 @@ public:

 private:
 	bool send_battery_status();
+	bool send_gps1_status();
+	bool send_gps2_status();

+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};

 	int _uart_fd;
 	hrt_abstime _last_update {0U};
 	uint32_t _next_type {0U};

-	static constexpr uint32_t NUM_DATA_TYPES {1U};	// number of different telemetry data types
+	static constexpr uint32_t NUM_DATA_TYPES {3U};	// number of different telemetry data types
 	static constexpr uint32_t UPDATE_RATE_HZ {10U};	// update rate [Hz]

 	// Factors that should be applied to get correct values
@@ -45,4 +45,5 @@ px4_add_unit_gtest(SRC math/test/MedianFilterTest.cpp)
 px4_add_unit_gtest(SRC math/test/NotchFilterTest.cpp)
 px4_add_unit_gtest(SRC math/test/second_order_reference_model_test.cpp)
 px4_add_unit_gtest(SRC math/FunctionsTest.cpp)
+px4_add_unit_gtest(SRC math/test/UtilitiesTest.cpp)
 px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp)
@@ -31,9 +31,27 @@
 *
 ****************************************************************************/

-#include "utils.hpp"
+#ifndef MATH_UTILITIES_H
+#define MATH_UTILITIES_H

-matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
+#include <matrix/math.hpp>
+
+namespace math
+{
+
+namespace Utilities
+{
+
+// return the square of two floating point numbers - used in auto coded sections
+static constexpr float sq(float var) { return var * var; }
+
+// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
+// to the corresponding rotation matrix that rotates from frame 2 to frame 1
+// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
+// rot312(1) - Second rotation is a RH rotation about the X axis (rad)
+// rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
+// See http://www.atacolorado.com/eulersequences.doc
+inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
 {
 	// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence
 	const float c2 = cosf(rot312(2)); // third rotation is pitch
@@ -57,15 +75,7 @@ matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
 	return R;
 }

-float kahanSummation(float sum_previous, float input, float &accumulator)
-{
-	const float y = input - accumulator;
-	const float t = sum_previous + y;
-	accumulator = (t - sum_previous) - y;
-	return t;
-}
-
-matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
+inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
 {
 	const float q00 = quat(0) * quat(0);
 	const float q11 = quat(1) * quat(1);
@@ -92,7 +102,25 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
 	return dcm;
 }

-float getEuler321Yaw(const matrix::Quatf &q)
+// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
+// when there is more roll than pitch tilt and a 3-1-2 rotation sequence
+// when there is more pitch than roll tilt to avoid gimbal lock.
+inline bool shouldUse321RotationSequence(const matrix::Dcmf &R)
+{
+	return fabsf(R(2, 0)) < fabsf(R(2, 1));
+}
+
+inline float getEuler321Yaw(const matrix::Dcmf &R)
+{
+	return atan2f(R(1, 0), R(0, 0));
+}
+
+inline float getEuler312Yaw(const matrix::Dcmf &R)
+{
+	return atan2f(-R(0, 1), R(1, 1));
+}
+
+inline float getEuler321Yaw(const matrix::Quatf &q)
 {
 	// Values from yaw_input_321.c file produced by
 	// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
@@ -101,7 +129,7 @@ float getEuler321Yaw(const matrix::Quatf &q)
 	return atan2f(a, b);
 }

-float getEuler312Yaw(const matrix::Quatf &q)
+inline float getEuler312Yaw(const matrix::Quatf &q)
 {
 	// Values from yaw_input_312.c file produced by
 	// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
@@ -110,14 +138,24 @@ float getEuler312Yaw(const matrix::Quatf &q)
 	return atan2f(a, b);
 }

-matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
+inline float getEulerYaw(const matrix::Dcmf &R)
+{
+	if (shouldUse321RotationSequence(R)) {
+		return getEuler321Yaw(R);
+
+	} else {
+		return getEuler312Yaw(R);
+	}
+}
+
+inline matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 {
 	matrix::Eulerf euler321(rot_in);
 	euler321(2) = yaw;
 	return matrix::Dcmf(euler321);
 }

-matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
+inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 {
 	const matrix::Vector3f rotVec312(yaw,  // yaw
 					 asinf(rot_in(2, 1)),  // roll
@@ -125,9 +163,18 @@ matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 	return taitBryan312ToRotMat(rotVec312);
 }

-matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in)
+// Checks which euler rotation sequence to use and update yaw in rotation matrix
+inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 {
-	return shouldUse321RotationSequence(rot_in) ?
-	       updateEuler321YawInRotMat(yaw, rot_in) :
-	       updateEuler312YawInRotMat(yaw, rot_in);
+	if (shouldUse321RotationSequence(rot_in)) {
+		return updateEuler321YawInRotMat(yaw, rot_in);
+
+	} else {
+		return updateEuler312YawInRotMat(yaw, rot_in);
+	}
 }
+
+} // namespace Utilities
+} // namespace math
+
+#endif // MATH_UTILITIES_H
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL 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 test_EKF_utils.cpp
+ *
+ * @brief Unit tests for the miscellaneous EKF utilities
+ */
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <vector>
+#include <mathlib/mathlib.h>
+
+using namespace math::Utilities;
+
+TEST(euler312YawTest, fromQuaternion)
+{
+	matrix::Quatf q1(3.5f, 2.4f, -0.5f, -3.f);
+	q1.normalize();
+	const matrix::Eulerf euler1(q1);
+	EXPECT_FLOAT_EQ(euler1(2), getEuler321Yaw(q1));
+
+	matrix::Quatf q2(0.f, 0, -1.f, 0.f);
+	q2.normalize();
+	const matrix::Eulerf euler2(q2);
+	EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2));
+}
+
+TEST(shouldUse321RotationSequenceTest, pitch90)
+{
+	matrix::Eulerf euler(0.f, math::radians(90), 0.f);
+	matrix::Dcmf R(euler);
+	EXPECT_FALSE(shouldUse321RotationSequence(R));
+}
+
+TEST(shouldUse321RotationSequenceTest, roll90)
+{
+	matrix::Eulerf euler(math::radians(90.f), 0.f, 0.f);
+	matrix::Dcmf R(euler);
+	EXPECT_TRUE(shouldUse321RotationSequence(R));
+}
+
+TEST(shouldUse321RotationSequenceTest, moreRollThanPitch)
+{
+	matrix::Eulerf euler(math::radians(45.f), math::radians(30.f), 0.f);
+	matrix::Dcmf R(euler);
+	EXPECT_TRUE(shouldUse321RotationSequence(R));
+}
+
+TEST(shouldUse321RotationSequenceTest, morePitchThanRoll)
+{
+	matrix::Eulerf euler(math::radians(30.f), math::radians(45.f), 0.f);
+	matrix::Dcmf R(euler);
+	EXPECT_FALSE(shouldUse321RotationSequence(R));
+}
@@ -45,5 +45,6 @@
 #include "math/Functions.hpp"
 #include "math/SearchMin.hpp"
 #include "math/TrajMath.hpp"
+#include "math/Utilities.hpp"

 #endif
@@ -193,6 +193,9 @@ public:
 	uint16_t &minValue(int index) { return _min_value[index]; }
 	uint16_t &maxValue(int index) { return _max_value[index]; }

+	param_t functionParamHandle(int index) const { return _param_handles[index].function; }
+	param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; }
+
 	/**
 	 * Returns the actual failsafe value taking into account the assigned function
 	 */
@@ -350,6 +350,19 @@ static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
 	offset += 2;
 }

+/**
+ * write an uint32_t value to a buffer at a given offset and increment the offset
+ */
+static inline void write_uint32_t(uint8_t *buf, int &offset, uint32_t value)
+{
+	// Little Endian
+	buf[offset] = value & 0xFFU;
+	buf[offset + 1] = (value & 0xFF00) >> 8U;
+	buf[offset + 2] = (value & 0xFF0000) >> 16U;
+	buf[offset + 3] = (value & 0xFF000000) >> 24U;
+	offset += 4;
+}
+
 /**
 * write frame header
 */
@@ -385,3 +398,34 @@ bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,

 	return write(uart_fd, buf, offset) == offset;
 }
+
+bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude)
+{
+	uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
+	int offset = 0;
+	write_frame_header(buf, offset, ghstTelemetryType::gpsPrimary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
+	write_uint32_t(buf, offset, latitude);
+	write_uint32_t(buf, offset, longitude);
+	write_uint16_t(buf, offset, altitude);
+	write_frame_crc(buf, offset, sizeof(buf));
+
+	return write(uart_fd, buf, offset) == offset;
+}
+
+bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course, uint8_t numSats,
+				     uint16_t home_dist, uint16_t home_dir, uint8_t flags)
+{
+	uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
+	int offset = 0;
+	write_frame_header(buf, offset, ghstTelemetryType::gpsSecondary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
+	write_uint16_t(buf, offset, ground_speed);
+	write_uint16_t(buf, offset, ground_course);
+	write_uint8_t(buf, offset, numSats);
+	write_uint16_t(buf, offset, home_dist);
+	write_uint16_t(buf, offset, home_dir);
+	write_uint8_t(buf, offset, flags);
+	write_frame_crc(buf, offset, sizeof(buf));
+
+	return write(uart_fd, buf, offset) == offset;
+}
+
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	fd014acd47	WIP: adis16477 debug	2022-08-09 11:13:17 -04:00
Daniel Agar	e992757fb3	mpu9250: try all I2C addresses if not manually specified	2022-08-09 10:05:33 -04:00
Jukka Laitinen	c7aaf52fd4	Double the allocated stack size of 64-bit NuttX built-in modules Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-08-09 08:08:54 +02:00
Daniel Agar	7c809f034d	replay: ReplayEkf2 disable parameter auto save - not needed and silences a startup error	2022-08-08 21:27:47 -04:00
Daniel Agar	34dee09b74	ekf2: replay fixes, don't use HRT for timeout checks - this interferes with current ekf2 replay where the latest IMU sample is effectively the current timestamp	2022-08-08 21:27:47 -04:00
Daniel Agar	1c49a4349f	ekf2: force skip multi-EKF config if replay is enabled	2022-08-08 21:27:47 -04:00
Daniel Agar	66b55d9d0a	ekf2: fix yaw estimator velocity accuracy - additionally require GPS speed accuracy is within EKF2_REQ_SACC	2022-08-08 21:27:01 -04:00
bresch	d9d127a237	lightware dist sensor: set min range based on datasheet	2022-08-08 19:32:44 -04:00
Daniel Agar	0bce1ef573	drivers/imu: new TDK IIM-42652 IMU support	2022-08-08 13:51:39 -04:00
Thomas Stastny	0ea347a5c9	fw pos ctrl: fix the touchdown offset on flare, and nudge the wheel directly	2022-08-08 09:32:44 +02:00
Thomas Stastny	02d7a46025	fw pos ctrl: increase landing nudge rate	2022-08-08 09:32:44 +02:00
Beat Küng	3e68870547	gtest: update to version 1.12.1 Fixes the error googletest-src/googletest/src/gtest-death-test.cc:1283:24: error: ‘dummy’ may be used uninitialized [-Werror=maybe-uninitialized] with GCC 11	2022-08-08 07:43:42 +02:00
Daniel Agar	bce4237963	move ekf2 Matrix helper utilities to mathlib	2022-08-05 09:58:07 -04:00
Silvan Fuhrer	6ebc88fed7	ROMFS: vtol_defaults: reduce aggressiveness around roll and yaw axis For most VTOLs the param defaults for the agressiveness of the MC attitude controller are too high, as VTOLs usually have high intertia and lot af drag due to wings and can thus not rotate as fast as MCs. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	a064164c14	FW pos C params: add param group FW Auto Landing Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	b039ae1614	FW pos c params: change grouping of some clearly longitudinal params to TECS Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	45073f000a	FW Position control: reduce defaults for max pitch Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	d04f21aa16	FW attitude controller: reduce FW_MAN_P_MAX from 45 to 30 45° is a very large pitch angle, and for me 30° is much more reasonable for a default value. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	828992adf7	increase default of MPC_Z_VEL_MAX_DN and MPC_Z_V_AUTO_DN from 1 to 1.5 I think most vehicle can safely decend with at least 1.5m/s, and having this value too low makes Descents/Landings/RTLs unnecessary long. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	c42667ac64	ROMFS: vtol_defaults: remvoe custom NAV_ACC_RAD, leave at param default (10) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	4614c1a0b4	ROMFS: vtol_defaults: increase default hover speeds Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Silvan Fuhrer	0d10491b89	ROMFS: vtol_defaults: remove MPC_TKO_SPEED from VTOL defaults The VTOL default was set to 1, while the param default is 1.5. I don't see why it shuold be a different default for VTOLs and thus remove it. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-05 14:11:59 +02:00
Matthias Grob	87cbda1992	FlightTaskOrbit: parameterize hardcoded maximum radius (#20012 )	2022-08-05 09:19:32 +02:00
Daniel Agar	dfdfbbfa9c	msg/vehicle_odometry.msg: simplify covariance handling and update all usage (#19966 ) - replace float32[21] URT covariances with smaller dedicated position/velocity/orientation variances (the crossterms are unused, awkward, and relatively costly) - these are easier to casually inspect and more representative of what's actually being used currently and reduces the size of vehicle_odometry_s quite a bit - ekf2: add new helper to get roll/pitch/yaw covariances - mavlink: receiver ODOMETRY handle more frame types for both pose (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU) and velocity (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU, MAV_FRAME_BODY_FRD) - mavlink: delete unused ATT_POS_MOCAP stream (this is just a passthrough) Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>	2022-08-04 12:55:21 -04:00
bresch	61f390b0dd	ekf2_test: fix height offset compensation after origin reset	2022-08-04 16:50:31 +02:00
bresch	e34de53e2e	ekf2_test: let the GPS start before setting the new origin fix test by reducing the distance to the new origin: the maximum size of the local position origin is a cube of 1e6m. If the origin is moved further than this, the state is clipped to that maximum value	2022-08-04 16:50:31 +02:00
Silvan Fuhrer	55f395a7e9	FlightTaskAuto: apply cruise speed from position triplet also when negative (#20006 ) Navigator sets the cruise_speed to -1 if the controller shouldn't listen to it and instead use the default speed (for MC: MPC_XY_CRUISE). This is for example for RTL the case, where we want to return at the default speed, independetly of what the mission speed before was. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-08-04 13:36:15 +02:00
FARHANG	2498cbbb74	boards: px4_fmu-v6c rc.board_defaults remove irrelevant ethernet configuration	2022-08-03 11:13:12 -04:00
Tony Cake	f321117568	GHST: Add support for GPS Telemetry (#19953 ) Add support for the basic GPS telemetry values when using the GHST protocol. * Fix formatting in GHST GPS telemetry changes * GHST GPS Telemetry formatting cleanup * GHST GPS Telemetry, Last formatting change	2022-08-03 10:44:21 +02:00
Hamish Willee	270c456121	CI - build on main as well (#20001 )	2022-08-02 16:52:17 -07:00
Roman Bapst	dbf7d32e07	Skip VTOL_TAKEOFF mission item when in fixed wing mode (#19985 ) * mission: skip VTOL_TAKEOFF mission item when in fixed wing mode Signed-off-by: RomanBapst <bapstroman@gmail.com> * mission: added better comment regarding skipping VTOL Takeoff in fw mode Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-08-02 15:34:42 +02:00
RomanBapst	f11f2e9797	addressed review comments Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-08-02 15:29:25 +02:00
RomanBapst	a425bc4c92	vehicle_local_position: fixed comment Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-08-02 15:29:25 +02:00
RomanBapst	fbd4534edc	WindEstimator: reworked filter initialisation - separate initialisation with and without airspeed Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-08-02 15:29:25 +02:00
RomanBapst	a63f1b71fe	wind_estimator: added simple check for validity of synthetic airspeed - synthetic airspeed will only be declared valid as soon as the wind variance has dropped below a parameterized threshold. This is useful for vehicles without an airspeed sensor which rely on synthetic airspeed but only once the vehicle has turned sufficiently for the wind estimates to be reliable. Signed-off-by: RomanBapst <bapstroman@gmail.com>	2022-08-02 15:29:25 +02:00
Hamish Willee	a715b5468e	mc_wind_estimator - improve readability (#19545 )	2022-08-02 09:26:06 +02:00
Hamish Willee	30e2490d5b	Docs are now in user guide and main (#19977 ) * Fix links to docs in source to point to docs on main not master * More docs and scripts that need to point to main	2022-08-01 11:39:39 +10:00
Peter van der Perk	c566fb414b	S32K1XX add dummy iwdg driver	2022-07-31 11:21:41 -04:00
Beat Küng	e7588d2da0	px4io+pwm_out: set the PWM rate and disarmed value when a channel is first set to a servo This should simplify the first setup a bit.	2022-07-31 11:20:57 -04:00
Igor Mišić	f929017618	boards: link missing arch_io_pins lib	2022-07-31 11:19:20 -04:00
Daniel Agar	41d9c3dd2a	ekf2: add AUX velocity aid src status - also includes velocity and position helpers for using estimator aid source status messages that will later be used for GPS, EV, etc	2022-07-29 12:02:31 -04:00
Daniel Agar	a397c09e59	ekf2: use estimator_aid_src for all yaw sources (mag, gnss, ev)	2022-07-29 11:20:48 -04:00
Agata Barcis	d5d88cba5b	generate_microRTPS_bridge.py updated to support ROS2 humble Signed-off-by: Agata Barcis <agata.barcis@tii.ae>	2022-07-29 15:21:05 +02:00
Silvan Fuhrer	638eff426a	AirspeedValidator: increase max update step size of tas_scale_validated from 1% to 5% Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-07-29 09:29:27 +02:00