github: test file changed action

docker: pass on CI env variables
ubuntu: alphabetize list
2026-06-06 13:10:05 +08:00 · 2022-08-16 10:01:40 -07:00 · 2022-08-16 09:36:46 -07:00 · 2022-08-16 09:36:46 -07:00 · 2022-08-16 09:36:46 -07:00 · 2022-08-16 09:36:04 -07:00
125 changed files with 1244 additions and 2040 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/main/en/getting_started/flight_reporting.html)).
+- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/master/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)

@@ -0,0 +1,14 @@
+name: docker-dev
+on: [push, pull_request]
+jobs:
+  changes:
+    runs-on: ubuntu-latest
+    steps:
+      - id: file_changes
+        uses: trilom/file-changes-action@v1.2.3
+      - name: test
+        run: |
+          echo '${{ steps.file_changes.outputs.files}}'
+          echo '${{ steps.file_changes.outputs.files_modified}}'
+          echo '${{ steps.file_changes.outputs.files_added}}'
+          echo '${{ steps.file_changes.outputs.files_removed}}'
@@ -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 main for new features.
+*Always* branch off master for new features.

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

 ### Edit and build the code

-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.
+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.

 ### Commit your changes

@@ -7,8 +7,7 @@ pipeline {
    stage('Analysis') {
      when {
        anyOf {
-          branch 'main'
-          branch 'master' // should be removed, but in case there is something going on...
+          branch 'master'
          branch 'pr-jenkins' // for testing
        }
      }
@@ -205,21 +204,20 @@ 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 main || 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 master || true')
+              sh('rm -rf px4_user_guide')
            }
          }
          when {
            anyOf {
-              branch 'main'
-              branch 'master' // should be removed, but in case there is something going on...
+              branch 'master'
              branch 'pr-jenkins' // for testing
            }
          }
@@ -247,8 +245,7 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'main'
-              branch 'master' // should be removed, but in case there is something going on...
+              branch 'master'
              branch 'pr-jenkins' // for testing
            }
          }
@@ -281,8 +278,7 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'main'
-              branch 'master' // should be removed, but in case there is something going on...
+              branch 'master'
              branch 'pr-jenkins' // for testing
            }
          }
@@ -311,8 +307,7 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'main'
-              branch 'master' // should be removed, but in case there is something going on...
+              branch 'master'
              branch 'pr-jenkins' // for testing
            }
          }
@@ -355,8 +350,7 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'main'
-              branch 'master' // should be removed, but in case there is something going on...
+              branch 'master'
              branch 'pr-jenkins' // for testing
            }
          }
@@ -379,8 +373,7 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'main'
-              branch 'master' // should be removed, but in case there is something going on...
+              branch 'master'
              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/main/LICENSE))
-* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](https://px4.io/ecosystem/commercial-systems/)):
+* 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)):
  * [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/main/en/frames_plane/wing_wing_z84.html
+# @url https://docs.px4.io/master/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/main/en/frames_plane/wing_wing_z84.html
+# @url https://docs.px4.io/master/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/main/en/frames_multicopter/spedix_s250_pixracer.html
+# @url https://docs.px4.io/master/en/frames_multicopter/spedix_s250_pixracer.html
 #
 # @type Quadrotor asymmetric
 # @class Copter
@@ -2,7 +2,7 @@
 #
 # @name HolyBro QAV250
 #
-# @url https://docs.px4.io/main/en/frames_multicopter/holybro_qav250_pixhawk4_mini.html
+# @url https://docs.px4.io/master/en/frames_multicopter/holybro_qav250_pixhawk4_mini.html
 #
 # @type Quadrotor x
 # @class Copter
@@ -0,0 +1,21 @@
+#!/bin/bash
+
+if [[ -z "${DOCKER_TAG}" ]]; then
+  # the default tag for docker images
+  # follows the pattern below, and we recommend
+  # that any images pushed to the registry continue
+  # to use the pattern for consistency
+  TAG_NAME="`date +"%Y-%m-%d"`"
+else
+  TAG_NAME="${DOCKER_TAG}"
+fi
+
+PX4_DOCKER_REPO="ghcr.io/px4/px4-dev:$TAG_NAME"
+PWD=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+SRC_DIR=$PWD/../
+
+echo "[docker_build.sh]: Building [$PX4_DOCKER_REPO]"
+
+docker build \
+  -t ${PX4_DOCKER_REPO} \
+  -f Tools/setup/Dockerfile "${SRC_DIR}"
@@ -1,67 +1,29 @@
-#! /bin/bash
+#!/bin/bash

-if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	echo "guessing PX4_DOCKER_REPO based on input";
-	if [[ $@ =~ .*px4_fmu.* ]]; then
-		# nuttx-px4fmu-v{1,2,3,4,5}
-		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
-	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
-		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
-	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
-		# scumaker_pilotpi_arm64
-		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest"
-	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
-		# posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
-	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
-		# clang tools
-		PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04"
-	elif [[ $@ =~ .*tests* ]]; then
-		# run all tests with simulation
-		PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2021-12-11"
-	fi
+if [[ -z "${DOCKER_TAG}" ]]; then
+  # The default tag name should be hardcoded
+  # to whatever we are currently using in CI on this branch
+  TAG_NAME="2022-08-16"
 else
-	echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'";
+  TAG_NAME="${DOCKER_TAG}"
 fi

-# otherwise default to nuttx
-if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
-fi
+PX4_DOCKER_REPO="ghcr.io/px4/px4-dev:$TAG_NAME"

-# docker hygiene
-
-#Delete all stopped containers (including data-only containers)
-#docker rm $(docker ps -a -q)
-
-#Delete all 'untagged/dangling' (<none>) images
-#docker rmi $(docker images -q -f dangling=true)
-
-echo "PX4_DOCKER_REPO: $PX4_DOCKER_REPO";
+echo "[docker_run.sh]: Running '$PX4_DOCKER_REPO'"

 PWD=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
 SRC_DIR=$PWD/../

-CCACHE_DIR=${HOME}/.ccache
-mkdir -p "${CCACHE_DIR}"
-
 docker run -it --rm -w "${SRC_DIR}" \
-	--env=AWS_ACCESS_KEY_ID \
-	--env=AWS_SECRET_ACCESS_KEY \
-	--env=BRANCH_NAME \
-	--env=CCACHE_DIR="${CCACHE_DIR}" \
-	--env=CI \
-	--env=CODECOV_TOKEN \
-	--env=COVERALLS_REPO_TOKEN \
-	--env=LOCAL_USER_ID="$(id -u)" \
-	--env=PX4_ASAN \
-	--env=PX4_MSAN \
-	--env=PX4_TSAN \
-	--env=PX4_UBSAN \
-	--env=TRAVIS_BRANCH \
-	--env=TRAVIS_BUILD_ID \
-	--publish 14556:14556/udp \
-	--volume=${CCACHE_DIR}:${CCACHE_DIR}:rw \
-	--volume=${SRC_DIR}:${SRC_DIR}:rw \
-	${PX4_DOCKER_REPO} /bin/bash -c "$1 $2 $3"
+  --env=LOCAL_USER_ID="$(id -u)" \
+  --env=AWS_ACCESS_KEY_ID \
+  --env=AWS_SECRET_ACCESS_KEY \
+  --env=BRANCH_NAME \
+  --env=CCACHE_DIR="${CCACHE_DIR}" \
+  --env=CI \
+  --publish 14556:14556/udp \
+  --volume=/tmp/.X11-unix:/tmp/.X11-unix \
+  --volume=/tmp:/tmp:rw \
+  --volume=${SRC_DIR}:${SRC_DIR}:rw \
+  ${PX4_DOCKER_REPO} /bin/bash -c "$1 $2 $3"
@@ -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/main/en/config/actuators.html',
+                'help-url': 'https://docs.px4.io/master/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/main/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/master/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/main/src/%s)\n\n" % (module.scope(), module.scope())
+            result += "Source: [%s](https://github.com/PX4/PX4-Autopilot/tree/master/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/PX4-user_guide/blob/main/en/SUMMARY.md
+    # TOC in https://github.com/PX4/Devguide/blob/master/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/main/en/flight_controller/silicon_errata.html")
+                               "See https://docs.px4.io/master/en/flight_controller/silicon_errata.html")
        except RuntimeError:
            # timeout, no response yet
            return False
