gimbal: add paranoid quaternion checks and initialized values

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>
2026-06-01 04:10:04 +08:00 · 2022-08-04 17:19:19 -04:00 · 2022-08-04 12:55:21 -04:00 · 2022-08-04 16:50:31 +02:00 · 2022-08-04 16:50:31 +02:00 · 2022-08-04 13:36:15 +02:00
118 changed files with 1908 additions and 984 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/)
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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)
@@ -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
+)
@@ -1,6 +1,7 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,56 +32,50 @@
 *
 ****************************************************************************/

-#ifndef ATT_POS_MOCAP_HPP
-#define ATT_POS_MOCAP_HPP
+#include <nuttx/config.h>
+#include "arm_internal.h"
+#include "arm_arch.h"
+#include "chip.h"

-#include <uORB/topics/vehicle_odometry.h>
+#include "nvic.h"

-class MavlinkStreamAttPosMocap : public MavlinkStream
+
+/****************************************************************************
+ * Name: watchdog_pet()
+ *
+ * Description:
+ *   This function resets the Independent watchdog (IWDG)
+ *
+ *
+ * Input Parameters:
+ *   none.
+ *
+ * Returned value:
+ *   none.
+ *
+ ****************************************************************************/
+
+void watchdog_pet(void)
 {
-public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttPosMocap(mavlink); }
+}

-	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; }
+/****************************************************************************
+ * Name: watchdog_init()
+ *
+ * Description:
+ *   This function initialize the Independent watchdog (IWDG)
+ *
+ *
+ * Input Parameters:
+ *   none.
+ *
+ * Returned value:
+ *   none.
+ *
+ ****************************************************************************/

-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
+void watchdog_init(void)
+{

-	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;
-		}
-
-		return false;
-	}
-};
-
-#endif // ATT_POS_MOCAP_HPP
+	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

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

@@ -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
@@ -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;
+}
+
@@ -64,7 +64,9 @@ enum class ghstFrameType {
 };

 enum class ghstTelemetryType {
-	batteryPack = 0x23	// battery pack status frame type
+	batteryPack = 0x23,	// battery pack status frame type
+	gpsPrimary = 0x25,	// GPS primary data (lat/long/alt)
+	gpsSecondary = 0x26	// GPS secondary data (course, dist, flags)
 };

 struct ghst_frame_header_t {
@@ -139,4 +141,27 @@ __EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len,
 __EXPORT bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,
 		uint16_t current_in_10mA, uint16_t fuel_in_10mAh);

+/**
+ * Send primary GPS information
+ * @param uart_fd UART file descriptor
+ * @param latitude GPS latitude [1e-7 degrees]
+ * @param longitude GPS longitude [1e-7 degrees]
+ * @param altitude GPS altitude [1m]
+ * @return true on success
+ */
+__EXPORT bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude);
+
+/**
+ * Send secondary GPS information
+ * @param uart_fd UART file descriptor
+ * @param ground_speed Ground Speed [1 km/h]
+ * @param ground_course Ground Course [1e-7 degrees]
+ * @param num_sats GPS Satellite count
+ * @param home_dist Distance to Home [10 m]
+ * @param home_dir Direction to Home [1e-7 degrees]
+ * @param flags GPS Flags
+ * @return true on success
+ */
+__EXPORT bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course,
+		uint8_t num_sats, uint16_t home_dist, uint16_t home_dir, uint8_t flags);
 __END_DECLS
@@ -79,7 +79,7 @@ if validate:
        print("  v1.9.0-beta1")
        print("  v1.9.0-1.0.0")
        print("  v1.9.0-1.0.0-alpha2")
-        print("See also https://dev.px4.io/master/en/setup/building_px4.html#firmware_version")
+        print("See also https://docs.px4.io/main/en/dev_setup/building_px4.html#building-for-nuttx")
        print("")
        sys.exit(1)

@@ -39,51 +39,48 @@
 #include "WindEstimator.hpp"

 bool
-WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
-			  const matrix::Quatf &q_att)
+WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
+			  const float tas_meas, const float tas_variance)
 {
-	// do no initialise if ground velocity is low
-	// this should prevent the filter from initialising on the ground
-	if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0f) {
-		return false;
+	if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) {
+
+		const float cos_heading = cosf(heading_rad);
+		const float sin_heading = sinf(heading_rad);
+
+		// initialise wind states assuming zero side slip and horizontal flight
+		_state(INDEX_W_N) = velI(0) - tas_meas * cos_heading;
+		_state(INDEX_W_E) = velI(1) - tas_meas * sin_heading;
+		_state(INDEX_TAS_SCALE) = _scale_init;
+
+		constexpr float initial_sideslip_uncertainty = math::radians(INITIAL_BETA_ERROR_DEG);
+		const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty));
+		constexpr float heading_variance = sq(math::radians(INITIAL_HEADING_ERROR_DEG));
+
+		// rotate wind velocity into earth frame aligned with vehicle yaw
+		const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading;
+		const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading;
+
+		_P(INDEX_W_N, INDEX_W_N) = tas_variance * sq(cos_heading) + heading_variance * sq(-Wx * sin_heading - Wy * cos_heading)
+					   + initial_wind_var_body_y * sq(sin_heading);
+		_P(INDEX_W_N, INDEX_W_E) = tas_variance * sin_heading * cos_heading + heading_variance *
+					   (-Wx * sin_heading - Wy * cos_heading) * (Wx * cos_heading - Wy * sin_heading) -
+					   initial_wind_var_body_y * sin_heading * cos_heading;
+		_P(INDEX_W_E, INDEX_W_N) = _P(INDEX_W_N, INDEX_W_E);
+		_P(INDEX_W_E, INDEX_W_E) = tas_variance * sq(sin_heading) + heading_variance * sq(Wx * cos_heading - Wy * sin_heading) +
+					   initial_wind_var_body_y * sq(cos_heading);
+
+		// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
+		_P(INDEX_W_N, INDEX_W_N) += hor_vel_variance;
+		_P(INDEX_W_E, INDEX_W_E) += hor_vel_variance;
+
+	} else {
+		// no airspeed available
+		_state.setZero();
+		_state(INDEX_TAS_SCALE) = 1.0f;
+		_P.setZero();
+		_P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = sq(INITIAL_WIND_ERROR);
 	}

-	const float v_n = velI(0);
-	const float v_e = velI(1);
-
-	const float heading_est = matrix::Eulerf(q_att).psi();
-
-	// initilaise wind states assuming zero side slip and horizontal flight
-	_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
-	_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est);
-	_state(INDEX_TAS_SCALE) = _scale_init;
-
-	// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
-	float L0 = v_e * v_e;
-	float L1 = v_n * v_n;
-	float L2 = L0 + L1;
-	float L3 = tas_meas / (L2 * sqrtf(L2));
-	float L4 = L3 * v_e * v_n + 1.0f;
-	float L5 = 1.0f / sqrtf(L2);
-	float L6 = -L5 * tas_meas;
-
-	matrix::Matrix3f L;
-	L.setZero();
-	L(0, 0) = L4;
-	L(0, 1) = L0 * L3 + L6;
-	L(1, 0) = L1 * L3 + L6;
-	L(1, 1) = L4;
-	L(2, 2) = 1.0f;
-
-	// get an estimate of the state covariance matrix given the estimated variance of ground velocity
-	// and measured airspeed
-	_P.setZero();
-	_P(INDEX_W_N, INDEX_W_N) = velIvar(0);
-	_P(INDEX_W_E, INDEX_W_E) = velIvar(1);
-	_P(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = 0.0001f;
-
-	_P = L * _P * L.transpose();
-
 	// reset the timestamp for measurement rejection
 	_time_rejected_tas = 0;
 	_time_rejected_beta = 0;
@@ -124,13 +121,12 @@ WindEstimator::update(uint64_t time_now)

 void
 WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI,
-			     const matrix::Vector2f &velIvar, const matrix::Quatf &q_att)
+			     const float hor_vel_variance, const matrix::Quatf &q_att)
 {
-	matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) };

 	if (!_initialised) {
 		// try to initialise
-		_initialised = initialise(velI, velIvar_constrained, true_airspeed, q_att);
+		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
 		return;
 	}

@@ -157,7 +153,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	if (meas_is_rejected || _tas_innov_var < 0.f) {
 		// only reset filter if _tas_innov_var gets unfeasible
 		if (_tas_innov_var < 0.0f) {
-			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed, q_att);
+			_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
 		}

 		// we either did a filter reset or the current measurement was rejected so do not fuse
@@ -176,10 +172,11 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 }

 void
-WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
+WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const float hor_vel_variance,
+			 const matrix::Quatf &q_att)
 {
 	if (!_initialised) {
-		_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
+		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
 		return;
 	}

@@ -251,7 +248,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const

 	if (meas_is_rejected || reinit_filter) {
 		if (reinit_filter) {
-			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
+			_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
 		}

 		// we either did a filter reset or the current measurement was rejected so do not fuse
@@ -60,8 +60,9 @@ public:
 	void update(uint64_t time_now);

 	void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
-			   const matrix::Vector2f &velIvar, const matrix::Quatf &q_att);
-	void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att);
+			   const float hor_vel_variance, const matrix::Quatf &q_att);
+	void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const float hor_vel_variance,
+		       const matrix::Quatf &q_att);

 	bool is_estimate_valid() { return _initialised; }

@@ -97,6 +98,13 @@ private:
 		INDEX_TAS_SCALE
 	};	///< enum which can be used to access state.

+	static constexpr float INITIAL_WIND_ERROR =
+		2.5f; // initial uncertainty of each wind state when filter is initialised without airspeed [m/s]
+	static constexpr float INITIAL_BETA_ERROR_DEG =
+		15.0f;	// sidelip uncertainty used to initialise the filter with a true airspeed measurement [deg]
+	static constexpr float INITIAL_HEADING_ERROR_DEG =
+		10.0f; // heading uncertainty used to initialise the filter with a true airspeed measurement [deg]
+
 	matrix::Vector3f _state{0.f, 0.f, 1.f};
 	matrix::Matrix3f _P;		///< state covariance matrix

@@ -127,8 +135,11 @@ private:
 	bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle

 	// initialise state and state covariance matrix
-	bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
-			const matrix::Quatf &q_att);
+	bool initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
+			const float tas_meas = NAN, const float tas_variance = NAN);

 	void run_sanity_checks();
+
+	// return the square of two floating point numbers
+	static constexpr float sq(float var) { return var * var; }
 };
@@ -81,11 +81,11 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
 		Quatf q(att_q);

 		// airspeed fusion (with raw TAS)
-		const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
-		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}, q);
+		const float hor_vel_variance =  lpos_evh * lpos_evh;
+		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q);

 		// sideslip fusion
-		_wind_estimator.fuse_beta(time_now_usec, vI, q);
+		_wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q);
 	}
 }

@@ -145,9 +145,9 @@ AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vec
 		// check passes if the average airspeed with the scale applied is closer to groundspeed than without
 		if (fabsf(TAS_to_grounspeed_error_new) < fabsf(TAS_to_grounspeed_error_current)) {

-			// constrain the scale update to max 0.01 at a time
-			const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_validated - 0.01f,
-							    _CAS_scale_validated + 0.01f);
+			// constrain the scale update to max 0.05 at a time
+			const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_validated - 0.05f,
+							    _CAS_scale_validated + 0.05f);

 			_CAS_scale_validated = new_scale_constrained;
 		}
@@ -147,8 +147,8 @@ private:
 	bool _initialized{false}; /**< module initialized*/
 	bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
 	bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
-	float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
-	float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
+	float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */
+	float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */
 	bool _armed_prev{false};

 	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
@@ -184,7 +184,8 @@ private:
 		(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
 		(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */

-		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
+		(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas
 	)

 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -373,7 +374,7 @@ AirspeedModule::Run()
 				// takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed
 				if (_in_takeoff_situation &&
 				    (airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() ||
-				     _ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
+				     (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get()))) {
 					_in_takeoff_situation = false;
 				}

@@ -516,11 +517,14 @@ void AirspeedModule::update_wind_estimator_sideslip()
 	_wind_estimator_sideslip.update(_time_now_usec);

 	if (_vehicle_local_position_valid
-	    && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
+	    && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW &&
+	    _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
 		Quatf q(_vehicle_attitude.q);

-		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
+		const float hor_vel_variance =  _vehicle_local_position.evh * _vehicle_local_position.evh;
+
+		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, q);
 	}

 	_wind_estimate_sideslip.timestamp = _time_now_usec;
@@ -540,13 +544,20 @@ void AirspeedModule::update_wind_estimator_sideslip()

 void AirspeedModule::update_ground_minus_wind_airspeed()
 {
-	// calculate airspeed estimate based on groundspeed-windspeed to use as fallback
-	const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
-	const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
-	const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
-	_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
-	_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
-				 _vehicle_air_data.baro_temp_celcius);
+	const float wind_uncertainty = sqrtf(_wind_estimate_sideslip.variance_north + _wind_estimate_sideslip.variance_east);
+
+	if (wind_uncertainty < _param_wind_sigma_max_synth_tas.get()) {
+		// calculate airspeed estimate based on groundspeed-windspeed
+		const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
+		const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
+		const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
+		_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
+		_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
+					 _vehicle_air_data.baro_temp_celcius);
+
+	} else {
+		_ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN;
+	}
 }


@@ -227,3 +227,17 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
 * @max 1000
 */
 PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);