@@ -0,0 +1,51 @@
+#
+# PX4 base development environment
+#
+
+FROM ubuntu:20.04
+LABEL maintainer="Daniel Agar <daniel@agar.ca>, Ramon Roche <mrpollo@gmail.com>"
+
+ENV DEBIAN_FRONTEND noninteractive
+ENV LANG C.UTF-8
+ENV LC_ALL C.UTF-8
+
+# Installing required utilities
+RUN apt-get update && apt-get -y --quiet --no-install-recommends install \
+  ca-certificates \
+  gnupg \
+  lsb-core \
+  lsb-release \
+  sudo \
+  software-properties-common \
+  wget \
+  gosu \
+  ;
+
+# Install PX4 Requirements
+COPY Tools/setup/requirements.txt /tmp/requirements.txt
+COPY Tools/setup/ubuntu.sh /tmp/ubuntu.sh
+# The PATH env is set within ubuntu.sh, but given how we
+# are running the image using `gosu` to avoid read-only problems
+# with the filesystem the env variable does not persist
+ENV PATH="/opt/gcc-arm-none-eabi-9-2020-q2-update/bin:$PATH"
+
+RUN bash /tmp/ubuntu.sh --from-docker
+
+ENV DISPLAY :99
+
+ENV FASTRTPSGEN_DIR="/usr/local/bin/"
+ENV TERM=xterm
+ENV TZ=UTC
+
+# SITL UDP PORTS
+EXPOSE 14556/udp
+EXPOSE 14557/udp
+
+# create user with id 1001 (jenkins docker workflow default)
+RUN useradd --shell /bin/bash -u 1001 -c "" -m user && usermod -a -G dialout user
+
+# create and start as LOCAL_USER_ID
+COPY Tools/setup/docker-entrypoint.sh /usr/local/bin/entrypoint.sh
+ENTRYPOINT ["/usr/local/bin/entrypoint.sh"]
+
+CMD ["/bin/bash"]
@@ -0,0 +1,29 @@
+#!/bin/bash
+
+echo "[docker-entrypoint.sh] Starting entrypoint"
+# Start virtual X server in the background
+# - DISPLAY default is :99, set in dockerfile
+# - Users can override with `-e DISPLAY=` in `docker run` command to avoid
+#   running Xvfb and attach their screen
+if [[ -x "$(command -v Xvfb)" && "$DISPLAY" == ":99" ]]; then
+	echo "[docker-entrypoint.sh] Starting Xvfb"
+	Xvfb :99 -screen 0 1600x1200x24+32 &
+fi
+
+# Check if the ROS_DISTRO is passed and use it
+# to source the ROS environment
+if [ -n "${ROS_DISTRO}" ]; then
+	source "/opt/ros/$ROS_DISTRO/setup.bash"
+fi
+
+# Use the LOCAL_USER_ID if passed in at runtime
+if [ -n "${LOCAL_USER_ID}" ]; then
+	echo "[docker-entrypoint.sh] Starting with UID : $LOCAL_USER_ID"
+	# modify existing user's id
+	usermod -u $LOCAL_USER_ID user
+
+	# run as user
+  exec gosu user "$@"
+else
+	exec "$@"
+fi
@@ -2,20 +2,18 @@

 set -e

-## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
+## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04).
 ## Can also be used in docker.
 ##
 ## Installs:
-## - Common dependencies and tools for nuttx, jMAVSim, Gazebo
+## - Common dependencies and tools for NuttX, and Gazebo
 ## - NuttX toolchain (omit with arg: --no-nuttx)
-## - jMAVSim and Gazebo9 simulator (omit with arg: --no-sim-tools)
-##
-## Not Installs:
-## - FastRTPS and FastCDR
+## - Gazebo simulator (omit with arg: --no-sim-tools)

 INSTALL_NUTTX="true"
 INSTALL_SIM="true"
 INSTALL_ARCH=`uname -m`
+INSIDE_DOCKER="false"

 # Parse arguments
 for arg in "$@"
@@ -28,19 +26,22 @@ do
 		INSTALL_SIM="false"
 	fi

-done

-# detect if running in docker
-if [ -f /.dockerenv ]; then
-	echo "Running within docker, installing initial dependencies";
-	apt-get --quiet -y update && DEBIAN_FRONTEND=noninteractive apt-get --quiet -y install \
-		ca-certificates \
-		gnupg \
-		lsb-core \
-		sudo \
-		wget \
-		;
-fi
+	if [[ $arg == "--from-docker" ]]; then
+		INSIDE_DOCKER="true"
+	fi
+
+  if [[ $arg == "--help" ]]; then
+    echo "#⚡️ PX4 Dependency Installer for Ubuntu"
+    echo "# Options:
+#
+#  --no-nuttx       boolean
+#  --no-sim-tools   boolean"
+    echo "#"
+    exit
+  fi
+
+done

 # script directory
 DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
@@ -69,9 +70,22 @@ elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 	echo "Ubuntu 20.04"
 fi

+VERBOSE_BAR="####################"
+echo
+echo $VERBOSE_BAR
+echo "#⚡️ Starting PX4 Dependency Installer for Ubuntu ${UBUNTU_RELEASE} (${INSTALL_ARCH})"
+echo "# Options:
+#
+#  - Install NuttX = ${INSTALL_NUTTX}
+#  - Install Simulation = ${INSTALL_SIM}"
+echo $VERBOSE_BAR
+echo

 echo
-echo "Installing PX4 general dependencies"
+echo $VERBOSE_BAR
+echo "🍻 Installing System Dependencies"
+echo $VERBOSE_BAR
+echo

 sudo apt-get update -y --quiet
 sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
@@ -85,6 +99,7 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 	gdb \
 	git \
 	lcov \
+	libssl-dev \
 	libxml2-dev \
 	libxml2-utils \
 	make \
@@ -100,32 +115,42 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 	zip \
 	;

-# Python3 dependencies
+# Python 3 dependencies
 echo
-echo "Installing PX4 Python3 dependencies"
+echo $VERBOSE_BAR
+echo "🍻 Installing Python dependencies"
+echo $VERBOSE_BAR
+echo
+
 if [ -n "$VIRTUAL_ENV" ]; then
 	# virtual environments don't allow --user option
 	python -m pip install -r ${DIR}/requirements.txt
 else
 	# older versions of Ubuntu require --user option
-	python3 -m pip install --user -r ${DIR}/requirements.txt
+	if [[ $INSIDE_DOCKER == "true" ]]; then
+		# when running inside a docker container we don't need to install
+		# under --user since the installer user is root
+		# its best to install packages globaly for any user to find
+		python3 -m pip install -r /tmp/requirements.txt
+	else
+		python3 -m pip install --user -r ${DIR}/requirements.txt
+	fi
 fi

 # NuttX toolchain (arm-none-eabi-gcc)
 if [[ $INSTALL_NUTTX == "true" ]]; then

 	echo
-	echo "Installing NuttX dependencies"
+	echo $VERBOSE_BAR
+	echo "🍻 Installing NuttX dependencies"
+	echo $VERBOSE_BAR
+	echo

 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		automake \
 		binutils-dev \
 		bison \
-		build-essential \
 		flex \
-		g++-multilib \
-		gcc-multilib \
-		gdb-multiarch \
 		genromfs \
 		gettext \
 		gperf \
@@ -145,7 +170,12 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		u-boot-tools \
 		util-linux \
 		vim-common \
+		g++-arm-linux-gnueabihf \
+		gcc-arm-linux-gnueabihf \
+		g++-aarch64-linux-gnu \
+		gcc-aarch64-linux-gnu \
 		;