+
+/**
+ * Horizontal wind uncertainty threshold for synthetic airspeed.
+ *
+ * The synthetic airspeed estimate (from groundspeed and heading) will be declared valid
+ * as soon and as long the horizontal wind uncertainty drops below this value.
+ *
+ * @unit m/s
+ * @min 0.001
+ * @max 5
+ * @decimal 3
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f);
@@ -252,10 +252,10 @@ void AttitudeEstimatorQ::update_motion_capture_odometry()
 		if (_vehicle_mocap_odometry_sub.update(&mocap)) {
 			// validation check for mocap attitude data
 			bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
-					       && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
-							       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
-							       fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
-									       mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
+					       && (PX4_ISFINITE(mocap.orientation_variance[0]) ? sqrtf(fmaxf(
+							       mocap.orientation_variance[0],
+							       fmaxf(mocap.orientation_variance[1],
+									       mocap.orientation_variance[2]))) <= _eo_max_std_dev : true);

 			if (mocap_att_valid) {
 				Dcmf Rmoc = Quatf(mocap.q);
@@ -361,10 +361,10 @@ void AttitudeEstimatorQ::update_visual_odometry()
 		if (_vehicle_visual_odometry_sub.update(&vision)) {
 			// validation check for vision attitude data
 			bool vision_att_valid = PX4_ISFINITE(vision.q[0])
-						&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
-								vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
-								fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
-										vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
+						&& (PX4_ISFINITE(vision.orientation_variance[0]) ? sqrtf(fmaxf(
+								vision.orientation_variance[0],
+								fmaxf(vision.orientation_variance[1],
+										vision.orientation_variance[2]))) <= _eo_max_std_dev : true);

 			if (vision_att_valid) {
 				Dcmf Rvis = Quatf(vision.q);
@@ -48,7 +48,7 @@ bool PreFlightCheck::airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_
 	// We no longer support VTOL on fmu-v2, so we need to warn existing users.
 	if (status.is_vtol) {
 		mavlink_log_critical(mavlink_log_pub,
-				     "VTOL is not supported with fmu-v2, see docs.px4.io/master/en/config/firmware.html#bootloader");
+				     "VTOL is not supported with fmu-v2, see docs.px4.io/main/en/config/firmware.html#bootloader");
 		success = false;
 	}

@@ -207,7 +207,7 @@ struct extVisionSample {
 	Vector3f    vel{};         ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
 	Quatf       quat{};        ///< quaternion defining rotation from body to earth frame
 	Vector3f    posVar{};      ///< XYZ position variances (m**2)
-	Matrix3f    velCov{};      ///< XYZ velocity covariances ((m/sec)**2)
+	Vector3f    velVar{};      ///< XYZ velocity variances ((m/sec)**2)
 	float       angVar{};      ///< angular heading variance (rad**2)
 	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
@@ -361,21 +361,41 @@ void Ekf::controlExternalVisionFusion()
 		}

 		// determine if we should use the yaw observation
-		if (_control_status.flags.ev_yaw) {
-			if (reset && _control_status_prev.flags.ev_yaw) {
-				resetYawToEv();
+		resetEstimatorAidStatus(_aid_src_ev_yaw);
+		const float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
+		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
+
+		if (PX4_ISFINITE(measured_hdg)) {
+			_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
+			_aid_src_ev_yaw.observation = measured_hdg;
+			_aid_src_ev_yaw.observation_variance = ev_yaw_obs_var;
+			_aid_src_ev_yaw.fusion_enabled = _control_status.flags.ev_yaw;
+
+			if (_control_status.flags.ev_yaw) {
+				if (reset && _control_status_prev.flags.ev_yaw) {
+					resetYawToEv();
+				}
+
+				const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
+
+				fuseYaw(innovation, ev_yaw_obs_var, _aid_src_ev_yaw);
+
+			} else {
+				// populate estimator_aid_src_ev_yaw with delta heading innovations for logging
+				// use the change in yaw since the last measurement
+				const float measured_hdg_prev = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed_prev.quat) : getEuler312Yaw(_ev_sample_delayed_prev.quat);
+
+				// calculate the change in yaw since the last measurement
+				const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev);
+
+				_aid_src_ev_yaw.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yaw_pred_prev - ev_delta_yaw);
 			}
-
-			float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
-
-			float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
-			float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
-			fuseYaw(innovation, obs_var);
 		}

 		// record observation and estimate for use next time
 		_ev_sample_delayed_prev = _ev_sample_delayed;
 		_hpos_pred_prev = _state.pos.xy();
+		_yaw_pred_prev = getEulerYaw(_state.quat_nominal);

 	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel ||  _control_status.flags.ev_yaw)
 		   && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
@@ -584,7 +604,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)

 				fuseGpsYaw();

-				const bool is_fusion_failing = isTimedOut(_time_last_gps_yaw_fuse, _params.reset_timeout_max);
+				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);

 				if (is_fusion_failing) {
 					if (_nb_gps_yaw_reset_available > 0) {
@@ -1111,18 +1131,13 @@ void Ekf::controlAuxVelFusion()

 		if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {

+			updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
+
 			if (isHorizontalAidingActive()) {
-
-				const float aux_vel_innov_gate = fmaxf(_params.auxvel_gate, 1.f);
-
-				_aux_vel_innov = _state.vel - auxvel_sample_delayed.vel;
-
-				fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
-						       _aux_vel_innov_var, _aux_vel_test_ratio);
-
-				// Can be enabled after bit for this is added to EKF_AID_MASK
-				// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
-				//		_aux_vel_innov_var, _aux_vel_test_ratio);
+				_aid_src_aux_vel.fusion_enabled[0] = PX4_ISFINITE(auxvel_sample_delayed.vel(0));
+				_aid_src_aux_vel.fusion_enabled[1] = PX4_ISFINITE(auxvel_sample_delayed.vel(1));
+				_aid_src_aux_vel.fusion_enabled[2] = PX4_ISFINITE(auxvel_sample_delayed.vel(2));
+				fuseVelocity(_aid_src_aux_vel);
 			}
 		}
 	}
@@ -96,7 +96,7 @@ public:

 	void getAuxVelInnov(float aux_vel_innov[2]) const;
 	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
-	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }
+	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = Vector3f(_aid_src_aux_vel.test_ratio).max(); }

 	void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); }
 	void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); }
@@ -111,13 +111,54 @@ public:
 	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
 	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }

-	void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
-	void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
-	void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
+	void getHeadingInnov(float &heading_innov) const {
+		if (_control_status.flags.mag_hdg) {
+			heading_innov = _aid_src_mag_heading.innovation;

-	void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
-	void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
-	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
+		} else if (_control_status.flags.mag_3D) {
+			heading_innov = Vector3f(_aid_src_mag.innovation).max();
+
+		} else if (_control_status.flags.gps_yaw) {
+			heading_innov = _aid_src_gnss_yaw.innovation;
+
+		} else if (_control_status.flags.ev_yaw) {
+			heading_innov = _aid_src_ev_yaw.innovation;
+		}
+	}
+
+	void getHeadingInnovVar(float &heading_innov_var) const {
+		if (_control_status.flags.mag_hdg) {
+			heading_innov_var = _aid_src_mag_heading.innovation_variance;
+
+		} else if (_control_status.flags.mag_3D) {
+			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
+
+		} else if (_control_status.flags.gps_yaw) {
+			heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
+
+		} else if (_control_status.flags.ev_yaw) {
+			heading_innov_var = _aid_src_ev_yaw.innovation_variance;
+		}
+	}
+
+	void getHeadingInnovRatio(float &heading_innov_ratio) const {
+		if (_control_status.flags.mag_hdg) {
+			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
+
+		} else if (_control_status.flags.mag_3D) {
+			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
+
+		} else if (_control_status.flags.gps_yaw) {
+			heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
+
+		} else if (_control_status.flags.ev_yaw) {
+			heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
+		}
+	}
+
+	void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
+	void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
+	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }

 	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
 	void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); }
@@ -160,6 +201,8 @@ public:
 	// get the orientation (quaterion) covariances
 	matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<4, 4>(0, 0); }

+	matrix::SquareMatrix<float, 3> orientation_covariances_euler() const;
+
 	// get the linear velocity covariances
 	matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<3, 3>(4, 4); }

@@ -354,9 +397,17 @@ public:

 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }

+	const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
+
+	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
 	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }

+	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
+	const auto &aid_src_mag() const { return _aid_src_mag; }
+
+	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
+
 private:

 	// set the internal states and status to their default value
@@ -391,6 +442,7 @@ private:
 	// variables used when position data is being fused using a relative position odometry model
 	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
 	Vector2f _hpos_pred_prev{};		///< previous value of NE position state used by odometry fusion (m)
+	float _yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
 	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
 	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
 	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
@@ -418,10 +470,7 @@ private:
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
-	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
-	uint64_t _time_last_mag_heading_fuse{0};
-	uint64_t _time_last_mag_3d_fuse{0};
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source

@@ -467,15 +516,6 @@ private:
 	Vector3f _ev_pos_innov{};	///< external vision position innovations (m)
 	Vector3f _ev_pos_innov_var{};	///< external vision position innovation variances (m**2)

-	Vector3f _aux_vel_innov{};	///< horizontal auxiliary velocity innovations: (m/sec)
-	Vector3f _aux_vel_innov_var{};	///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
-
-	float _heading_innov{0.0f};	///< heading measurement innovation (rad)
-	float _heading_innov_var{0.0f};	///< heading measurement innovation variance (rad**2)
-
-	Vector3f _mag_innov{};		///< earth magnetic field innovations (Gauss)
-	Vector3f _mag_innov_var{};	///< earth magnetic field innovation variance (Gauss**2)
-
 	Vector2f _drag_innov{};		///< multirotor drag measurement innovation (m/sec**2)
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)

@@ -505,9 +545,17 @@ private:

 	estimator_aid_source_2d_s _aid_src_fake_pos{};

+	estimator_aid_source_1d_s _aid_src_ev_yaw{};
+
+	estimator_aid_source_1d_s _aid_src_gnss_yaw{};
 	estimator_aid_source_3d_s _aid_src_gnss_vel{};
 	estimator_aid_source_3d_s _aid_src_gnss_pos{};

+	estimator_aid_source_1d_s _aid_src_mag_heading{};
+	estimator_aid_source_3d_s _aid_src_mag{};
+
+	estimator_aid_source_3d_s _aid_src_aux_vel{};
+
 	// output predictor states
 	Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
 	Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
@@ -606,12 +654,12 @@ private:
 	void predictCovariance();

 	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, bool update_all_states = true);
+	bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true);

 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	// innovation : prediction - measurement
 	// variance : observaton variance
-	bool fuseYaw(const float innovation, const float variance);
+	bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status);

 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();
@@ -674,6 +722,12 @@ private:
 	// fuse optical flow line of sight rate measurements
 	void fuseOptFlow();

+	void updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const;
+	void updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const;
+
+	void fuseVelocity(estimator_aid_source_3d_s& vel_aid_src);
+	void fusePosition(estimator_aid_source_3d_s& pos_aid_src);
+
 	bool fuseHorizontalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
 				    Vector3f &innov_var, Vector2f &test_ratio);

@@ -686,10 +740,11 @@ private:
 	bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
 				  float &innov_var, float &test_ratio);

+	void updateGpsYaw(const gpsSample &gps_sample);
 	void updateGpsVel(const gpsSample &gps_sample);
-	void fuseGpsVel();
-
 	void updateGpsPos(const gpsSample &gps_sample);
+
+	void fuseGpsVel();
 	void fuseGpsPos();

 	// calculate optical flow body angular rate compensation
@@ -1111,16 +1166,28 @@ private:

 	void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
 	{
-		status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
-		status.innovation_rejected = (status.test_ratio > 1.f);
+		if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance)) {
+			status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
+			status.innovation_rejected = (status.test_ratio > 1.f);
+
+		} else {
+			status.test_ratio = INFINITY;
+			status.innovation_rejected = true;
+		}
 	}

 	template <typename T>
 	void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
 	{
 		for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
-			status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
-			status.innovation_rejected[i] = (status.test_ratio[i] > 1.f);
+			if (PX4_ISFINITE(status.innovation[i]) && PX4_ISFINITE(status.innovation_variance[i])) {
+				status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
+				status.innovation_rejected[i] = (status.test_ratio[i] > 1.f);
+
+			} else {
+				status.test_ratio[i] = INFINITY;
+				status.innovation_rejected[i] = true;
+			}
 		}
 	}
 };
@@ -657,14 +657,14 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp

 void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
 {
-	aux_vel_innov[0] = _aux_vel_innov(0);
-	aux_vel_innov[1] = _aux_vel_innov(1);
+	aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
+	aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
 }

 void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
 {
-	aux_vel_innov_var[0] = _aux_vel_innov_var(0);
-	aux_vel_innov_var[1] = _aux_vel_innov_var(1);
+	aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
+	aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
 }

 // get the state vector at the delayed time horizon
@@ -733,9 +733,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons

 			resetVerticalPositionTo(_gps_alt_ref - current_alt);

-			_baro_hgt_offset -= _state_reset_status.posD_change;
+			_baro_hgt_offset += _state_reset_status.posD_change;

-			_rng_hgt_offset -= _state_reset_status.posD_change;
+			_rng_hgt_offset += _state_reset_status.posD_change;
 			_ev_hgt_offset -= _state_reset_status.posD_change;
 		}

@@ -931,7 +931,17 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	status = _innov_check_fail_status.value;

 	// return the largest magnetometer innovation test ratio
-	mag = sqrtf(math::max(_yaw_test_ratio, _mag_test_ratio.max()));
+	if (_control_status.flags.mag_hdg) {
+		mag = sqrtf(_aid_src_mag_heading.test_ratio);
+
+	} else if (_control_status.flags.mag_3D) {
+		mag = sqrtf(Vector3f(_aid_src_mag.test_ratio).max());
+
+	} else if (_control_status.flags.gps_yaw) {
+		mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
+	} else {
+		mag = NAN;
+	}

 	// return the largest velocity and position innovation test ratio
 	vel = NAN;
@@ -1002,9 +1012,23 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
+
+	bool mag_innov_good = true;
+
+	if (_control_status.flags.mag_hdg) {
+		if (_aid_src_mag_heading.test_ratio < 1.f) {
+			mag_innov_good = false;
+		}
+
+	} else if (_control_status.flags.mag_3D) {
+		if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) {
+			mag_innov_good = false;
+		}
+	}
+
 	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
 	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
-	const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
+
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
 	soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
 	*status = soln_status.value;
@@ -1260,10 +1284,6 @@ void Ekf::stopMag3DFusion()
 		_control_status.flags.mag_3D = false;
 		_control_status.flags.mag_dec = false;

-		_mag_innov.zero();
-		_mag_innov_var.zero();
-		_mag_test_ratio.zero();
-
 		_fault_status.flags.bad_mag_x = false;
 		_fault_status.flags.bad_mag_y = false;
 		_fault_status.flags.bad_mag_z = false;
@@ -1278,8 +1298,6 @@ void Ekf::stopMagHdgFusion()
 		_control_status.flags.mag_hdg = false;

 		_fault_status.flags.bad_hdg = false;
-
-		_yaw_test_ratio = 0.f;
 	}
 }

@@ -1298,8 +1316,6 @@ void Ekf::startMag3DFusion()

 		stopMagHdgFusion();

-		_yaw_test_ratio = 0.0f;
-
 		zeroMagCov();
 		loadMagCovData();
 		_control_status.flags.mag_3D = true;
@@ -1492,7 +1508,7 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const

 Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
 {
-	Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
+	Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar);

 	// rotate measurement into correct earth frame if required
 	switch (_ev_sample_delayed.vel_frame) {
@@ -1679,6 +1695,7 @@ void Ekf::stopGpsYawFusion()
 	if (_control_status.flags.gps_yaw) {
 		ECL_INFO("stopping GPS yaw fusion");
 		_control_status.flags.gps_yaw = false;
+		resetEstimatorAidStatus(_aid_src_gnss_yaw);
 	}
 }

@@ -1744,9 +1761,9 @@ void Ekf::stopEvYawFusion()

 void Ekf::stopAuxVelFusion()
 {
-	_aux_vel_innov.setZero();
-	_aux_vel_innov_var.setZero();
-	_aux_vel_test_ratio.setZero();
+	ECL_INFO("stopping aux vel fusion");
+	//_control_status.flags.aux_vel = false;
+	resetEstimatorAidStatus(_aid_src_aux_vel);
 }

 void Ekf::stopFlowFusion()
@@ -1886,3 +1903,60 @@ void Ekf::resetGpsDriftCheckFilters()
 	_gps_vertical_position_drift_rate_m_s = NAN;
 	_gps_filtered_horizontal_velocity_m_s = NAN;
 }
+
+matrix::SquareMatrix<float, 3> Ekf::orientation_covariances_euler() const
+{
+	// Jacobian matrix (3x4) containing the partial derivatives of the
+	// Euler angle equations with respect to the quaternions
+	matrix::Matrix<float, 3, 4> G;
+
+	// quaternion components
+	float q1 = _state.quat_nominal(0);
+	float q2 = _state.quat_nominal(1);
+	float q3 = _state.quat_nominal(2);
+	float q4 = _state.quat_nominal(3);
+
+	// numerator components
+	float n1 =  2 * q1 * q2 + 2 * q2 * q4;
+	float n2 = -2 * q2 * q2 - 2 * q3 * q3 + 1;
+	float n3 =  2 * q1 * q4 + 2 * q2 * q3;
+	float n4 = -2 * q3 * q3 - 2 * q4 * q4 + 1;
+	float n5 =  2 * q1 * q3 + 2 * q2 * q4;
+	float n6 = -2 * q1 * q2 - 2 * q2 * q4;
+	float n7 = -2 * q1 * q4 - 2 * q2 * q3;
+
+	// Protect against division by 0
+	float d1 = n1 * n1 + n2 * n2;
+	float d2 = n3 * n3 + n4 * n4;
+
+	if (fabsf(d1) < FLT_EPSILON) {
+		d1 = FLT_EPSILON;
+	}
+
+	if (fabsf(d2) < FLT_EPSILON) {
+		d2 = FLT_EPSILON;
+	}
+
+	// Protect against square root of negative numbers
+	float x = math::max(-n5 * n5 + 1, 0.0f);
+
+	// compute G matrix
+	float sqrt_x = sqrtf(x);
+	float g00_03 = 2 * q2 * n2 / d1;
+	G(0, 0) =  g00_03;
+	G(0, 1) = -4 * q2 * n6 / d1 + (2 * q1 + 2 * q4) * n2 / d1;
+	G(0, 2) = -4 * q3 * n6 / d1;
+	G(0, 3) =  g00_03;
+	G(1, 0) =  2 * q3 / sqrt_x;
+	G(1, 1) =  2 * q4 / sqrt_x;
+	G(1, 2) =  2 * q1 / sqrt_x;
+	G(1, 3) =  2 * q2 / sqrt_x;
+	G(2, 0) =  2 * q4 * n4 / d2;
+	G(2, 1) =  2 * q3 * n4 / d2;
+	G(2, 2) =  2 * q2 * n4 / d2 - 4 * q3 * n7 / d2;
+	G(2, 3) =  2 * q1 * n4 / d2 - 4 * q4 * n7 / d2;
+
+	const matrix::SquareMatrix<float, 4> quat_covariances = P.slice<4, 4>(0, 0);
+
+	return G * quat_covariances * G.transpose();
+}
@@ -245,6 +245,8 @@ public:

 	// Getters for samples on the delayed time horizon
 	const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
+	const imuSample &get_imu_sample_newest() const { return _newest_high_rate_imu_sample; }
+
 	const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
 	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }

@@ -328,12 +330,9 @@ protected:
 	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).

 	// innovation consistency check monitoring ratios
-	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
-	AlphaFilter<float> _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
-	Vector3f _mag_test_ratio{};		// magnetometer XYZ innovation consistency check ratios
+	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 	Vector2f _ev_vel_test_ratio{};		// EV velocity innovation consistency check ratios
 	Vector2f _ev_pos_test_ratio{};		// EV position innovation consistency check ratios
-	Vector2f _aux_vel_test_ratio{};		// Auxiliary horizontal velocity innovation consistency check ratio
 	float _optflow_test_ratio{};		// Optical flow innovation consistency check ratio
 	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio
 	float _beta_test_ratio{};		// sideslip innovation consistency check ratio
@@ -49,10 +49,7 @@ void Ekf::controlGpsFusion()
 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {

-		// reset flags
-		resetEstimatorAidStatusFlags(_aid_src_gnss_vel);
-		resetEstimatorAidStatusFlags(_aid_src_gnss_pos);
-
+		updateGpsYaw(_gps_sample_delayed);
 		updateGpsVel(_gps_sample_delayed);
 		updateGpsPos(_gps_sample_delayed);

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -39,16 +39,43 @@
 /* #include <mathlib/mathlib.h> */
 #include "ekf.h"