+
 	if [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		kconfig-frontends \
@@ -158,32 +188,47 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		sudo usermod -a -G dialout $USER
 	fi

-	# arm-none-eabi-gcc
 	NUTTX_GCC_VERSION="9-2020-q2-update"
 	NUTTX_GCC_VERSION_SHORT="9-2020q2"
+	echo
+	echo $VERBOSE_BAR
+	echo "🍻 Verifying proper gcc version (${NUTTX_GCC_VERSION}), and installing if not found"
+	echo

 	source $HOME/.profile # load changed path for the case the script is reran before relogin
 	if [ $(which arm-none-eabi-gcc) ]; then
 		GCC_VER_STR=$(arm-none-eabi-gcc --version)
-		GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
+		GCC_VER_FOUND=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
 	fi

-	if [[ "$GCC_FOUND_VER" == "1" ]]; then
-		echo "arm-none-eabi-gcc-${NUTTX_GCC_VERSION} found, skipping installation"
+	if [[ $(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}") == "1" ]]; then
+		echo "📌 Skipping installation, the arm cross compiler was found"
+		echo $VERBOSE_BAR
+		echo

 	else
-		echo "Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
-		wget -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
-			sudo tar -jxf /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 -C /opt/;
+		echo "📌 The arm cross compiler was not found";
+		echo " * Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
+		COMPILER_NAME="gcc-arm-none-eabi-${NUTTX_GCC_VERSION}"
+		COMPILER_PATH="/tmp/$COMPILER_NAME-linux.tar.bz2"
+    wget -O $COMPILER_PATH https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/${COMPILER_NAME}-${INSTALL_ARCH}-linux.tar.bz2
+		sudo tar -jxf $COMPILER_PATH -C /opt/;

 		# add arm-none-eabi-gcc to user's PATH
-		exportline="export PATH=/opt/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}/bin:\$PATH"
-
-		if grep -Fxq "$exportline" $HOME/.profile; then
+		exportline="export PATH=\"/opt/${COMPILER_NAME}/bin:\$PATH\""
+		if [[ $INSIDE_DOCKER == "true" ]]; then
+			# when running on a docker container its best to set the environment globally
+			# since we don't know which user is going to be running commands on the container
+			touch /etc/profile.d/px4env.sh
+			echo $exportline >> /etc/profile.d/px4env.sh
+		elif grep -Fxq "$exportline" $HOME/.profile; then
 			echo "${NUTTX_GCC_VERSION} path already set.";
 		else
 			echo $exportline >> $HOME/.profile;
 		fi
+		echo " * arm-none-eabi-gcc (${NUTTX_GCC_VERSION}) Installed Succesful to /opt/${COMPILER_NAME}/bin"
+		echo $VERBOSE_BAR
+		echo
 	fi
 fi

@@ -191,45 +236,32 @@ fi
 if [[ $INSTALL_SIM == "true" ]]; then

 	echo
-	echo "Installing PX4 simulation dependencies"
+	echo $VERBOSE_BAR
+	echo "🍻 Installing PX4 Simulation Tools"
+	echo
+
+	# default and Ubuntu 20.04
+	gazebo_version=11
+	gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
+	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
+		gazebo_version=9
+	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		gazebo_version=11
+		gazebo_packages="gazebo libgazebo-dev"
+	fi
+
+	echo "  * Gazebo Version $gazebo_version"
+	echo $VERBOSE_BAR

 	# General simulation dependencies
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		bc \
-		;
-
-	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
-		java_version=11
-	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
-		java_version=13
-	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		java_version=11
-	else
-		java_version=14
-	fi
-	# Java (jmavsim or fastrtps)
-	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		ant \
-		openjdk-$java_version-jre \
-		openjdk-$java_version-jdk \
 		libvecmath-java \
 		;

-	# Set Java 11 as default
-	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")
-
-	# Gazebo
-	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
-		gazebo_version=9
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
-	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		gazebo_packages="gazebo libgazebo-dev"
-	else
-		# default and Ubuntu 20.04
-		gazebo_version=11
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
-	fi
-
+	# Installing Gazebo and dependencies
+	# Setup OSRF Gazebo repository
 	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 	# Update list, since new gazebo-stable.list has been added
@@ -257,7 +289,26 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	fi
 fi

-if [[ $INSTALL_NUTTX == "true" ]]; then
-	echo
-	echo "Relogin or reboot computer before attempting to build NuttX targets"
+if [[ $INSIDE_DOCKER == "true" ]]; then
+	# cleanup installation
+	rm -rf /tmp/
 fi
+
+if [[ $INSIDE_DOCKER == "false" ]] && [[ $INSTALL_NUTTX == "true" ]]; then
+	echo
+	echo $VERBOSE_BAR
+	echo "💡 We recommend you relogin/reboot before attempting to build NuttX targets"
+	echo $VERBOSE_BAR
+	echo
+fi
+
+echo
+echo
+echo $VERBOSE_BAR
+echo "#⚡️ PX4 Dependency Installer Ended Succesfully
+#
+#  For more information on PX4 Autopilot check out our docs
+#  at docs.px4.io, if you find a bug please file an issue
+#  on GitHub https://github.com/px4/px4-autopilot"
+echo $VERBOSE_BAR
+echo
@@ -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/main/msg/'+topic_filename(topic)+'.msg'
+            node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/master/msg/'+topic_filename(topic)+'.msg'
            nodes.append(node)

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

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

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

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

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

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

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

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

 	target_link_libraries(drivers_board
 		PRIVATE
-			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
@@ -47,7 +47,6 @@ 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,7 +57,6 @@ else()

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

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

 	target_link_libraries(drivers_board
 		PRIVATE
-			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
@@ -46,7 +46,6 @@ 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,7 +60,6 @@ else()

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

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

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

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

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

 	target_link_libraries(drivers_board
 		PRIVATE
-			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
@@ -46,7 +46,6 @@ 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,7 +45,6 @@ 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,7 +52,6 @@ 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,3 +9,12 @@ 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,7 +60,6 @@ else()

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

 	target_link_libraries(drivers_board
 		PRIVATE
-			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
@@ -46,7 +46,6 @@ 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/main/en/modules/modules_main.html")
+		add_definitions(-DCONSTRAINED_FLASH_NO_HELP="https://docs.px4.io/master/en/modules/modules_main.html")
 	endif()

 	if(CONSTRAINED_MEMORY)
@@ -20,4 +20,3 @@ 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,4 +20,3 @@ 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,6 +65,9 @@ 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', 'humble', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"):
+    if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', '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/main/msg/%s)" % msg_file
+        msg_url="[source file](https://github.com/PX4/PX4-Autopilot/blob/master/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/main/msg/tools/generate_msg_docs.py) from the source code.
+This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/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 vertical velocity error, (metres/sec)
+float32 evv				# Standard deviation of horizontal velocity error, (metres/sec)

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

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

-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
+# 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

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

-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 and linear velocity local frame of reference
+uint8 local_frame

-float32[3] velocity         # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
+# 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] angular_velocity # Angular velocity in body-fixed frame (rad/s). 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] position_variance
-float32[3] orientation_variance
-float32[3] velocity_variance
+# 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

 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/main/en/dev_setup/dev_env.html")
+		message(FATAL_ERROR "GCC 7 or older no longer supported. https://docs.px4.io/master/en/dev_setup/dev_env.html")
 	endif()
 endif()

@@ -1,36 +0,0 @@
-############################################################################
-#
-#   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
-)
@@ -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/main/en/sensor/leddar_one.html
+Setup/usage information: https://docs.px4.io/master/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/main/en/sensor/sfxx_lidar.html
+Setup/usage information: https://docs.px4.io/master/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/main/en/sensor/sfxx_lidar.html
+Setup/usage information: https://docs.px4.io/master/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/main/en/sensor/lidar_lite.html
+Setup/usage information: https://docs.px4.io/master/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/main/en/sensor/lidar_lite.html
+Setup/usage information: https://docs.px4.io/master/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/main/en/sensor/rangefinders.html#teraranger-rangefinders
+Setup/usage information: https://docs.px4.io/master/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/main/en/sensor/tfmini.html
+Setup/usage information: https://docs.px4.io/master/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/main/en/sensor/pmw3901.html#thone-thoneflow-3901u
+Setup/usage information: https://docs.px4.io/master/en/sensor/pmw3901.html#thone-thoneflow-3901u

 ### Examples

@@ -494,57 +494,8 @@ 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,7 +147,6 @@ 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,8 +179,6 @@ 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};

@@ -479,12 +477,6 @@ 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);
@@ -613,6 +605,8 @@ void PX4IO::Run()

 			_param_update_force = false;

+			ModuleParams::updateParams();
+
 			update_params();

 			/* Check if the flight termination circuit breaker has been updated */
@@ -705,67 +699,14 @@ 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,14 +61,6 @@ 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;
@@ -101,39 +93,3 @@ 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,7 +44,6 @@

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

 /**
@@ -70,17 +69,14 @@ 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 {3U};	// number of different telemetry data types
+	static constexpr uint32_t NUM_DATA_TYPES {1U};	// 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,9 +193,6 @@ 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,19 +350,6 @@ 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
 */
@@ -398,34 +385,3 @@ 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,9 +64,7 @@ enum class ghstFrameType {
 };

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

 struct ghst_frame_header_t {
@@ -141,27 +139,4 @@ __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://docs.px4.io/main/en/dev_setup/building_px4.html#building-for-nuttx")
+        print("See also https://dev.px4.io/master/en/setup/building_px4.html#firmware_version")
        print("")
        sys.exit(1)

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

 bool
-WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
-			  const float tas_meas, const float tas_variance)
+WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
+			  const matrix::Quatf &q_att)
 {
-	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);
+	// 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;
 	}

+	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;
@@ -121,12 +124,13 @@ WindEstimator::update(uint64_t time_now)

 void
 WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI,
-			     const float hor_vel_variance, const matrix::Quatf &q_att)
+			     const matrix::Vector2f &velIvar, 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, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
+		_initialised = initialise(velI, velIvar_constrained, true_airspeed, q_att);
 		return;
 	}