+void Ekf::updateGpsYaw(const gpsSample &gps_sample)
+{
+	auto &gps_yaw = _aid_src_gnss_yaw;
+	resetEstimatorAidStatusFlags(gps_yaw);
+
+	if (PX4_ISFINITE(gps_sample.yaw)) {
+		// initially populate for estimator_aid_src_gnss_yaw logging
+
+		// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
+		const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
+
+		gps_yaw.observation = measured_hdg;
+		gps_yaw.observation_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
+
+		// define the predicted antenna array vector and rotate into earth frame
+		const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+		const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+		const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));
+		gps_yaw.innovation = wrap_pi(predicted_hdg - gps_yaw.observation);
+
+		gps_yaw.fusion_enabled = _control_status.flags.gps_yaw;
+
+		gps_yaw.timestamp_sample = gps_sample.time_us;
+	}
+}
+
 void Ekf::updateGpsVel(const gpsSample &gps_sample)
 {
+	auto &gps_vel = _aid_src_gnss_vel;
+	resetEstimatorAidStatus(gps_vel);
+
 	const float vel_var = sq(gps_sample.sacc);
 	const Vector3f obs_var{vel_var, vel_var, vel_var * sq(1.5f)};

 	// innovation gate size
 	const float innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);

-	auto &gps_vel = _aid_src_gnss_vel;
-
 	for (int i = 0; i < 3; i++) {
 		gps_vel.observation[i] = gps_sample.vel(i);
 		gps_vel.observation_variance[i] = obs_var(i);
@@ -72,6 +99,9 @@ void Ekf::updateGpsVel(const gpsSample &gps_sample)

 void Ekf::updateGpsPos(const gpsSample &gps_sample)
 {
+	auto &gps_pos = _aid_src_gnss_pos;
+	resetEstimatorAidStatus(gps_pos);
+
 	Vector3f position;
 	position(0) = gps_sample.pos(0);
 	position(1) = gps_sample.pos(1);
@@ -100,8 +130,6 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 	// innovation gate size
 	float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);

-	auto &gps_pos = _aid_src_gnss_pos;
-
 	for (int i = 0; i < 3; i++) {
 		gps_pos.observation[i] = position(i);
 		gps_pos.observation_variance[i] = obs_var(i);
@@ -60,18 +60,27 @@ void Ekf::fuseGpsYaw()
 	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
 	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;

-	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
-		return;
-	}
-
 	// calculate predicted antenna yaw angle
 	const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));

+	// wrap the innovation to the interval between +-pi
+	const float heading_innov = wrap_pi(predicted_hdg - measured_hdg);
+
 	// using magnetic heading process noise
 	// TODO extend interface to use yaw uncertainty provided by GPS if available
 	const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));

+	_aid_src_gnss_yaw.timestamp_sample = _gps_sample_delayed.time_us;
+	_aid_src_gnss_yaw.observation = measured_hdg;
+	_aid_src_gnss_yaw.observation_variance = R_YAW;
+	_aid_src_gnss_yaw.innovation = heading_innov;
+	_aid_src_gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw;
+
+	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+		return;
+	}
+
 	// calculate intermediate variables
 	const float HK0 = sinf(_gps_yaw_offset);
 	const float HK1 = q0*q3;
@@ -121,9 +130,11 @@ void Ekf::fuseGpsYaw()
 	// const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);

 	// check if the innovation variance calculation is badly conditioned
-	_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
+	const float heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);

-	if (_heading_innov_var < R_YAW) {
+	_aid_src_gnss_yaw.innovation_variance = heading_innov_var;
+
+	if (heading_innov_var < R_YAW) {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_hdg = true;

@@ -134,19 +145,15 @@ void Ekf::fuseGpsYaw()
 	}

 	_fault_status.flags.bad_hdg = false;
-	const float HK32 = HK18 / _heading_innov_var;
+	const float HK32 = HK18 / heading_innov_var;

 	// calculate the innovation and define the innovation gate
 	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-	_heading_innov = predicted_hdg - measured_hdg;
-
-	// wrap the innovation to the interval between +-pi
-	_heading_innov = wrap_pi(_heading_innov);

 	// innovation test ratio
-	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
+	setEstimatorAidStatusTestRatio(_aid_src_gnss_yaw, innov_gate);

-	if (_yaw_test_ratio > 1.0f) {
+	if (_aid_src_gnss_yaw.innovation_rejected) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 		return;

@@ -154,10 +161,10 @@ void Ekf::fuseGpsYaw()
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}

-	_yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio);
+	_gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(heading_innov) * _aid_src_gnss_yaw.test_ratio);

-	if ((fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
-		&& !_control_status.flags.in_air && isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)) {
+	if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
+		&& !_control_status.flags.in_air && isTimedOut(_aid_src_gnss_yaw.time_last_fuse, (uint64_t)1e6)) {

 		// A constant large signed test ratio is a sign of wrong gyro bias
 		// Reset the yaw gyro variance to converge faster and avoid
@@ -184,12 +191,13 @@ void Ekf::fuseGpsYaw()
 		Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row));
 	}

-	const bool is_fused = measurementUpdate(Kfusion, Hfusion, _heading_innov);
+	const bool is_fused = measurementUpdate(Kfusion, Hfusion, heading_innov);
 	_fault_status.flags.bad_hdg = !is_fused;
+	_aid_src_gnss_yaw.fused = is_fused;

 	if (is_fused) {
-		_time_last_gps_yaw_fuse = _time_last_imu;
 		_time_last_heading_fuse = _time_last_imu;
+		_aid_src_gnss_yaw.time_last_fuse = _time_last_imu;
 	}
 }

@@ -207,11 +215,11 @@ bool Ekf::resetYawToGps()
 	// GPS yaw measurement is alreday compensated for antenna offset in the driver
 	const float measured_yaw = _gps_sample_delayed.yaw;

-	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
+	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance, true);

-	_time_last_gps_yaw_fuse = _time_last_imu;
-	_yaw_signed_test_ratio_lpf.reset(0.f);
+	_aid_src_gnss_yaw.time_last_fuse = _time_last_imu;
+	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);

 	return true;
 }
@@ -66,6 +66,28 @@ void Ekf::controlMagFusion()
 			}

 			_control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag);
+
+
+			// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
+			const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
+			const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
+			const Vector3f mag_earth_pred = R_to_earth * mag_observation;
+
+			resetEstimatorAidStatus(_aid_src_mag_heading);
+			_aid_src_mag_heading.timestamp_sample = mag_sample.time_us;
+			_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();;
+			_aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation);
+
+			// compute magnetometer innovations (for estimator_aid_src_mag logging)
+			//  rotate magnetometer earth field state into body frame
+			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+			const Vector3f mag_I_rot = R_to_body * _state.mag_I;
+			const Vector3f mag_innov = mag_I_rot - mag_observation;
+
+			resetEstimatorAidStatus(_aid_src_mag);
+			_aid_src_mag.timestamp_sample = mag_sample.time_us;
+			mag_observation.copyTo(_aid_src_mag.observation);
+			mag_innov.copyTo(_aid_src_mag.innovation);
 		}
 	}

@@ -344,9 +366,9 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 		float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
 		float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);

-		if (fuseYaw(innovation, obs_var)) {
-			_time_last_mag_heading_fuse = _time_last_imu;
-		}
+		_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
+
+		fuseYaw(innovation, obs_var, _aid_src_mag_heading);
 	}
 }

@@ -364,13 +386,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
-		fuseMag(mag, update_all_states);
+		fuseMag(mag, _aid_src_mag, update_all_states);

 	} else {
 		// The normal sequence is to fuse the magnetometer data first before fusing
 		// declination angle at a higher uncertainty to allow some learning of
 		// declination angle over time.
-		fuseMag(mag, update_all_states);
+		fuseMag(mag, _aid_src_mag, update_all_states);

 		if (_control_status.flags.mag_dec) {
 			fuseDeclination(0.5f);
@@ -45,7 +45,7 @@

 #include <mathlib/mathlib.h>

-bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
+bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states)
 {
 	// assign intermediate variables
 	const float q0 = _state.quat_nominal(0);
@@ -87,9 +87,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 	const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19);
 	const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19);

-	_mag_innov_var(0) = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG;
+	aid_src_mag.innovation_variance[0] = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG;