@@ -153,7 +157,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, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
+			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed, q_att);
 		}

 		// we either did a filter reset or the current measurement was rejected so do not fuse
@@ -172,11 +176,10 @@ 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 float hor_vel_variance,
-			 const matrix::Quatf &q_att)
+WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
 {
 	if (!_initialised) {
-		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
+		_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
 		return;
 	}

@@ -248,7 +251,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, hor_vel_variance, matrix::Eulerf(q_att).psi());
+			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
 		}

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

 	void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
-			   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);
+			   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);

 	bool is_estimate_valid() { return _initialised; }

@@ -98,13 +97,6 @@ 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

@@ -135,11 +127,8 @@ 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 float hor_vel_variance, const float heading_rad,
-			const float tas_meas = NAN, const float tas_variance = NAN);
+	bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
+			const matrix::Quatf &q_att);

 	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 float hor_vel_variance =  lpos_evh * lpos_evh;
-		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q);
+		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);

 		// sideslip fusion
-		_wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q);
+		_wind_estimator.fuse_beta(time_now_usec, vI, 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.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);
+			// 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);

 			_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{NAN}; /**< true airspeed from groundspeed minus windspeed */
-	float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */
+	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 */
 	bool _armed_prev{false};

 	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
@@ -184,8 +184,7 @@ 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::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
 	)

 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -374,7 +373,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() ||
-				     (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get()))) {
+				     _ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
 					_in_takeoff_situation = false;
 				}

@@ -517,14 +516,11 @@ 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 &&
-	    _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+	    && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
 		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
 		Quatf q(_vehicle_attitude.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_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
 	}

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

 void AirspeedModule::update_ground_minus_wind_airspeed()
 {
-	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;
-	}
+	// 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);
 }