-	if (_mag_innov_var(0) < R_MAG) {
+	if (aid_src_mag.innovation_variance[0] < R_MAG) {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_mag_x = true;

@@ -101,7 +101,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)

 	_fault_status.flags.bad_mag_x = false;

-	const float HKX24 = 1.0F/_mag_innov_var(0);
+	const float HKX24 = 1.0F/aid_src_mag.innovation_variance[0];

 	// intermediate variables for calculation of innovations variances for Y and Z axes
 	// don't calculate all terms needed for observation jacobians and Kalman gains because
@@ -126,12 +126,11 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 	const float IV17 = 2*IV0 - 2*IV1;
 	const float IV18 = IV10 - IV8 + IV9;

-	_mag_innov_var(1) = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
-	_mag_innov_var(2) = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
+	aid_src_mag.innovation_variance[1] = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
+	aid_src_mag.innovation_variance[2] = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;

-	// chedk innovation variances for being badly conditioned
-
-	if (_mag_innov_var(1) < R_MAG) {
+	// check innovation variances for being badly conditioned
+	if (aid_src_mag.innovation_variance[1] < R_MAG) {
 		// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_mag_y = true;

@@ -143,7 +142,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)

 	_fault_status.flags.bad_mag_y = false;

-	if (_mag_innov_var(2) < R_MAG) {
+	if (aid_src_mag.innovation_variance[2] < R_MAG) {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_mag_z = true;

@@ -157,46 +156,40 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)

 	// rotate magnetometer earth field state into body frame
 	const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-
 	const Vector3f mag_I_rot = R_to_body * _state.mag_I;

 	// compute magnetometer innovations
-	_mag_innov = mag_I_rot + _state.mag_B - mag;
+	const Vector3f mag_observation = mag - _state.mag_B;
+	Vector3f mag_innov = mag_I_rot - mag_observation;

 	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
 	if (_control_status.flags.synthetic_mag_z) {
-		_mag_innov(2) = 0.0f;
+		mag_innov(2) = 0.0f;
 	}

+	for (int i = 0; i < 3; i++) {
+		aid_src_mag.observation[i] = mag_observation(i);
+		aid_src_mag.observation_variance[i] = R_MAG;
+		aid_src_mag.innovation[i] = mag_innov(i);
+		aid_src_mag.fusion_enabled[i] = _control_status.flags.mag_3D && update_all_states;
+	}
+
+	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
+	if (_control_status.flags.synthetic_mag_z) {
+		aid_src_mag.innovation[2] = 0.0f;
+		aid_src_mag.innovation_rejected[2] = false;
+	}
+
+	const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
+	setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
+
 	// Perform an innovation consistency check and report the result
-	bool all_innovation_checks_passed = true;
-
-	for (uint8_t index = 0; index <= 2; index++) {
-		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
-
-		bool rejected = (_mag_test_ratio(index) > 1.f);
-
-		switch (index) {
-		case 0:
-			_innov_check_fail_status.flags.reject_mag_x = rejected;
-			break;
-
-		case 1:
-			_innov_check_fail_status.flags.reject_mag_y = rejected;
-			break;
-
-		case 2:
-			_innov_check_fail_status.flags.reject_mag_z = rejected;
-			break;
-		}
-
-		if (rejected) {
-			all_innovation_checks_passed = false;
-		}
-	}
+	_innov_check_fail_status.flags.reject_mag_x = aid_src_mag.innovation_rejected[0];
+	_innov_check_fail_status.flags.reject_mag_y = aid_src_mag.innovation_rejected[1];
+	_innov_check_fail_status.flags.reject_mag_z = aid_src_mag.innovation_rejected[2];

 	// if any axis fails, abort the mag fusion
-	if (!all_innovation_checks_passed) {
+	if (aid_src_mag.innovation_rejected[0] || aid_src_mag.innovation_rejected[1] || aid_src_mag.innovation_rejected[2]) {
 		return false;
 	}

@@ -246,7 +239,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)

 		} else if (index == 1) {

-			// recalculate innovation variance becasue states and covariances have changed due to previous fusion
+			// recalculate innovation variance because states and covariances have changed due to previous fusion
 			const float HKY0 = magD*q1 + magE*q0 - magN*q3;
 			const float HKY1 = magD*q0 - magE*q1 + magN*q2;
 			const float HKY2 = magD*q3 + magE*q2 + magN*q1;
@@ -272,9 +265,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 			const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
 			const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);

-			_mag_innov_var(1) = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
+			aid_src_mag.innovation_variance[1] = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);

-			if (_mag_innov_var(1) < R_MAG) {
+			if (aid_src_mag.innovation_variance[1] < R_MAG) {
 				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 				_fault_status.flags.bad_mag_y = true;

@@ -283,7 +276,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
 				return false;
 			}
-			const float HKY24 = 1.0F/_mag_innov_var(1);
+			const float HKY24 = 1.0F/aid_src_mag.innovation_variance[1];

 			// Calculate Y axis observation jacobians
 			Hfusion.setZero();
@@ -352,9 +345,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 			const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
 			const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);

-			_mag_innov_var(2) = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
+			aid_src_mag.innovation_variance[2] = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);

-			if (_mag_innov_var(2) < R_MAG) {
+			if (aid_src_mag.innovation_variance[2] < R_MAG) {
 				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 				_fault_status.flags.bad_mag_z = true;

@@ -364,7 +357,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 				return false;
 			}

-			const float HKZ24 = 1.0F/_mag_innov_var(2);
+			const float HKZ24 = 1.0F/aid_src_mag.innovation_variance[2];

 			// calculate Z axis observation jacobians
 			Hfusion.setZero();
@@ -404,7 +397,16 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 			Kfusion(21) = HKZ23*HKZ24;
 		}

-		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index));
+		const bool is_fused = measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index]);
+
+		if (is_fused) {
+			aid_src_mag.fused[index] = true;
+			aid_src_mag.time_last_fuse[index] = _time_last_imu;
+
+		} else {
+			aid_src_mag.fused[index] = false;
+		}
+

 		switch (index) {
 		case 0:
@@ -425,17 +427,14 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 		}
 	}

-	if (!_fault_status.flags.bad_mag_x && !_fault_status.flags.bad_mag_y && !_fault_status.flags.bad_mag_z) {
-		_time_last_mag_3d_fuse = _time_last_imu;
-		return true;
-	}
-
-	return false;
+	return aid_src_mag.fused[0] && aid_src_mag.fused[1] && aid_src_mag.fused[2];
 }

 // update quaternion states and covariances using the yaw innovation and yaw observation variance
-bool Ekf::fuseYaw(const float innovation, const float variance)
+bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status)
 {
+	aid_src_status.innovation = innovation;
+
 	// assign intermediate state variables
 	const float q0 = _state.quat_nominal(0);
 	const float q1 = _state.quat_nominal(1);
@@ -551,7 +550,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance)

 	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
 	// calculate the innovation variance
-	_heading_innov_var = variance;
+	aid_src_status.innovation_variance = variance;

 	for (unsigned row = 0; row <= 3; row++) {
 		float tmp = 0.0f;
@@ -560,16 +559,16 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 			tmp += P(row, col) * H_YAW(col);
 		}

-		_heading_innov_var += H_YAW(row) * tmp;
+		aid_src_status.innovation_variance += H_YAW(row) * tmp;
 	}

-	float heading_innov_var_inv;
+	float heading_innov_var_inv = 0.f;

 	// check if the innovation variance calculation is badly conditioned
-	if (_heading_innov_var >= variance) {
+	if (aid_src_status.innovation_variance >= variance) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
 		_fault_status.flags.bad_hdg = false;
-		heading_innov_var_inv = 1.0f / _heading_innov_var;
+		heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;

 	} else {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
@@ -607,10 +606,10 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);

 	// innovation test ratio
-	_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
+	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);

 	// set the magnetometer unhealthy if the test fails
-	if (_yaw_test_ratio > 1.0f) {
+	if (aid_src_status.innovation_rejected) {
 		_innov_check_fail_status.flags.reject_yaw = true;

 		// if we are in air we don't want to fuse the measurement
@@ -618,13 +617,13 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 		// by interference or a large initial gyro bias
 		if (!_control_status.flags.in_air
 		&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
-		&& isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)
+		&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
 		) {
 			// constrain the innovation to the maximum set by the gate
 			// we need to delay this forced fusion to avoid starting it
 			// immediately after touchdown, when the drone is still armed
-			float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var));
-			_heading_innov = math::constrain(innovation, -gate_limit, gate_limit);
+			float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
+			aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);

 			// also reset the yaw gyro variance to converge faster and avoid
 			// being stuck on a previous bad estimate
@@ -636,7 +635,6 @@ bool Ekf::fuseYaw(const float innovation, const float variance)

 	} else {
 		_innov_check_fail_status.flags.reject_yaw = false;
-		_heading_innov = innovation;
 	}

 	// apply covariance correction via P_new = (I -K*H)*P
@@ -672,9 +670,11 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 		fixCovarianceErrors(true);

 		// apply the state corrections
-		fuse(Kfusion, _heading_innov);
+		fuse(Kfusion, aid_src_status.innovation);

 		_time_last_heading_fuse = _time_last_imu;
+		aid_src_status.time_last_fuse = _time_last_imu;
+		aid_src_status.fused = true;

 		return true;
 	}
@@ -1,25 +1,30 @@
 # PX4 Drag fusion parameter tuning algorithm
+
 In PX4, drag fusion can be enabled in order to estimate the wind when flying a multirotor, assuming that the body vertical acceleration is produced by the rotors and that the lateral forces are produced by drag.
 The model assumes a combination of:
-1. momentum drag: created by the rotors changing the direction of the incoming air, linear to the relative airspeed. Parameter `EKF2_MCOEF`
-2. bluff body drag: created by the wetted area of the aircraft, quadratic to the relative airspeed. Parameters `EKF2_BCOEF_X` and `EKF2_BCOEF_Y`
+1. **momentum drag:** created by the rotors changing the direction of the incoming air, linear to the relative airspeed.
+   Parameter [`EKF2_MCOEF`](https://docs.px4.io/master/en/advanced_config/parameter_reference.html#EKF2_MCOEF)
+2. **bluff body drag:** created by the [wetted area](https://en.wikipedia.org/wiki/Wetted_area) of the aircraft, quadratic to the relative airspeed.
+   Parameters [`EKF2_BCOEF_X`](https://docs.px4.io/master/en/advanced_config/parameter_reference.html#EKF2_BCOEF_X) and [`EKF2_BCOEF_Y`](https://docs.px4.io/master/en/advanced_config/parameter_reference.html#EKF2_BCOEF_Y)

 The python script was created to automate the tuning of the aforementioned parameters using flight test data.

 ## How to use this script
+
 First, a flight log with enough information is required in order to accurately estimate the parameters.
 The best way to do this is to fly the drone in altitude mode, accelerate to a moderate-high speed and let the drone slow-down by its own drag.
 Repeat the same maneuver in all directions and several times to obtain a good dataset.

-/!\ NOTE: the current state of this script assumes no wind. Some modifications are required to estimate both the wind and the parameters at the same time.
+> **NOTE:** the current state of this script assumes no wind. Some modifications are required to estimate both the wind and the parameters at the same time.

 Then, install the required python packages:
 ```
 pip install -r requirements.txt
 ```
+
 and run the script and give it the log file as an argument:
 ```
-python drag_replay.py <logfilename.ulg>
+python mc_wind_estimator_tuning.py <logfilename.ulg>
 ```

 The estimated parameters are displayed in the console and the fit quality is shown in a plot:
@@ -27,6 +32,8 @@ The estimated parameters are displayed in the console and the fit quality is sho
 param set EKF2_BCOEF_X 0.0
 param set EKF2_BCOEF_Y 62.1
 param set EKF2_MCOEF 0.16
-/!\EXPERIMENTAL param set EKF2_DRAG_NOISE 0.31
+
+# EXPERIMENTAL
+param set EKF2_DRAG_NOISE 0.31  
 ```
 ![DeepinScreenshot_matplotlib_20220329100027](https://user-images.githubusercontent.com/14822839/160563024-efddd100-d7db-46f7-8676-cf4296e9f737.png)
@@ -159,6 +159,102 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
 	}
 }

+void Ekf::updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const
+{
+	resetEstimatorAidStatus(vel_aid_src);
+
+	for (int i = 0; i < 3; i++) {
+		vel_aid_src.observation[i] = velocity(i);
+		vel_aid_src.observation_variance[i] = obs_var(i);
+
+		vel_aid_src.innovation[i] = _state.vel(i) - velocity(i);
+		vel_aid_src.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
+	}
+
+	setEstimatorAidStatusTestRatio(vel_aid_src, innov_gate);
+
+	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && vel_aid_src.innovation_rejected[2]) {
+		const float innov_limit = innov_gate * sqrtf(vel_aid_src.innovation_variance[2]);
+		vel_aid_src.innovation[2] = math::constrain(vel_aid_src.innovation[2], -innov_limit, innov_limit);
+		vel_aid_src.innovation_rejected[2] = false;
+	}
+
+	vel_aid_src.timestamp_sample = sample_time_us;
+}
+
+void Ekf::updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const
+{
+	resetEstimatorAidStatus(pos_aid_src);
+
+	for (int i = 0; i < 3; i++) {
+		pos_aid_src.observation[i] = position(i);
+		pos_aid_src.observation_variance[i] = obs_var(i);
+
+		pos_aid_src.innovation[i] = _state.pos(i) - position(i);
+		pos_aid_src.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
+	}
+
+	setEstimatorAidStatusTestRatio(pos_aid_src, innov_gate);
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && pos_aid_src.innovation_rejected[2]) {
+		const float innov_limit = innov_gate * sqrtf(pos_aid_src.innovation_variance[2]);
+		pos_aid_src.innovation[2] = math::constrain(pos_aid_src.innovation[2], -innov_limit, innov_limit);
+		pos_aid_src.innovation_rejected[2] = false;
+	}
+
+	pos_aid_src.timestamp_sample = sample_time_us;
+}
+
+void Ekf::fuseVelocity(estimator_aid_source_3d_s& vel_aid_src)
+{
+	// vx & vy
+	if (vel_aid_src.fusion_enabled[0] && !vel_aid_src.innovation_rejected[0]
+	 && vel_aid_src.fusion_enabled[1] && !vel_aid_src.innovation_rejected[1]
+	) {
+		for (int i = 0; i < 2; i++) {
+			if (fuseVelPosHeight(vel_aid_src.innovation[i], vel_aid_src.innovation_variance[i], i)) {
+				vel_aid_src.fused[i] = true;
+				vel_aid_src.time_last_fuse[i] = _time_last_imu;
+			}
+		}
+	}
+
+	// vz
+	if (vel_aid_src.fusion_enabled[2] && !vel_aid_src.innovation_rejected[2]) {
+		if (fuseVelPosHeight(vel_aid_src.innovation[2], vel_aid_src.innovation_variance[2], 2)) {
+			vel_aid_src.fused[2] = true;
+			vel_aid_src.time_last_fuse[2] = _time_last_imu;
+		}
+	}
+}
+
+void Ekf::fusePosition(estimator_aid_source_3d_s& pos_aid_src)
+{
+	// x & y
+	if (pos_aid_src.fusion_enabled[0] && !pos_aid_src.innovation_rejected[0]
+	 && pos_aid_src.fusion_enabled[1] && !pos_aid_src.innovation_rejected[1]
+	) {
+		for (int i = 0; i < 2; i++) {
+			if (fuseVelPosHeight(pos_aid_src.innovation[i], pos_aid_src.innovation_variance[i], 3 + i)) {
+				pos_aid_src.fused[i] = true;
+				pos_aid_src.time_last_fuse[i] = _time_last_imu;
+			}
+		}
+	}
+
+	// z
+	if (pos_aid_src.fusion_enabled[2] && !pos_aid_src.innovation_rejected[2]) {
+		if (fuseVelPosHeight(pos_aid_src.innovation[2], pos_aid_src.innovation_variance[2], 5)) {
+			pos_aid_src.fused[2] = true;
+			pos_aid_src.time_last_fuse[2] = _time_last_imu;
+		}
+	}
+}
+
 // Helper function that fuses a single velocity or position measurement
 bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
 {
@@ -47,7 +47,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 		// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
 		float innovation = 0.f;
 		float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
-		fuseYaw(innovation, obs_var);
+		estimator_aid_source_1d_s unused;
+		fuseYaw(innovation, obs_var, unused);

 		_last_static_yaw = NAN;

@@ -61,7 +62,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 			if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
 				float innovation = wrap_pi(euler_yaw - _last_static_yaw);
 				float obs_var = 0.01f;
-				fuseYaw(innovation, obs_var);
+				estimator_aid_source_1d_s unused;
+				fuseYaw(innovation, obs_var, unused);
 			}

 		} else {
@@ -76,7 +78,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 		if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
 			float innovation = 0.f;
 			float obs_var = 0.01f;
-			fuseYaw(innovation, obs_var);
+			estimator_aid_source_1d_s unused;
+			fuseYaw(innovation, obs_var, unused);
 		}

 		_last_static_yaw = NAN;
@@ -594,7 +594,7 @@ void EKF2::Run()
 			perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));

 			PublishLocalPosition(now);
-			PublishOdometry(now, imu_sample_new);
+			PublishOdometry(now);
 			PublishGlobalPosition(now);
 			PublishWindEstimate(now);

@@ -650,10 +650,22 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	// fake position
 	PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);

-	// GNSS velocity & position
+	// EV yaw
+	PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
+
+	// GNSS yaw/velocity/position
+	PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);

+	// mag heading
+	PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);
+
+	// mag 3d
+	PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
+
+	// aux velocity
+	PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
 }

 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -1047,72 +1059,43 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	_local_position_pub.publish(lpos);
 }

-void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
+void EKF2::PublishOdometry(const hrt_abstime &timestamp)
 {
 	// generate vehicle odometry data
 	vehicle_odometry_s odom;
 	odom.timestamp_sample = timestamp;

-	odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
+	// position
+	odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
+	_ekf.getPosition().copyTo(odom.position);

-	// Vehicle odometry position
-	const Vector3f position{_ekf.getPosition()};
-	odom.x = position(0);
-	odom.y = position(1);
-	odom.z = position(2);
-
-	// Vehicle odometry linear velocity
-	odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
-	const Vector3f velocity{_ekf.getVelocity()};
-	odom.vx = velocity(0);
-	odom.vy = velocity(1);
-	odom.vz = velocity(2);
-
-	// Vehicle odometry quaternion
+	// orientation quaternion
 	_ekf.getQuaternion().copyTo(odom.q);

-	// Vehicle odometry angular rates
-	const Vector3f gyro_bias{_ekf.getGyroBias()};
-	const Vector3f rates{imu.delta_ang / imu.delta_ang_dt};
-	odom.rollspeed = rates(0) - gyro_bias(0);
-	odom.pitchspeed = rates(1) - gyro_bias(1);
-	odom.yawspeed = rates(2) - gyro_bias(2);
+	// velocity
+	odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
+	_ekf.getVelocity().copyTo(odom.velocity);

-	// get the covariance matrix size
-	static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
-	static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
+	// angular_velocity
+	const Vector3f rates{_ekf.get_imu_sample_newest().delta_ang / _ekf.get_imu_sample_newest().delta_ang_dt};
+	const Vector3f angular_velocity = rates - _ekf.getGyroBias();
+	angular_velocity.copyTo(odom.angular_velocity);

-	// Get covariances to vehicle odometry
-	float covariances[24];
-	_ekf.covariances_diagonal().copyTo(covariances);
+	// velocity covariances
+	_ekf.velocity_covariances().diag().copyTo(odom.velocity_variance);

-	// initially set pose covariances to 0
-	for (size_t i = 0; i < POS_URT_SIZE; i++) {
-		odom.pose_covariance[i] = 0.0;
-	}
+	// position covariances
+	_ekf.position_covariances().diag().copyTo(odom.position_variance);

-	// set the position variances
-	odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
-	odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
-	odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
-
-	// TODO: implement propagation from quaternion covariance to Euler angle covariance
-	// by employing the covariance law
-
-	// initially set velocity covariances to 0
-	for (size_t i = 0; i < VEL_URT_SIZE; i++) {
-		odom.velocity_covariance[i] = 0.0;
-	}
-
-	// set the linear velocity variances
-	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
-	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
-	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
+	// orientation covariance
+	_ekf.orientation_covariances_euler().diag().copyTo(odom.orientation_variance);

 	odom.reset_counter = _ekf.get_quat_reset_count()
 			     + _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
 			     + _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count();

+	odom.quality = 0;
+
 	// publish vehicle odometry data
 	odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_odometry_pub.publish(odom);
@@ -1126,37 +1109,30 @@ void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_od
 	vehicle_odometry_s aligned_ev_odom{ev_odom};

 	// Rotate external position and velocity into EKF navigation frame
-	const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z);
-	aligned_ev_odom.x = aligned_pos(0);
-	aligned_ev_odom.y = aligned_pos(1);
-	aligned_ev_odom.z = aligned_pos(2);
+	const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.position);
+	aligned_pos.copyTo(aligned_ev_odom.position);

 	switch (ev_odom.velocity_frame) {
-	case vehicle_odometry_s::BODY_FRAME_FRD: {
-			const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
-			aligned_ev_odom.vx = aligned_vel(0);
-			aligned_ev_odom.vy = aligned_vel(1);
-			aligned_ev_odom.vz = aligned_vel(2);
+	case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: {
+			const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.velocity);
+			aligned_vel.copyTo(aligned_ev_odom.velocity);
 			break;
 		}

-	case vehicle_odometry_s::LOCAL_FRAME_FRD: {
-			const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
-			aligned_ev_odom.vx = aligned_vel(0);
-			aligned_ev_odom.vy = aligned_vel(1);
-			aligned_ev_odom.vz = aligned_vel(2);
+	case vehicle_odometry_s::VELOCITY_FRAME_FRD: {
+			const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.velocity);
+			aligned_vel.copyTo(aligned_ev_odom.velocity);
 			break;
 		}
 	}

-	aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
+	aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;

 	// Compute orientation in EKF navigation frame
-	Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q) ;
+	Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q);
 	ev_quat_aligned.normalize();

 	ev_quat_aligned.copyTo(aligned_ev_odom.q);
-	quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);

 	aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
@@ -1368,9 +1344,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_ver_vel                  = _ekf.innov_check_fail_status_flags().reject_ver_vel;
 		status_flags.reject_hor_pos                  = _ekf.innov_check_fail_status_flags().reject_hor_pos;
 		status_flags.reject_ver_pos                  = _ekf.innov_check_fail_status_flags().reject_ver_pos;
-		status_flags.reject_mag_x                    = _ekf.innov_check_fail_status_flags().reject_mag_x;
-		status_flags.reject_mag_y                    = _ekf.innov_check_fail_status_flags().reject_mag_y;
-		status_flags.reject_mag_z                    = _ekf.innov_check_fail_status_flags().reject_mag_z;
 		status_flags.reject_yaw                      = _ekf.innov_check_fail_status_flags().reject_yaw;
 		status_flags.reject_airspeed                 = _ekf.innov_check_fail_status_flags().reject_airspeed;
 		status_flags.reject_sideslip                 = _ekf.innov_check_fail_status_flags().reject_sideslip;
@@ -1557,8 +1530,8 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 			// velocity of vehicle relative to target has opposite sign to target relative to vehicle
 			auxVelSample auxvel_sample{
 				.time_us = landing_target_pose.timestamp,
-				.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f},
-				.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f},
+				.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, NAN},
+				.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, NAN},
 			};
 			_ekf.setAuxVelData(auxvel_sample);
 		}
@@ -1627,77 +1600,122 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		}

 		extVisionSample ev_data{};
+		ev_data.pos.setNaN();
+		ev_data.vel.setNaN();
+		ev_data.quat.setNaN();

 		// if error estimates are unavailable, use parameter defined defaults

 		// check for valid velocity data
-		if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
-			ev_data.vel(0) = ev_odom.vx;
-			ev_data.vel(1) = ev_odom.vy;
-			ev_data.vel(2) = ev_odom.vz;
+		if (PX4_ISFINITE(ev_odom.velocity[0]) && PX4_ISFINITE(ev_odom.velocity[1]) && PX4_ISFINITE(ev_odom.velocity[2])) {
+			bool velocity_valid = true;

-			if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
-				ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
+			switch (ev_odom.velocity_frame) {
+			// case vehicle_odometry_s::VELOCITY_FRAME_NED:
+			// 	ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
+			// 	break;

-			} else {
+			case vehicle_odometry_s::VELOCITY_FRAME_FRD:
 				ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
+				break;
+
+			case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD:
+				ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
+				break;
+
+			default:
+				velocity_valid = false;
+				break;
 			}

-			// velocity measurement error from ev_data or parameters
-			float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
+			if (velocity_valid) {
+				ev_data.vel(0) = ev_odom.velocity[0];
+				ev_data.vel(1) = ev_odom.velocity[1];
+				ev_data.vel(2) = ev_odom.velocity[2];

-			if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
-			    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
-			    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
-				ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
-				ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1];
-				ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2];
-				ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
-				ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
-				ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
+				const float evv_noise_var = sq(_param_ekf2_evv_noise.get());

-			} else {
-				ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
+				// velocity measurement error from ev_data or parameters
+				if (!_param_ekf2_ev_noise_md.get() &&
+				    PX4_ISFINITE(ev_odom.velocity_variance[0]) &&
+				    PX4_ISFINITE(ev_odom.velocity_variance[1]) &&
+				    PX4_ISFINITE(ev_odom.velocity_variance[2])) {
+
+					ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]);
+					ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]);
+					ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]);
+
+				} else {
+					ev_data.velVar.setAll(evv_noise_var);
+				}
+
+				new_ev_odom = true;
 			}
-
-			new_ev_odom = true;
 		}

 		// check for valid position data
-		if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
-			ev_data.pos(0) = ev_odom.x;
-			ev_data.pos(1) = ev_odom.y;
-			ev_data.pos(2) = ev_odom.z;
+		if (PX4_ISFINITE(ev_odom.position[0]) && PX4_ISFINITE(ev_odom.position[1]) && PX4_ISFINITE(ev_odom.position[2])) {

-			float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
+			bool position_valid = true;

-			// position measurement error from ev_data or parameters
-			if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
-			    && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
-			    && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
-				ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
-				ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
-				ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
+			// switch (ev_odom.pose_frame) {
+			// case vehicle_odometry_s::POSE_FRAME_NED:
+			// 	ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
+			// 	break;

-			} else {
-				ev_data.posVar.setAll(param_evp_noise_var);
+			// case vehicle_odometry_s::POSE_FRAME_FRD:
+			// 	ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
+			// 	break;
+
+			// default:
+			// 	position_valid = false;
+			// 	break;
+			// }
+
+			if (position_valid) {
+				ev_data.pos(0) = ev_odom.position[0];
+				ev_data.pos(1) = ev_odom.position[1];
+				ev_data.pos(2) = ev_odom.position[2];
+
+				const float evp_noise_var = sq(_param_ekf2_evp_noise.get());
+
+				// position measurement error from ev_data or parameters
+				if (!_param_ekf2_ev_noise_md.get() &&
+				    PX4_ISFINITE(ev_odom.position_variance[0]) &&
+				    PX4_ISFINITE(ev_odom.position_variance[1]) &&
+				    PX4_ISFINITE(ev_odom.position_variance[2])
+				   ) {
+					ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]);
+					ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]);
+					ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]);
+
+				} else {
+					ev_data.posVar.setAll(evp_noise_var);
+				}
+
+				new_ev_odom = true;
 			}
-
-			new_ev_odom = true;
 		}

 		// check for valid orientation data
-		if (PX4_ISFINITE(ev_odom.q[0])) {
+		if ((PX4_ISFINITE(ev_odom.q[0]) && PX4_ISFINITE(ev_odom.q[1])
+		     && PX4_ISFINITE(ev_odom.q[2]) && PX4_ISFINITE(ev_odom.q[3]))
+		    && ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f)
+			|| (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f))
+		   ) {
 			ev_data.quat = Quatf(ev_odom.q);
+			ev_data.quat.normalize();

 			// orientation measurement error from ev_data or parameters
-			float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
+			const float eva_noise_var = sq(_param_ekf2_eva_noise.get());

-			if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
-				ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
+			if (!_param_ekf2_ev_noise_md.get() &&
+			    PX4_ISFINITE(ev_odom.orientation_variance[2])
+			   ) {
+				ev_data.angVar = fmaxf(eva_noise_var, ev_odom.orientation_variance[2]);

 			} else {
-				ev_data.angVar = param_eva_noise_var;
+				ev_data.angVar = eva_noise_var;
 			}

 			new_ev_odom = true;
@@ -1705,6 +1723,8 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo

 		// use timestamp from external computer, clocks are synchronized when using MAVROS
 		ev_data.time_us = ev_odom.timestamp_sample;
+		ev_data.reset_counter = ev_odom.reset_counter;
+		//ev_data.quality = ev_odom.quality;

 		if (new_ev_odom)  {
 			_ekf.setExtVisionData(ev_data);
@@ -2240,7 +2260,7 @@ int EKF2::print_usage(const char *reason)
 ### Description
 Attitude and position estimator using an Extended Kalman Filter. It is used for Multirotors and Fixed-Wing.

-The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html) page.
+The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf.html) page.

 ekf2 can be started in replay mode (`-r`): in this mode, it does not access the system time, but only uses the
 timestamps from the sensor topics.
@@ -144,7 +144,7 @@ private:
 	void PublishInnovationTestRatios(const hrt_abstime &timestamp);
 	void PublishInnovationVariances(const hrt_abstime &timestamp);
 	void PublishLocalPosition(const hrt_abstime &timestamp);
-	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
+	void PublishOdometry(const hrt_abstime &timestamp);
 	void PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom);
 	void PublishOpticalFlowVel(const hrt_abstime &timestamp);
 	void PublishSensorBias(const hrt_abstime &timestamp);
@@ -257,9 +257,17 @@ private:

 	hrt_abstime _status_fake_pos_pub_last{0};

+	hrt_abstime _status_ev_yaw_pub_last{0};
+
+	hrt_abstime _status_gnss_yaw_pub_last{0};
 	hrt_abstime _status_gnss_vel_pub_last{0};
 	hrt_abstime _status_gnss_pos_pub_last{0};

+	hrt_abstime _status_mag_pub_last{0};
+	hrt_abstime _status_mag_heading_pub_last{0};
+
+	hrt_abstime _status_aux_vel_pub_last{0};
+
 	float _last_baro_bias_published{};

 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
@@ -324,9 +332,17 @@ private:

 	uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};

+	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
 	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
 	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};

+	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
+	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
+
+	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
+
+	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
+
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;
@@ -26,12 +26,7 @@ void Vio::setData(const extVisionSample &vio_data)

 void Vio::setVelocityVariance(const Vector3f &velVar)
 {
-	setVelocityCovariance(matrix::diag(velVar));
-}
-
-void Vio::setVelocityCovariance(const Matrix3f &velCov)
-{
-	_vio_data.velCov = velCov;
+	_vio_data.velVar = velVar;
 }

 void Vio::setPositionVariance(const Vector3f &posVar)
@@ -76,7 +71,7 @@ extVisionSample Vio::dataAtRest()
 	vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
 	vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
 	vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
-	vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
+	vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
 	vio_data.angVar = 0.05f;
 	vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
 	return vio_data;
@@ -53,7 +53,6 @@ public:

 	void setData(const extVisionSample &vio_data);
 	void setVelocityVariance(const Vector3f &velVar);
-	void setVelocityCovariance(const Matrix3f &velCov);
 	void setPositionVariance(const Vector3f &posVar);
 	void setAngularVariance(float angVar);
 	void setVelocity(const Vector3f &vel);
@@ -223,7 +223,7 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
 	_sensor_simulator.setGpsLatitude(_latitude_new);
 	_sensor_simulator.setGpsLongitude(_longitude_new);
 	_sensor_simulator.setGpsAltitude(_altitude_new);
-	_sensor_simulator.runSeconds(2);
+	_sensor_simulator.runSeconds(5);

 	_ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude);

@@ -231,8 +231,9 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
 	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
 	EXPECT_NEAR(_altitude, _altitude_new, 0.01f);

-	_latitude_new  = -15.0000005;
-	_longitude_new = -115.0000005;
+	// Note: we cannot reset too far since the local position is limited to 1e6m
+	_latitude_new  = 14.0000005;
+	_longitude_new = 109.0000005;
 	_altitude_new  = 1500.0;

 	_ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new);
@@ -242,16 +243,21 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
 	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
 	EXPECT_FLOAT_EQ(_altitude, _altitude_new);

+	_sensor_simulator.runSeconds(1);
+
 	float hpos = 0.f;
 	float vpos = 0.f;
 	float hvel = 0.f;
 	float vvel = 0.f;
+	float baro_vpos = 0.f;

 	// After the change of origin, the pos and vel innovations should stay small
 	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
+	_ekf->getBaroHgtInnovRatio(baro_vpos);

 	EXPECT_NEAR(hpos, 0.f, 0.05f);
 	EXPECT_NEAR(vpos, 0.f, 0.05f);
+	EXPECT_NEAR(baro_vpos, 0.f, 0.05f);

 	EXPECT_NEAR(hvel, 0.f, 0.02f);
 	EXPECT_NEAR(vvel, 0.f, 0.02f);
@@ -276,6 +282,8 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
 	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
 	EXPECT_FLOAT_EQ(_altitude, _altitude_new);

+	_sensor_simulator.runSeconds(1);
+
 	float hpos = 0.f;
 	float vpos = 0.f;
 	float hvel = 0.f;
@@ -276,13 +276,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody)
 	// WHEN: measurement is given in BODY-FRAME and
 	//       x variance is bigger than y variance
 	_sensor_simulator._vio.setVelocityFrameToBody();
-	float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
-				  0.0f, 0.01f, 0.0f,
-				  0.0f, 0.0f, 0.01f
-				 };
-	const Matrix3f vel_cov_body(vel_cov_data);
+
+	const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
 	const Vector3f vel_body(1.0f, 0.0f, 0.0f);
-	_sensor_simulator._vio.setVelocityCovariance(vel_cov_body);
+	_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
 	_sensor_simulator._vio.setVelocity(vel_body);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
@@ -312,13 +309,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
 	// WHEN: measurement is given in LOCAL-FRAME and
 	//       x variance is bigger than y variance
 	_sensor_simulator._vio.setVelocityFrameToLocal();
-	float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
-				  0.0f, 0.01f, 0.0f,
-				  0.0f, 0.0f, 0.01f
-				 };
-	const Matrix3f vel_cov_earth(vel_cov_data);
+
+	const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f};
 	const Vector3f vel_earth(1.0f, 0.0f, 0.0f);
-	_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
+	_sensor_simulator._vio.setVelocityVariance(vel_cov_earth);
 	_sensor_simulator._vio.setVelocity(vel_earth);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
@@ -342,12 +342,11 @@ bool FlightTaskAuto::_evaluateTriplets()
 	const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed;

 	if (PX4_ISFINITE(cruise_speed_from_triplet)
-	    && (cruise_speed_from_triplet > 0.f)
 	    && (_sub_triplet_setpoint.get().current.timestamp > _time_last_cruise_speed_override)) {
 		_mc_cruise_speed = cruise_speed_from_triplet;
 	}

-	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
+	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < FLT_EPSILON)) {
 		// If no speed is planned use the default cruise speed as limit
 		_mc_cruise_speed = _param_mpc_xy_cruise.get();
 	}
@@ -34,6 +34,7 @@

 #pragma once

+#include <math.h>
 #include <stdint.h>

 namespace gimbal
@@ -55,8 +56,8 @@ struct ControlData {

 	union TypeData {
 		struct TypeAngle {
-			float q[4];
-			float angular_velocity[3];
+			float q[4] {NAN, NAN, NAN, NAN};
+			float angular_velocity[3] {NAN, NAN, NAN};

 			enum class Frame : uint8_t {
 				AngleBodyFrame = 0, // Also called follow mode, angle relative to vehicle forward (usually default for yaw axis).
@@ -539,7 +539,7 @@ static void usage()
 Mount/gimbal Gimbal control driver. It maps several different input methods (eg. RC or MAVLink) to a configured
 output (eg. AUX channels or MAVLink).

-Documentation how to use it is on the [gimbal_control](https://docs.px4.io/master/en/advanced/gimbal_control.html) page.
+Documentation how to use it is on the [gimbal_control](https://docs.px4.io/main/en/advanced/gimbal_control.html) page.

 ### Examples
 Test the output by setting a angles (all omitted axes are set to 0):
@@ -100,7 +100,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData
 	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
 	control_data.type = ControlData::Type::Angle;

-	float new_aux_values[3];
+	float new_aux_values[3] {};

 	for (int i = 0; i < 3; ++i) {
 		new_aux_values[i] = _get_aux_value(manual_control_setpoint, i);
@@ -112,7 +112,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData
 	// Detect a big stick movement
 	const bool major_movement = [&]() {
 		for (int i = 0; i < 3; ++i) {
-			if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
+			if (!PX4_ISFINITE(_last_set_aux_values[i]) || (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f)) {
 				return true;
 			}
 		}
@@ -61,7 +61,7 @@ private:

 	int _manual_control_setpoint_sub{-1};

-	float _last_set_aux_values[3] {};
+	float _last_set_aux_values[3] {NAN, NAN, NAN};
 };

 } /* namespace gimbal */
@@ -43,6 +43,8 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>

+using namespace time_literals;
+
 namespace gimbal
 {

@@ -84,9 +86,7 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 void OutputBase::_set_angle_setpoints(const ControlData &control_data)
 {
 	switch (control_data.type) {
-	case ControlData::Type::Angle:
-
-		{
+	case ControlData::Type::Angle: {
 			for (int i = 0; i < 3; ++i) {
 				switch (control_data.type_data.angle.frames[i]) {
 				case ControlData::TypeData::TypeAngle::Frame::AngularRate:
@@ -104,9 +104,7 @@ void OutputBase::_set_angle_setpoints(const ControlData &control_data)
 				_angle_velocity[i] = control_data.type_data.angle.angular_velocity[i];
 			}

-			for (int i = 0; i < 4; ++i) {
-				_q_setpoint[i] = control_data.type_data.angle.q[i];
-			}
+			_q_setpoint = matrix::Quatf{control_data.type_data.angle.q};
 		}

 		break;
@@ -116,10 +114,10 @@ void OutputBase::_set_angle_setpoints(const ControlData &control_data)
 		break;

 	case ControlData::Type::Neutral:
-		_q_setpoint[0] = 1.f;
-		_q_setpoint[1] = 0.f;
-		_q_setpoint[2] = 0.f;
-		_q_setpoint[3] = 0.f;
+		_q_setpoint(0) = 1.f;
+		_q_setpoint(1) = 0.f;
+		_q_setpoint(2) = 0.f;
+		_q_setpoint(3) = 0.f;
 		_angle_velocity[0] = NAN;
 		_angle_velocity[1] = NAN;
 		_angle_velocity[2] = NAN;
@@ -173,7 +171,7 @@ void OutputBase::_handle_position_update(const ControlData &control_data, bool f
 		yaw += control_data.type_data.lonlat.yaw_offset;
 	}

-	matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint);
+	_q_setpoint = matrix::Eulerf(roll, pitch, yaw);

 	_angle_velocity[0] = NAN;
 	_angle_velocity[1] = NAN;
@@ -201,25 +199,41 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 	}

 	// get the output angles and stabilize if necessary
+	bool attitude_valid = false;
 	matrix::Eulerf euler_vehicle{};

 	if (compensate[0] || compensate[1] || compensate[2]) {
 		vehicle_attitude_s vehicle_attitude;

 		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
-			euler_vehicle = matrix::Quatf(vehicle_attitude.q);
+			const matrix::Quatf q{vehicle_attitude.q};
+
+			const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f)
+								&& (fabsf(q(1)) <= 1.f)
+								&& (fabsf(q(2)) <= 1.f)
+								&& (fabsf(q(3)) <= 1.f);
+			const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f);
+
+			attitude_valid = (vehicle_attitude.timestamp != 0) && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 1_s)
+					 && norm_in_tolerance && no_element_larger_than_one;
+
+			if (attitude_valid) {
+				euler_vehicle = q;
+			}
 		}
 	}

 	float dt = math::constrain((t - _last_update) * 1.e-6f, 0.001f, 1.f);

-	const bool q_setpoint_valid = PX4_ISFINITE(_q_setpoint[0]) && PX4_ISFINITE(_q_setpoint[1])
-				      && PX4_ISFINITE(_q_setpoint[2]) && PX4_ISFINITE(_q_setpoint[3]);
-	matrix::Eulerf euler_gimbal{};
+	const bool q_setpoint_no_element_larger_than_one = (fabsf(_q_setpoint(0)) <= 1.f)
+			&& (fabsf(_q_setpoint(1)) <= 1.f)
+			&& (fabsf(_q_setpoint(2)) <= 1.f)
+			&& (fabsf(_q_setpoint(3)) <= 1.f);
+	const bool q_setpoint_norm_in_tolerance = (fabsf(1.f - _q_setpoint.norm()) <= 1e-6f);

-	if (q_setpoint_valid) {
-		euler_gimbal = matrix::Quatf{_q_setpoint};
-	}
+	const bool q_setpoint_valid = q_setpoint_no_element_larger_than_one && q_setpoint_norm_in_tolerance;
+
+	const matrix::Eulerf euler_gimbal{_q_setpoint};

 	for (int i = 0; i < 3; ++i) {

@@ -231,17 +245,22 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 			_angle_outputs[i] += dt * _angle_velocity[i];
 		}

-		if (compensate[i] && PX4_ISFINITE(euler_vehicle(i))) {
+		if (compensate[i] && PX4_ISFINITE(_angle_outputs[i]) &&
+		    attitude_valid && PX4_ISFINITE(euler_vehicle(i))) {
+
 			_angle_outputs[i] -= euler_vehicle(i);
 		}

 		if (PX4_ISFINITE(_angle_outputs[i])) {
 			// bring angles into proper range [-pi, pi]
 			_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]);
+
+		} else {
+			// reset
+			_angle_outputs[i] = 0.f;
 		}
 	}

-
 	// constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed
 	if (_landed) {
 		if (PX4_ISFINITE(_angle_outputs[1])) {
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	672051c34a	gimbal: add paranoid quaternion checks and initialized values	2022-08-04 17:19:19 -04: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