@@ -227,17 +227,3 @@ 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.orientation_variance[0]) ? sqrtf(fmaxf(
-							       mocap.orientation_variance[0],
-							       fmaxf(mocap.orientation_variance[1],
-									       mocap.orientation_variance[2]))) <= _eo_max_std_dev : true);
+					       && (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);

 			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.orientation_variance[0]) ? sqrtf(fmaxf(
-								vision.orientation_variance[0],
-								fmaxf(vision.orientation_variance[1],
-										vision.orientation_variance[2]))) <= _eo_max_std_dev : true);
+						&& (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);

 			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/main/en/config/firmware.html#bootloader");
+				     "VTOL is not supported with fmu-v2, see docs.px4.io/master/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)
-	Vector3f    velVar{};      ///< XYZ velocity variances ((m/sec)**2)
+	Matrix3f    velCov{};      ///< XYZ velocity covariances ((m/sec)**2)
 	float       angVar{};      ///< angular heading variance (rad**2)
 	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
@@ -361,41 +361,21 @@ void Ekf::controlExternalVisionFusion()
 		}

 		// determine if we should use the yaw observation
-		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);
+		if (_control_status.flags.ev_yaw) {
+			if (reset && _control_status_prev.flags.ev_yaw) {
+				resetYawToEv();
 			}
+
+			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)) {
@@ -604,7 +584,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)

 				fuseGpsYaw();

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

 				if (is_fusion_failing) {
 					if (_nb_gps_yaw_reset_available > 0) {
@@ -1131,13 +1111,18 @@ 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()) {
-				_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);
+
+				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);
 			}
 		}
 	}
@@ -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 = Vector3f(_aid_src_aux_vel.test_ratio).max(); }
+	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }

 	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,54 +111,13 @@ 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 {
-		if (_control_status.flags.mag_hdg) {
-			heading_innov = _aid_src_mag_heading.innovation;
+	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; }

-		} 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 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(); }

 	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); }
@@ -201,8 +160,6 @@ 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); }

@@ -397,17 +354,9 @@ 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
@@ -442,7 +391,6 @@ 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)
@@ -470,7 +418,10 @@ 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

@@ -516,6 +467,15 @@ 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)

@@ -545,17 +505,9 @@ 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)
@@ -654,12 +606,12 @@ private:
 	void predictCovariance();

 	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true);
+	bool fuseMag(const Vector3f &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, estimator_aid_source_1d_s& aid_src_status);
+	bool fuseYaw(const float innovation, const float variance);

 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();
@@ -722,12 +674,6 @@ 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);

@@ -740,11 +686,10 @@ 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 updateGpsPos(const gpsSample &gps_sample);
-
 	void fuseGpsVel();
+
+	void updateGpsPos(const gpsSample &gps_sample);
 	void fuseGpsPos();

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

 	void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
 	{
-		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;
-		}
+		status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
+		status.innovation_rejected = (status.test_ratio > 1.f);
 	}

 	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++) {
-			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;
-			}
+			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);
 		}
 	}
 };
@@ -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] = _aid_src_aux_vel.innovation[0];
-	aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
+	aux_vel_innov[0] = _aux_vel_innov(0);
+	aux_vel_innov[1] = _aux_vel_innov(1);
 }

 void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
 {
-	aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
-	aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
+	aux_vel_innov_var[0] = _aux_vel_innov_var(0);
+	aux_vel_innov_var[1] = _aux_vel_innov_var(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,17 +931,7 @@ 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
-	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;
-	}
+	mag = sqrtf(math::max(_yaw_test_ratio, _mag_test_ratio.max()));

 	// return the largest velocity and position innovation test ratio
 	vel = NAN;
@@ -1012,23 +1002,9 @@ 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;
@@ -1284,6 +1260,10 @@ 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;
@@ -1298,6 +1278,8 @@ void Ekf::stopMagHdgFusion()
 		_control_status.flags.mag_hdg = false;

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

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

 		stopMagHdgFusion();

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

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

 	// rotate measurement into correct earth frame if required
 	switch (_ev_sample_delayed.vel_frame) {
@@ -1695,7 +1679,6 @@ 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);
 	}
 }

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

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

 void Ekf::stopFlowFusion()
@@ -1903,60 +1886,3 @@ 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,8 +245,6 @@ 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; }

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

 	// innovation consistency check monitoring ratios
-	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
+	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
 	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,7 +49,10 @@ void Ekf::controlGpsFusion()
 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {

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

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021-2022 PX4. All rights reserved.
+ *   Copyright (c) 2021 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -39,43 +39,16 @@
 /* #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);
@@ -99,9 +72,6 @@ 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);
@@ -130,6 +100,8 @@ 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,27 +60,18 @@ 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;

-	// 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 predicted antenna yaw angle
+	const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));
+
+	// 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));
+
 	// calculate intermediate variables
 	const float HK0 = sinf(_gps_yaw_offset);
 	const float HK1 = q0*q3;
@@ -130,11 +121,9 @@ 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
-	const float heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
+	_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);

-	_aid_src_gnss_yaw.innovation_variance = heading_innov_var;
-
-	if (heading_innov_var < R_YAW) {
+	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;

@@ -145,15 +134,19 @@ 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
-	setEstimatorAidStatusTestRatio(_aid_src_gnss_yaw, innov_gate);
+	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);

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

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

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

-	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)) {
+	if ((fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
+		&& !_control_status.flags.in_air && isTimedOut(_time_last_heading_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
@@ -191,13 +184,12 @@ 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;
 	}
 }

@@ -215,11 +207,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.e-2f));
+	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance, true);

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

 	return true;
 }
@@ -66,28 +66,6 @@ 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);
 		}
 	}

@@ -366,9 +344,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);

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

@@ -386,13 +364,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
-		fuseMag(mag, _aid_src_mag, update_all_states);
+		fuseMag(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, _aid_src_mag, update_all_states);
+		fuseMag(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, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states)
+bool Ekf::fuseMag(const Vector3f &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, estimator_aid_source_3d_s &aid_src_mag, b
 	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);

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

-	if (aid_src_mag.innovation_variance[0] < R_MAG) {
+	if (_mag_innov_var(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, estimator_aid_source_3d_s &aid_src_mag, b

 	_fault_status.flags.bad_mag_x = false;

-	const float HKX24 = 1.0F/aid_src_mag.innovation_variance[0];
+	const float HKX24 = 1.0F/_mag_innov_var(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,11 +126,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
 	const float IV17 = 2*IV0 - 2*IV1;
 	const float IV18 = IV10 - IV8 + IV9;

-	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;
+	_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;

-	// check innovation variances for being badly conditioned
-	if (aid_src_mag.innovation_variance[1] < R_MAG) {
+	// chedk innovation variances for being badly conditioned
+
+	if (_mag_innov_var(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;

@@ -142,7 +143,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b

 	_fault_status.flags.bad_mag_y = false;

-	if (aid_src_mag.innovation_variance[2] < R_MAG) {
+	if (_mag_innov_var(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;

@@ -156,40 +157,46 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b

 	// 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
-	const Vector3f mag_observation = mag - _state.mag_B;
-	Vector3f mag_innov = mag_I_rot - mag_observation;
+	_mag_innov = mag_I_rot + _state.mag_B - mag;

 	// 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
-	_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];
+	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;
+		}
+	}

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

@@ -239,7 +246,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b

 		} else if (index == 1) {

-			// recalculate innovation variance because states and covariances have changed due to previous fusion
+			// recalculate innovation variance becasue 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;
@@ -265,9 +272,9 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
 			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);

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

-			if (aid_src_mag.innovation_variance[1] < R_MAG) {
+			if (_mag_innov_var(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;

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

 			// Calculate Y axis observation jacobians
 			Hfusion.setZero();
@@ -345,9 +352,9 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
 			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);

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

-			if (aid_src_mag.innovation_variance[2] < R_MAG) {
+			if (_mag_innov_var(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;

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

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

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

-		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;
-		}
-
+		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index));

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

-	return aid_src_mag.fused[0] && aid_src_mag.fused[1] && aid_src_mag.fused[2];
+	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;
 }

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

 	// 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
-	aid_src_status.innovation_variance = variance;
+	_heading_innov_var = variance;

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

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

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

 	// check if the innovation variance calculation is badly conditioned
-	if (aid_src_status.innovation_variance >= variance) {
+	if (_heading_innov_var >= 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.f / aid_src_status.innovation_variance;
+		heading_innov_var_inv = 1.0f / _heading_innov_var;

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

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

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

 		// if we are in air we don't want to fuse the measurement
@@ -617,13 +618,13 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 		// by interference or a large initial gyro bias
 		if (!_control_status.flags.in_air
 		&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
-		&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
+		&& isTimedOut(_time_last_heading_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) * aid_src_status.innovation_variance));
-			aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
+			float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var));
+			_heading_innov = math::constrain(innovation, -gate_limit, gate_limit);

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

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

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

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

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

 		return true;
 	}
@@ -1,30 +1,25 @@
 # 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`](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)
+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`

 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 mc_wind_estimator_tuning.py <logfilename.ulg>
+python drag_replay.py <logfilename.ulg>
 ```

 The estimated parameters are displayed in the console and the fit quality is shown in a plot:
@@ -32,8 +27,6 @@ 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,102 +159,6 @@ 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,8 +47,7 @@ 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;
-		estimator_aid_source_1d_s unused;
-		fuseYaw(innovation, obs_var, unused);
+		fuseYaw(innovation, obs_var);

 		_last_static_yaw = NAN;

@@ -62,8 +61,7 @@ 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;
-				estimator_aid_source_1d_s unused;
-				fuseYaw(innovation, obs_var, unused);
+				fuseYaw(innovation, obs_var);
 			}

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

 		_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);
+			PublishOdometry(now, imu_sample_new);
 			PublishGlobalPosition(now);
 			PublishWindEstimate(now);

@@ -650,22 +650,10 @@ 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);

-	// 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);
+	// GNSS velocity & position
 	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)
@@ -1059,43 +1047,72 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	_local_position_pub.publish(lpos);
 }

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

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

-	// orientation quaternion
+	// 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
 	_ekf.getQuaternion().copyTo(odom.q);

-	// velocity
-	odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
-	_ekf.getVelocity().copyTo(odom.velocity);
+	// 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);

-	// 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 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]);

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

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

-	// orientation covariance
-	_ekf.orientation_covariances_euler().diag().copyTo(odom.orientation_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];

 	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);
@@ -1109,30 +1126,37 @@ 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.position);
-	aligned_pos.copyTo(aligned_ev_odom.position);
+	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);

 	switch (ev_odom.velocity_frame) {
-	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);
+	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);
 			break;
 		}

-	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);
+	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);
 			break;
 		}
 	}

-	aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
+	aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_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);
@@ -1344,6 +1368,9 @@ 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;
@@ -1530,8 +1557,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, NAN},
-				.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, NAN},
+				.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},
 			};
 			_ekf.setAuxVelData(auxvel_sample);
 		}
@@ -1600,122 +1627,77 @@ 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.velocity[0]) && PX4_ISFINITE(ev_odom.velocity[1]) && PX4_ISFINITE(ev_odom.velocity[2])) {
-			bool velocity_valid = true;
+		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;

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

-			default:
-				velocity_valid = false;
-				break;
+			} else {
+				ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
 			}

-			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];
+			// velocity measurement error from ev_data or parameters
+			float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());

-				const float evv_noise_var = sq(_param_ekf2_evv_noise.get());
+			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];

-				// 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;
+			} else {
+				ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
 			}
+
+			new_ev_odom = true;
 		}

 		// check for valid position data
-		if (PX4_ISFINITE(ev_odom.position[0]) && PX4_ISFINITE(ev_odom.position[1]) && PX4_ISFINITE(ev_odom.position[2])) {
+		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;

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

-			// switch (ev_odom.pose_frame) {
-			// case vehicle_odometry_s::POSE_FRAME_NED:
-			// 	ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
-			// 	break;
+			// 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]);

-			// 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;
+			} else {
+				ev_data.posVar.setAll(param_evp_noise_var);
 			}
+
+			new_ev_odom = true;
 		}

 		// check for valid orientation data
-		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))
-		   ) {
+		if (PX4_ISFINITE(ev_odom.q[0])) {
 			ev_data.quat = Quatf(ev_odom.q);
-			ev_data.quat.normalize();

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

-			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]);
+			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]);

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

 			new_ev_odom = true;
@@ -1723,8 +1705,6 @@ 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);
@@ -2260,7 +2240,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/main/en/advanced_config/tuning_the_ecl_ekf.html) page.
+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.

 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);
+	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
 	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,17 +257,9 @@ 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
@@ -332,17 +324,9 @@ 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,7 +26,12 @@ void Vio::setData(const extVisionSample &vio_data)

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

 void Vio::setPositionVariance(const Vector3f &posVar)
@@ -71,7 +76,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.velVar = Vector3f{0.1f, 0.1f, 0.1f};
+	vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
 	vio_data.angVar = 0.05f;
 	vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
 	return vio_data;
@@ -53,6 +53,7 @@ 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(5);
+	_sensor_simulator.runSeconds(2);

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

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

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

 	_ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new);
@@ -243,21 +242,16 @@ 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);
@@ -282,8 +276,6 @@ 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,10 +276,13 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody)
 	// WHEN: measurement is given in BODY-FRAME and
 	//       x variance is bigger than y variance
 	_sensor_simulator._vio.setVelocityFrameToBody();
-
-	const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
+	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_body(1.0f, 0.0f, 0.0f);
-	_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
+	_sensor_simulator._vio.setVelocityCovariance(vel_cov_body);
 	_sensor_simulator._vio.setVelocity(vel_body);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
@@ -309,10 +312,13 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
 	// WHEN: measurement is given in LOCAL-FRAME and
 	//       x variance is bigger than y variance
 	_sensor_simulator._vio.setVelocityFrameToLocal();
-
-	const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f};
+	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_earth(1.0f, 0.0f, 0.0f);
-	_sensor_simulator._vio.setVelocityVariance(vel_cov_earth);
+	_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
 	_sensor_simulator._vio.setVelocity(vel_earth);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Ramon Roche	2eb2b2a17f	github: test file changed action	2022-08-16 10:01:40 -07:00
Ramon Roche	bba589ece2	docker: pass on CI env variables	2022-08-16 09:36:46 -07:00
Ramon Roche	ad827fc806	ubuntu: alphabetize list	2022-08-16 09:36:46 -07:00
Ramon Roche	606cfba407	docker: single tag to rule them all the new docker images are defaulting to a hardcoded tag that can be traced back to source code which is why we are adding the string to the build and run scripts	2022-08-16 09:36:46 -07:00
Ramon Roche	598e7e65b7	ubuntu: consolidate dependencies	2022-08-16 09:36:04 -07:00
JacobCrabill	c83e1eb175	Tools: Docker: Add dummy file for COPY command Allows the main target of teh COPY command to fail if the file doesn't exist ('COPY' requires at least one valid file to copy).	2022-08-03 08:42:44 -07:00
Ramon Roche	ba9b23c2d1	docker: new build interface script The new build script can pass arguments directly to ubuntu.sh when building, this is good for when you want a special build for example when you don't need RTPS or Simulation ``` ./Tools/docker_build.sh --no-sim-tools ``` In the future it can be extended if needed to generate multiple docker images based on the same Dockerfile, if for example we only want to build arm targets	2022-08-02 09:52:55 -07:00
Ramon Roche	38fc86cf3b	setup: ubuntu help options	2022-08-02 09:38:25 -07:00
Ramon Roche	a3d0ca9800	docker: cleanup verbose script output	2022-08-02 09:30:39 -07:00
Ramon Roche	34b9dc880f	docker: cleanup run script * add verbose output	2022-08-02 07:53:05 -07:00
Ramon Roche	04e08c5edb	setup: alphabetize list	2022-08-02 07:52:17 -07:00
Ramon Roche	2668510295	docker: multiarch development image To run the image you need to use the docker_run.sh script found in the Tools directory as shown below: ./Tools/docker_run.sh make all_variants_px4_fmu-v5x	2022-07-28 10:50:43 -07:00
Ramon Roche	5388f7f911	ubuntu.sh: updated dependencies and output I double checked each new dependency and made sure to build each target with the new install script as well as test FastRTPS and Simulations * removed unused dependencies * settled on the absolute most updated JDK we could possibly use given our dependencies (JDK 14) * added conditional support for use in a container environment via a new flag --from-docker * added multi-platform build tools for development	2022-07-28 10:49:19 -07:00