mavlink: remove default device and consolidate serial vs UDP init handling

Increase allowed rtk injections to 8 for moving base. Update GPS submodule.
Fix CANNODE_SUB_MBD typo
2026-05-31 10:40:06 +08:00 · 2022-11-18 11:53:56 -05:00 · 2022-11-18 11:04:17 -05:00 · 2022-11-18 11:04:17 -05:00 · 2022-11-18 07:10:27 +01:00 · 2022-11-18 07:10:27 +01:00
101 changed files with 1456 additions and 1901 deletions
@@ -920,7 +920,6 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true'
@@ -1,17 +1,29 @@
-Please use [PX4 Discuss](http://discuss.px4.io/) or [Discord](https://discord.gg/dronecode) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
+<!--

-## Describe problem solved by this pull request
-A clear and concise description of the problem this proposed change will solve. Or, what it will improve.
-E.g. For this use case I ran into...
+Thank you for your contribution!

-## Describe your solution
-A clear and concise description of what you have implemented.
+Get early feedback through
+- Dronecode Discord: https://discord.gg/dronecode
+- PX4 Discuss: http://discuss.px4.io/
+- opening a draft pr and sharing the link

-## Describe possible alternatives
-A clear and concise description of alternative solutions or features you've considered.
+-->

-## Test data / coverage
-How was it tested? What cases were covered? Logs uploaded to https://review.px4.io/ and screenshots of the important plot parts.
+### Solved Problem
+When ... I found that ...

-## Additional context
-Add any other related context or media.
+Fixes #{Github issue ID}
+
+### Solution
+- Add ... for ...
+- Refactor ...
+
+### Alternatives
+We could also ...
+
+### Test coverage
+- Unit/integration test: ...
+- Simulation/hardware testing logs: https://review.px4.io/
+
+### Context
+Related links, screenshot before/after, video
@@ -190,3 +190,7 @@ endmenu
 menu "examples"
 source "src/examples/Kconfig"
 endmenu
+
+menu "platforms"
+source "platforms/common/Kconfig"
+endmenu
@@ -2,3 +2,4 @@ CONFIG_PLATFORM_QURT=y
 CONFIG_BOARD_TOOLCHAIN="qurt"
 CONFIG_MODULES_MUORB_SLPI=y
 CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_ORB_COMMUNICATOR=y
@@ -1,6 +1,6 @@
 ############################################################################
 #
-# Copyright (c) 2022 ModalAI, Inc. All rights reserved.
+#   Copyright (c) 2022 ModalAI, Inc. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -30,25 +30,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-#
-# Overview:
-# Voxl2 PX4 is built in 2 parts, the part that runs on the
-# application (apps) processor, and the library that is loaded on the DSP.
-#
-############################################################################

-include(px4_git)
-
-list(APPEND CMAKE_MODULE_PATH
-	"${PX4_SOURCE_DIR}/platforms/posix/cmake"
-)
-
-# set(DISABLE_PARAMS_MODULE_SCOPING TRUE)
-
-add_definitions(-DORB_COMMUNICATOR)
-
-set(CONFIG_PARAM_SERVER "1")
-
-add_definitions( -D__PX4_LINUX )
-
-include(CMakeParseArguments)
+add_library(drivers_board
+	board_config.h
+	init.c
+    )
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 ModalAI, Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * VOXL2 internal definitions
+ */
+
+#pragma once
+
+#define BOARD_HAS_NO_RESET
+#define BOARD_HAS_NO_BOOTLOADER
+/*
+ * I2C buses
+ */
+#define PX4_NUMBER_I2C_BUSES    3
+
+#include <system_config.h>
+#include <px4_platform_common/board_common.h>
@@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#include "board_config.h"
+
+// Place holder for VOXL2-specific early startup code
@@ -25,10 +25,10 @@ The full instructions are available here:
 - Clone the repo (Don't forget to update and initialize all submodules)
 - In the top level directory
 ```
-px4$ boards/modalai/voxl2/run-docker.sh
-root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/clean.sh
-root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/build-posix.sh
-root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/build-qurt.sh
+px4$ boards/modalai/voxl2/scripts/run-docker.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/clean.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-apps.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-slpi.sh
 root@9373fa1401b8:/usr/local/workspace# exit
 ```

@@ -37,7 +37,7 @@ root@9373fa1401b8:/usr/local/workspace# exit
 Once the DSP and Linux images have been built they can be installed on a VOXL 2
 board using ADB. There is a script to do this.
 ```
-px4$ boards/modalai/voxl2/install-voxl.sh
+px4$ boards/modalai/voxl2/scripts/install-voxl.sh
 ```

 ## Running PX4 on VOXL 2
@@ -66,10 +66,17 @@ INFO  [px4] Startup script returned successfully
 pxh>
 ```

+## Notes
+
+You cannot cleanly shutdown PX4 with the shutdown command on VOXL 2. You have
+to power cycle the board and restart everything.
+
 ## Tips

 Start with a VOXL 2 that only has the system image installed, not the SDK

+Run the command ```voxl-px4 -s``` on target to run the self-test
+
 In order to see DSP specific debug messages the mini-dm tool in the Hexagon SDK
 can be used:
 ```
@@ -1,11 +0,0 @@
-#!/bin/bash
-
-echo "*** Starting qurt build ***"
-
-source /home/build-env.sh
-
-make modalai_voxl2_qurt
-
-cat build/modalai_voxl2_default/src/lib/version/build_git_version.h
-
-echo "*** End of qurt build ***"
@@ -0,0 +1,7 @@
+
+# libfc_sensor.so is provided in the Docker build environment
+target_link_libraries(px4 PRIVATE
+		/home/libfc_sensor.so
+		px4_layer
+		${module_libraries}
+)
@@ -1,43 +0,0 @@
-############################################################################
-#
-# Copyright (c) 2022 ModalAI, Inc. All rights reserved.
-#
-############################################################################
-#
-# This cmake config builds for QURT which is the operating system running on
-# the DSP side of VOXL 2
-#
-# Required environment variables:
-#	HEXAGON_TOOLS_ROOT
-#	HEXAGON_SDK_ROOT
-#
-############################################################################
-
-if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
-else()
-	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
-endif()
-
-if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "Environment variable HEXAGON_TOOLS_ROOT must be set")
-else()
-	set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT})
-endif()
-
-include(px4_git)
-
-list(APPEND CMAKE_MODULE_PATH
-	"${PX4_SOURCE_DIR}/platforms/qurt/cmake"
-)
-
-include(Toolchain-qurt)
-include(qurt_reqs)
-
-include_directories(${HEXAGON_SDK_INCLUDES})
-
-add_definitions(-DORB_COMMUNICATOR)
-
-set(CONFIG_PARAM_CLIENT "1")
-
-set(DISABLE_PARAMS_MODULE_SCOPING TRUE)
@@ -4,3 +4,4 @@ CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_MODULES_MUORB_APPS=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_ORB_COMMUNICATOR=y
@@ -1,12 +1,11 @@
 #!/bin/bash

-echo "*** Starting posix build ***"
+echo "*** Starting apps processor build ***"

 source /home/build-env.sh

-make modalai_voxl2_default
+make modalai_voxl2

 cat build/modalai_voxl2_default/src/lib/version/build_git_version.h

-
-echo "*** End of posix build ***"
+echo "*** End of apps processor build ***"
@@ -0,0 +1,11 @@
+#!/bin/bash
+
+echo "*** Starting qurt slpi build ***"
+
+source /home/build-env.sh
+
+make modalai_voxl2-slpi
+
+cat build/modalai_voxl2-slpi_default/src/lib/version/build_git_version.h
+
+echo "*** End of qurt slpi build ***"
@@ -1,19 +1,19 @@
 #!/bin/bash

-# Push qurt image to voxl2
-adb push build/modalai_voxl2_qurt/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
+# Push slpi image to voxl2
+adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp

-# Push posix image to voxl2
+# Push apps processor image to voxl2
 adb push build/modalai_voxl2_default/bin/px4 /usr/bin

 # Push scripts to voxl2
 adb push build/modalai_voxl2_default/bin/px4-alias.sh /usr/bin
-adb push boards/modalai/voxl2/voxl-px4 /usr/bin
+adb push boards/modalai/voxl2/target/voxl-px4 /usr/bin
 adb shell chmod a+x /usr/bin/px4-alias.sh
 adb shell chmod a+x /usr/bin/voxl-px4

 # Push configuration file
 adb shell mkdir -p /etc/modalai
-adb push boards/modalai/voxl2/voxl-px4.config /etc/modalai
+adb push boards/modalai/voxl2/target/voxl-px4.config /etc/modalai

 adb shell sync
@@ -41,11 +41,9 @@

 #define BOARD_HAS_NO_RESET
 #define BOARD_HAS_NO_BOOTLOADER
-#define ORB_COMMUNICATOR 1
-/*
- * I2C buses
- */
-#define PX4_NUMBER_I2C_BUSES    3
+
+// Define this as empty since there are no I2C buses
+#define BOARD_I2C_BUS_CLOCK_INIT

 #include <system_config.h>
 #include <px4_platform_common/board_common.h>
@@ -34,7 +34,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
    # Depend on BOARD_DEFCONFIG so that we reconfigure on config change
    set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${BOARD_DEFCONFIG})

-    if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "qurt" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
+    if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
        # Generate boardconfig from saved defconfig
        execute_process(COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
                        ${DEFCONFIG_PATH} ${BOARD_DEFCONFIG}
@@ -228,10 +228,6 @@ if(EXISTS ${BOARD_DEFCONFIG})

        # platform-specific include path
        include_directories(${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include)
-
-        if(PLATFORM STREQUAL "qurt")
-            include(${PX4_SOURCE_DIR}/boards/modalai/voxl2/cmake/voxl2_qurt.cmake)
-        endif()
    endif()

 	if(ARCHITECTURE)
@@ -186,7 +186,6 @@ set(msg_files
 	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
-	VehicleAngularAcceleration.msg
 	VehicleAngularAccelerationSetpoint.msg
 	VehicleAngularVelocity.msg
 	VehicleAttitude.msg
@@ -1,5 +0,0 @@
-
-uint64 timestamp        # time since system start (microseconds)
-uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
-
-float32[3] xyz          # angular acceleration about the FRD body frame XYZ-axis in rad/s^2
@@ -1,7 +1,9 @@

-uint64 timestamp        # time since system start (microseconds)
-uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+uint64 timestamp          # time since system start (microseconds)
+uint64 timestamp_sample   # timestamp of the data sample on which this message is based (microseconds)

-float32[3] xyz		# Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s
+float32[3] xyz		  # Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s
+
+float32[3] xyz_derivative # angular acceleration about the FRD body frame XYZ-axis in rad/s^2

 # TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth
@@ -0,0 +1 @@
+rsource "*/Kconfig"
@@ -0,0 +1,5 @@
+menuconfig ORB_COMMUNICATOR
+	bool "orb communicator"
+	default n
+	---help---
+		Enable support for the uorb communicator for distributed platforms
@@ -36,10 +36,6 @@
 #include "uORBManager.hpp"
 #include "uORBUtils.hpp"

-#ifdef ORB_COMMUNICATOR
-#include "uORBCommunicator.hpp"
-#endif /* ORB_COMMUNICATOR */
-
 #include <px4_platform_common/sem.hpp>
 #include <systemlib/px4_macros.h>

@@ -38,9 +38,9 @@

 #include "SubscriptionCallback.hpp"

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 #include "uORBCommunicator.hpp"
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 #if defined(__PX4_NUTTX)
 #include <nuttx/mm/mm.h>
@@ -304,7 +304,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 		return PX4_ERROR;
 	}

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	/*
 	 * if the write is successful, send the data over the Multi-ORB link
 	 */
@@ -317,7 +317,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 		}
 	}

-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 	return PX4_OK;
 }
@@ -346,7 +346,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle)
 	return PX4_OK;
 }

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
 {
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
@@ -357,19 +357,7 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)

 	return -1;
 }
-
-/*
-//TODO: Check if we need this since we only unadvertise when things all shutdown and it doesn't actually remove the device
-int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta)
-{
-	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
-	if (ch != nullptr && meta != nullptr) {
-		return ch->topic_unadvertised(meta->o_name);
-	}
-	return -1;
-}
-*/
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 px4_pollevent_t
 uORB::DeviceNode::poll_state(cdev::file_t *filp)
@@ -413,7 +401,7 @@ void uORB::DeviceNode::add_internal_subscriber()
 	lock();
 	_subscriber_count++;

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();

 	if (ch != nullptr && _subscriber_count > 0) {
@@ -421,7 +409,7 @@ void uORB::DeviceNode::add_internal_subscriber()
 		ch->add_subscription(_meta->o_name, 1);

 	} else
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 	{
 		unlock();
@@ -433,7 +421,7 @@ void uORB::DeviceNode::remove_internal_subscriber()
 	lock();
 	_subscriber_count--;

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();

 	if (ch != nullptr && _subscriber_count == 0) {
@@ -441,13 +429,13 @@ void uORB::DeviceNode::remove_internal_subscriber()
 		ch->remove_subscription(_meta->o_name);

 	} else
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 	{
 		unlock();
 	}
 }

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
 {
 	// if there is already data in the node, send this out to
@@ -490,7 +478,7 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data

 	return PX4_OK;
 }
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
 {
@@ -41,6 +41,7 @@
 #include <containers/IntrusiveSortedList.hpp>
 #include <containers/List.hpp>
 #include <px4_platform_common/atomic.h>
+#include <px4_platform_common/px4_config.h>

 namespace uORB
 {
@@ -122,9 +123,8 @@ public:

 	static int        unadvertise(orb_advert_t handle);

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	static int16_t topic_advertised(const orb_metadata *meta);
-	//static int16_t topic_unadvertised(const orb_metadata *meta);

 	/**
 	 * processes a request for add subscription from remote
@@ -145,7 +145,7 @@ public:
 	 * processed the received data message from remote.
 	 */
 	int16_t process_received_message(int32_t length, uint8_t *data);
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 	/**
 	  * Add the subscriber to the node's list of subscriber.  If there is
@@ -48,6 +48,10 @@
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"

+#ifdef CONFIG_ORB_COMMUNICATOR
+pthread_mutex_t uORB::Manager::_communicator_mutex = PTHREAD_MUTEX_INITIALIZER;
+#endif
+
 uORB::Manager *uORB::Manager::_Instance = nullptr;

 bool uORB::Manager::initialize()
@@ -258,7 +262,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		}
 	}

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR

 	/*
 	 * Generate the path to the node and try to open it.
@@ -300,7 +304,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		}
 	}

-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 	return ret;
 }
@@ -360,10 +364,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	// For remote systems call over and inform them
 	uORB::DeviceNode::topic_advertised(meta);
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 	/* the advertiser may perform an initial publish to initialise the object */
 	if (data != nullptr) {
@@ -611,7 +615,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
 	return fd;
 }

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
 {
 	_comm_channel = channel;
@@ -623,18 +627,53 @@ void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)

 uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator()
 {
-	return _comm_channel;
+	pthread_mutex_lock(&_communicator_mutex);
+	uORBCommunicator::IChannel *temp = _comm_channel;
+	pthread_mutex_unlock(&_communicator_mutex);
+
+	return temp;
 }

 int16_t uORB::Manager::process_remote_topic(const char *topic_name, bool isAdvertisement)
 {
+	PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);
+
 	int16_t rc = 0;

-	if (isAdvertisement) {
+	char nodepath[orb_maxpath];
+	int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
+	DeviceMaster *device_master = get_device_master();
+
+	if (ret == OK && device_master && isAdvertisement) {
+		uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
+
+		if (node) {
+			node->mark_as_advertised();
+			_remote_topics.insert(topic_name);
+			return rc;
+		}
+	}
+
+	// Didn't find a node so we need to create it via an advertisement
+	const struct orb_metadata *const *topic_list = orb_get_topics();
+	orb_id_t topic_ptr = nullptr;
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
+			topic_ptr = topic_list[i];
+			break;
+		}
+	}
+
+	if (topic_ptr) {
+		PX4_INFO("Advertising remote topic %s", topic_name);
 		_remote_topics.insert(topic_name);
+		orb_advertise(topic_ptr, nullptr);

 	} else {
+		PX4_INFO("process_remote_topic meta not found for %s\n", topic_name);
 		_remote_topics.erase(topic_name);
+		rc = -1;
 	}

 	return rc;
@@ -723,7 +762,7 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
 	return _remote_subscriber_topics.find(messageName);
 }

-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 #ifdef ORB_USE_PUBLISHER_RULES

@@ -40,11 +40,12 @@

 #include <uORB/topics/uORBTopics.hpp> // For ORB_ID enum
 #include <stdint.h>
+#include <px4_platform_common/px4_config.h>

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 #include "ORBSet.hpp"
 #include "uORBCommunicator.hpp"
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 namespace uORB
 {
@@ -159,9 +160,9 @@ typedef enum {
 * uORB Api's.
 */
 class uORB::Manager
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	: public uORBCommunicator::IChannelRxHandler
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 {
 public:
 	// public interfaces for this class.
@@ -464,7 +465,7 @@ public:
 	static bool is_advertised(const void *node_handle);
 #endif

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.
 	 * @param comm_channel
@@ -485,7 +486,7 @@ public:
 	 * for a given topic
 	 */
 	bool is_remote_subscriber_present(const char *messageName);
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 private: // class methods

@@ -500,13 +501,14 @@ private: // class methods
 private: // data members
 	static Manager *_Instance;

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	// the communicator channel instance.
 	uORBCommunicator::IChannel *_comm_channel{nullptr};
+	static pthread_mutex_t _communicator_mutex;

 	ORBSet _remote_subscriber_topics;
 	ORBSet _remote_topics;
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 	DeviceMaster *_device_master{nullptr};

@@ -514,7 +516,7 @@ private: //class methods
 	Manager();
 	virtual ~Manager();

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	/**
 	 * Interface to process a received topic from remote.
 	 * @param topic_name
@@ -570,7 +572,7 @@ private: //class methods
 	 *  otherwise = failure.
 	 */
 	virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data);
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */

 #ifdef ORB_USE_PUBLISHER_RULES

@@ -35,6 +35,7 @@

 #if defined(CONFIG_SYSTEM_CDCACM)
 __BEGIN_DECLS
+#include <board_config.h>
 #include <arch/board/board.h>
 #include <syslog.h>
 #include <nuttx/wqueue.h>
@@ -42,6 +43,7 @@ __BEGIN_DECLS

 #include <termios.h>
 #include <sys/ioctl.h>
+#include <fcntl.h>

 extern int sercon_main(int c, char **argv);
 extern int serdis_main(int c, char **argv);
@@ -97,18 +97,14 @@ if(EXISTS "${PX4_BOARD_DIR}/cmake/upload.cmake")
 	include(${PX4_BOARD_DIR}/cmake/upload.cmake)
 endif()

+# board defined link libraries
+if(EXISTS "${PX4_BOARD_DIR}/cmake/link_libraries.cmake")
+	include(${PX4_BOARD_DIR}/cmake/link_libraries.cmake)
+endif()

 if("${PX4_BOARD}" MATCHES "beaglebone_blue")
 	target_link_libraries(px4 PRIVATE robotics_cape)

-elseif("${PX4_BOARD}" MATCHES "modalai_voxl2")
-	# libfc_sensor.so is provided in the Docker build environment
-	target_link_libraries(px4 PRIVATE
-			/home/libfc_sensor.so
-			px4_layer
-			${module_libraries}
-	)
-
 elseif("${PX4_BOARD}" MATCHES "emlid_navio2")
 	target_link_libraries(px4 PRIVATE atomic)

@@ -33,11 +33,16 @@

 get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES)

-QURT_LIB(LIB_NAME px4
-	SOURCES
-		${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
-	LINK_LIBS
-		modules__muorb__slpi
-		${module_libraries}
-		px4_layer
+include_directories(
+	${CMAKE_CURRENT_BINARY_DIR}
+	)
+
+add_library(px4 SHARED
+	${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
+	)
+
+target_link_libraries(px4
+	modules__muorb__slpi
+	${module_libraries}
+	px4_layer
 	)
@@ -31,6 +31,27 @@
 #
 ############################################################################

+if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
+else()
+	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
+endif()
+
+if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Environment variable HEXAGON_TOOLS_ROOT must be set")
+else()
+	set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT})
+endif()
+
+include(px4_git)
+
+include(Toolchain-qurt)
+include(qurt_reqs)
+
+include_directories(${HEXAGON_SDK_INCLUDES})
+
+set(DISABLE_PARAMS_MODULE_SCOPING TRUE)
+
 #=============================================================================
 #
 #	Defined functions in this file
@@ -176,46 +176,3 @@ list2string(CMAKE_EXE_LINKER_FLAGS
 	)

 include (CMakeParseArguments)
-
-# Process DSP files
-function (QURT_LIB)
-	set(options)
-	set(oneValueArgs LIB_NAME)
-	set(multiValueArgs SOURCES LINK_LIBS INCS FLAGS)
-	cmake_parse_arguments(QURT_LIB "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} )
-
-	include_directories(
-		${CMAKE_CURRENT_BINARY_DIR}
-		)
-
-	if ("${QURT_LIB_SOURCES}" STREQUAL "")
-		message(FATAL_ERROR "QURT_LIB called without SOURCES")
-	else()
-		# Build lib that is run on the DSP
-		add_library(${QURT_LIB_LIB_NAME} SHARED
-			${QURT_LIB_SOURCES}
-			)
-
-		if (NOT "${QURT_LIB_FLAGS}" STREQUAL "")
-			set_target_properties(${QURT_LIB_LIB_NAME} PROPERTIES COMPILE_FLAGS "${QURT_LIB_FLAGS}")
-		endif()
-
-		if (NOT "${QURT_LIB_INCS}" STREQUAL "")
-			target_include_directories(${QURT_LIB_LIB_NAME} PUBLIC ${QURT_LIB_INCS})
-		endif()
-
-		target_link_libraries(${QURT_LIB_LIB_NAME}
-			${QURT_LIB_LINK_LIBS}
-			)
-	endif()
-
-	set(DSPLIB_TARGET_PATH "/usr/lib/rfsa/adsp/")
-
-	# Add a rule to load the files onto the target that run in the DSP
-	add_custom_target(lib${QURT_LIB_LIB_NAME}-load
-		DEPENDS ${QURT_LIB_LIB_NAME}
-		COMMAND adb wait-for-device
-		COMMAND adb push lib${QURT_LIB_LIB_NAME}.so ${DSPLIB_TARGET_PATH}
-		COMMAND echo "Pushed lib${QURT_LIB_LIB_NAME}.so and dependencies to ${DSPLIB_TARGET_PATH}"
-		)
-endfunction()
@@ -180,16 +180,6 @@ PARAM_DEFINE_FLOAT(UART_ESC_T_MINF, 0.15);
 */
 PARAM_DEFINE_INT32(UART_ESC_T_EXPO, 35);

-/**
- * UART ESC Turtle Mode Yaw Reversal
- *
- * @group UART ESC
- * @min 0
- * @max 1
- * @decimal 10
- * @increment 1
- */
-PARAM_DEFINE_INT32(UART_ESC_T_YAWR, 0);
 /**
 * UART ESC Turtle Mode Cosphi
 *
@@ -149,8 +149,8 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts)
 */
 static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 {
-	ts->tv_sec = (time_t)abstime / 1000000;
-	abstime -= (hrt_abstime)(ts->tv_sec * 1000000);
+	ts->tv_sec = (typeof(ts->tv_sec))(abstime / 1000000);
+	abstime -= (hrt_abstime)(ts->tv_sec) * 1000000;
 	ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000);
 }

@@ -535,11 +535,12 @@ void GPS::handleInjectDataTopic()

 	bool updated = false;

-	// Limit maximum number of GPS injections to 6 since usually
+	// Limit maximum number of GPS injections to 8 since usually
 	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
-	// Looking at 6 packets thus guarantees, that at least a full injection
+	// Looking at 8 packets thus guarantees, that at least a full injection
 	// data set is evaluated.
-	const size_t max_num_injections = 6;
+	// Moving Base reuires a higher rate, so we allow up to 8 packets.
+	const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH;
 	size_t num_injections = 0;

 	do {
@@ -37,6 +37,7 @@

 #include <poll.h>
 #include <termios.h>
+#include <fcntl.h>

 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -31,6 +31,7 @@
 *
 ****************************************************************************/

+#include <board_config.h>
 #include "SafetyButton.hpp"

 #ifndef GPIO_BTN_SAFETY
@@ -316,10 +316,10 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_subscriber_list.add(new BeepCommand(_node));
 	_subscriber_list.add(new LightsCommand(_node));

-	int32_t cannode_sub_mdb = 0;
-	param_get(param_find("CANNODE_SUB_MDB"), &cannode_sub_mdb);
+	int32_t cannode_sub_mbd = 0;
+	param_get(param_find("CANNODE_SUB_MBD"), &cannode_sub_mbd);

-	if (cannode_sub_mdb == 1) {
+	if (cannode_sub_mbd == 1) {
 		_subscriber_list.add(new MovingBaselineData(_node));
 	}

@@ -259,5 +259,16 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}

+	// 2022-11-11: translate VT_F_TRANS_THR/VT_PSHER_RMP_DT -> VT_PSHER_SLEW
+	{
+		if (strcmp("VT_PSHER_RMP_DT", node->name) == 0) {
+			strcpy(node->name, "VT_PSHER_SLEW");
+			double _param_vt_f_trans_thr = param_find("VT_F_TRANS_THR");
+			node->d = _param_vt_f_trans_thr / node->d;
+			PX4_INFO("copying %s -> %s", "VT_PSHER_RMP_DT", "VT_PSHER_SLEW");
+			return true;
+		}
+	}
+
 	return false;
 }
@@ -136,6 +136,7 @@ AngularVelocityController::Run()
 		_last_run = now;

 		const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
+		_angular_acceleration = Vector3f(vehicle_angular_velocity.xyz_derivative);

 		/* check for updates in other topics */
 		_vehicle_status_sub.update(&_vehicle_status);
@@ -164,13 +165,6 @@ AngularVelocityController::Run()
 			}
 		}

-		// check angular acceleration topic
-		vehicle_angular_acceleration_s vehicle_angular_acceleration;
-
-		if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
-			_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
-		}
-
 		// check rates setpoint topic
 		vehicle_rates_setpoint_s vehicle_rates_setpoint;

@@ -50,7 +50,6 @@
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
-#include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -105,7 +104,6 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

 	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};				/**< motor limits subscription */
-	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; 	/**< vehicle angular acceleration subscription */
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};			/**< vehicle control mode subscription */
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};			/**< vehicle land detected subscription */
 	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};			/**< vehicle rates setpoint subscription */
@@ -142,23 +142,6 @@ PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0);
 */
 PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);

-/**
- * Delay between RC loss and configured reaction
- *
- * RC signal not updated -> still use data for COM_RC_LOSS_T seconds
- * Consider RC signal lost -> wait COM_RCL_ACT_T seconds in Hold mode to regain signal
- * React with failsafe action NAV_RCL_ACT
- *
- * A zero value disables the delay.
- *
- * @group Commander
- * @unit s
- * @min 0.0
- * @max 25.0
- * @decimal 3
- */
-PARAM_DEFINE_FLOAT(COM_RCL_ACT_T, 15.0f);
-
 /**
 * Home position enabled
 *
@@ -224,7 +207,8 @@ PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
 *
 * @group Commander
 * @unit s
- * @decimal 2
+ * @decimal 1
+ * @increment 0.1
 */

 PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
@@ -240,7 +224,8 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
 *
 * @group Commander
 * @unit s
- * @decimal 2
+ * @decimal 1
+ * @increment 0.1
 */
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);

@@ -277,8 +262,6 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
 * @value 0 Warning
 * @value 2 Land mode
 * @value 3 Return at critical level, land at emergency level
- * @decimal 0
- * @increment 1
 */
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);

@@ -312,7 +295,6 @@ PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
 * @value 0 Warning
 * @value 1 Return
 * @value 2 Land
- * @decimal 0
 * @increment 1
 */
 PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
@@ -1033,6 +1015,8 @@ PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
 * @group Commander
 * @min 0
 * @max 30
+ * @decimal 1
+ * @increment 0.1
 * @unit s
 */
 PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f);
@@ -66,17 +66,18 @@ using math::Utilities::sq;
 using math::Utilities::updateYawInRotMat;

 // maximum sensor intervals in usec
-#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
-#define EV_MAX_INTERVAL   (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
-#define GPS_MAX_INTERVAL  (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
-#define RNG_MAX_INTERVAL  (uint64_t)2e5 ///< Maximum allowable time interval between range finder  measurements (uSec)
+static constexpr uint64_t BARO_MAX_INTERVAL     = 200e3;  ///< Maximum allowable time interval between pressure altitude measurements (uSec)
+static constexpr uint64_t EV_MAX_INTERVAL       = 200e3;  ///< Maximum allowable time interval between external vision system measurements (uSec)
+static constexpr uint64_t GNSS_MAX_INTERVAL     = 500e3;  ///< Maximum allowable time interval between GNSS measurements (uSec)
+static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
+static constexpr uint64_t RNG_MAX_INTERVAL      = 200e3;  ///< Maximum allowable time interval between range finder  measurements (uSec)

 // bad accelerometer detection and mitigation
-#define BADACC_PROBATION  (uint64_t)10e6        ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
-#define BADACC_BIAS_PNOISE      4.9f    ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
+static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
+static constexpr float BADACC_BIAS_PNOISE = 4.9f;  ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)

 // ground effect compensation
-#define GNDEFFECT_TIMEOUT       10E6    ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
+static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)

 enum class VelocityFrame : uint8_t {
 	LOCAL_FRAME_NED = 0,
@@ -253,6 +254,14 @@ struct auxVelSample {
 	Vector2f    velVar{};      ///< estimated error variance of the NE velocity (m/sec)**2
 };

+struct systemFlagUpdate {
+	uint64_t time_us{};
+	bool at_rest{false};
+	bool in_air{true};
+	bool is_fixed_wing{false};
+	bool gnd_effect{false};
+};
+
 struct stateSample {
 	Quatf    quat_nominal{};        ///< quaternion defining the rotation from body to earth frame
 	Vector3f vel{};                 ///< NED velocity in earth frame in m/s
@@ -408,11 +417,11 @@ struct parameters {
 	float acc_bias_learn_gyr_lim{3.0f};     ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
 	float acc_bias_learn_tc{0.5f};          ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)

-	const unsigned reset_timeout_max{7000000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
-	const unsigned no_aid_timeout_max{1000000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
+	const unsigned reset_timeout_max{7'000'000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
+	const unsigned no_aid_timeout_max{1'000'000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
 	const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec)

-	int32_t valid_timeout_max{5000000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
+	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)

 	// static barometer pressure position error coefficient along body axes
 	float static_pressure_coef_xp{0.0f};    // (-)
@@ -447,7 +456,6 @@ struct parameters {
 	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
 	const unsigned EKFGSF_reset_delay{1000000};     ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
 	const float EKFGSF_yaw_err_max{0.262f};         ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
-	const unsigned EKFGSF_reset_count_limit{3};     ///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
 };

 union fault_status_u {
@@ -47,6 +47,23 @@ void Ekf::controlFusionModes()
 {
 	// Store the status to enable change detection
 	_control_status_prev.value = _control_status.value;
+	_state_reset_count_prev = _state_reset_status.reset_count;
+
+	if (_system_flag_buffer) {
+		systemFlagUpdate system_flags_delayed;
+
+		if (_system_flag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &system_flags_delayed)) {
+
+			set_vehicle_at_rest(system_flags_delayed.at_rest);
+			set_in_air_status(system_flags_delayed.in_air);
+
+			set_is_fixed_wing(system_flags_delayed.is_fixed_wing);
+
+			if (system_flags_delayed.gnd_effect) {
+				set_gnd_effect();
+			}
+		}
+	}

 	// monitor the tilt alignment
 	if (!_control_status.flags.tilt_align) {
@@ -86,7 +103,7 @@ void Ekf::controlFusionModes()
 	}

 	if (_gps_buffer) {
-		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
+		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);

 		// check for arrival of new sensor data at the fusion time horizon
 		_time_prev_gps_us = _gps_sample_delayed.time_us;
@@ -418,7 +435,7 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi

 		const bool continuing_conditions_passing = !gps_checks_failing;

-		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GPS_MAX_INTERVAL);
+		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);

 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _control_status.flags.tilt_align
@@ -612,10 +629,3 @@ void Ekf::controlAuxVelFusion()
 		}
 	}
 }
-
-bool Ekf::hasHorizontalAidingTimedOut() const
-{
-	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
-}
@@ -335,43 +335,43 @@ public:
 	const auto &state_reset_status() const { return _state_reset_status; }

 	// return the amount the local vertical position changed in the last reset and the number of reset events
-	uint8_t get_posD_reset_count() const { return _state_reset_status.posD_counter; }
+	uint8_t get_posD_reset_count() const { return _state_reset_status.reset_count.posD; }
 	void get_posD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.posD_change;
-		*counter = _state_reset_status.posD_counter;
+		*counter = _state_reset_status.reset_count.posD;
 	}

 	// return the amount the local vertical velocity changed in the last reset and the number of reset events
-	uint8_t get_velD_reset_count() const { return _state_reset_status.velD_counter; }
+	uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; }
 	void get_velD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.velD_change;
-		*counter = _state_reset_status.velD_counter;
+		*counter = _state_reset_status.reset_count.velD;
 	}

 	// return the amount the local horizontal position changed in the last reset and the number of reset events
-	uint8_t get_posNE_reset_count() const { return _state_reset_status.posNE_counter; }
+	uint8_t get_posNE_reset_count() const { return _state_reset_status.reset_count.posNE; }
 	void get_posNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.posNE_change.copyTo(delta);
-		*counter = _state_reset_status.posNE_counter;
+		*counter = _state_reset_status.reset_count.posNE;
 	}

 	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
-	uint8_t get_velNE_reset_count() const { return _state_reset_status.velNE_counter; }
+	uint8_t get_velNE_reset_count() const { return _state_reset_status.reset_count.velNE; }
 	void get_velNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.velNE_change.copyTo(delta);
-		*counter = _state_reset_status.velNE_counter;
+		*counter = _state_reset_status.reset_count.velNE;
 	}

 	// return the amount the quaternion has changed in the last reset and the number of reset events
-	uint8_t get_quat_reset_count() const { return _state_reset_status.quat_counter; }
+	uint8_t get_quat_reset_count() const { return _state_reset_status.reset_count.quat; }
 	void get_quat_reset(float delta_quat[4], uint8_t *counter) const
 	{
 		_state_reset_status.quat_change.copyTo(delta_quat);
-		*counter = _state_reset_status.quat_counter;
+		*counter = _state_reset_status.reset_count.quat;
 	}

 	// get EKF innovation consistency check status information comprising of:
@@ -451,18 +451,27 @@ private:
 	void updateHorizontalDeadReckoningstatus();
 	void updateVerticalDeadReckoningStatus();

-	struct {
-		uint8_t velNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
-		uint8_t velD_counter;	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
-		uint8_t posNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
-		uint8_t posD_counter;	///< number of vertical position reset events (allow to wrap if count exceeds 255)
-		uint8_t quat_counter;	///< number of quaternion reset events (allow to wrap if count exceeds 255)
+	struct StateResetCounts
+	{
+		uint8_t velNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t velD{0};	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
+		uint8_t posNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t posD{0};	///< number of vertical position reset events (allow to wrap if count exceeds 255)
+		uint8_t quat{0};	///< number of quaternion reset events (allow to wrap if count exceeds 255)
+	};
+
+	struct StateResets {
 		Vector2f velNE_change;  ///< North East velocity change due to last reset (m)
 		float velD_change;	///< Down velocity change due to last reset (m/sec)
 		Vector2f posNE_change;	///< North, East position change due to last reset (m)
 		float posD_change;	///< Down position change due to last reset (m)
 		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
-	} _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
+
+		StateResetCounts reset_count{};
+	};
+
+	StateResets _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
+	StateResetCounts _state_reset_count_prev{};

 	Vector3f _ang_rate_delayed_raw{};	///< uncorrected angular rate vector at fusion time horizon (rad/sec)

@@ -717,6 +726,8 @@ private:
 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
 	void fuseOptFlow();
+	float predictFlowRange();
+	Vector2f predictFlowVelBody();

 	// horizontal and vertical position aid source
 	void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
@@ -875,7 +886,6 @@ private:
 	// control fusion of GPS observations
 	void controlGpsFusion();
 	bool shouldResetGpsFusion() const;
-	bool hasHorizontalAidingTimedOut() const;
 	bool isYawFailure() const;

 	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
@@ -1060,9 +1070,6 @@ private:
 	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};

-	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
-	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
-
 	void runYawEKFGSF();

 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
@@ -77,8 +77,16 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f

 	_output_new.vel.xy() += delta_horz_vel;

-	_state_reset_status.velNE_change = delta_horz_vel;
-	_state_reset_status.velNE_counter++;
+	// record the state change
+	if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
+		_state_reset_status.velNE_change = delta_horz_vel;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.velNE_change += delta_horz_vel;
+	}
+
+	_state_reset_status.reset_count.velNE++;

 	// Reset the timout timer
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
@@ -101,8 +109,16 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 	_output_new.vel(2) += delta_vert_vel;
 	_output_vert_new.vert_vel += delta_vert_vel;

-	_state_reset_status.velD_change = delta_vert_vel;
-	_state_reset_status.velD_counter++;
+	// record the state change
+	if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
+		_state_reset_status.velD_change = delta_vert_vel;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.velD_change += delta_vert_vel;
+	}
+
+	_state_reset_status.reset_count.velD++;

 	// Reset the timout timer
 	_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
@@ -151,8 +167,16 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f

 	_output_new.pos.xy() += delta_horz_pos;

-	_state_reset_status.posNE_change = delta_horz_pos;
-	_state_reset_status.posNE_counter++;
+	// record the state change
+	if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
+		_state_reset_status.posNE_change = delta_horz_pos;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.posNE_change += delta_horz_pos;
+	}
+
+	_state_reset_status.reset_count.posNE++;

 	// Reset the timout timer
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
@@ -180,27 +204,36 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 		P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var));
 	}

-	// store the reset amount and time to be published
-	_state_reset_status.posD_change = new_vert_pos - old_vert_pos;
-	_state_reset_status.posD_counter++;
+	const float delta_z = new_vert_pos - old_vert_pos;

 	// apply the change in height / height rate to our newest height / height rate estimate
 	// which have already been taken out from the output buffer
-	_output_new.pos(2) += _state_reset_status.posD_change;
+	_output_new.pos(2) += delta_z;

 	// add the reset amount to the output observer buffered data
 	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		_output_buffer[i].pos(2) += _state_reset_status.posD_change;
-		_output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change;
+		_output_buffer[i].pos(2) += delta_z;
+		_output_vert_buffer[i].vert_vel_integ += delta_z;
 	}

 	// add the reset amount to the output observer vertical position state
 	_output_vert_new.vert_vel_integ = _state.pos(2);

-	_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
-	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
-	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
+	// record the state change
+	if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
+		_state_reset_status.posD_change = delta_z;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.posD_change += delta_z;
+	}
+
+	_state_reset_status.reset_count.posD++;
+
+	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
+	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
+	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
+	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);

 	// Reset the timout timer
 	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
@@ -856,6 +889,8 @@ void Ekf::fuse(const Vector24f &K, float innovation)
 {
 	_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
 	_state.quat_nominal.normalize();
+	_R_to_earth = Dcmf(_state.quat_nominal);
+
 	_state.vel -= K.slice<3, 1>(4, 0) * innovation;
 	_state.pos -= K.slice<3, 1>(7, 0) * innovation;
 	_state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation;
@@ -1382,9 +1417,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	_state.quat_nominal = quat_after_reset;
 	uncorrelateQuatFromOtherStates();

-	// record the state change
-	_state_reset_status.quat_change = q_error;
-
 	// update the yaw angle variance
 	if (yaw_variance > FLT_EPSILON) {
 		increaseQuatYawErrVariance(yaw_variance);
@@ -1392,17 +1424,26 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)

 	// add the reset amount to the output observer buffered data
 	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
+		_output_buffer[i].quat_nominal = q_error * _output_buffer[i].quat_nominal;
 	}

 	// apply the change in attitude quaternion to our newest quaternion estimate
 	// which was already taken out from the output buffer
-	_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
+	_output_new.quat_nominal = q_error * _output_new.quat_nominal;
+
+	// record the state change
+	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
+		_state_reset_status.quat_change = q_error;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
+		_state_reset_status.quat_change.normalize();
+	}
+
+	_state_reset_status.reset_count.quat++;

 	_last_static_yaw = NAN;
-
-	// capture the reset event
-	_state_reset_status.quat_counter++;
 }

 // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
@@ -1434,9 +1475,6 @@ bool Ekf::resetYawToEKFGSF()
 		_inhibit_ev_yaw_use = true;
 	}

-	_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
-	_ekfgsf_yaw_reset_count++;
-
 	return true;
 }

@@ -410,6 +410,42 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 	}
 }

+void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_system_flag_buffer == nullptr) {
+		_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
+
+		if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
+			delete _system_flag_buffer;
+			_system_flag_buffer = nullptr;
+			printBufferAllocationFailed("system flag");
+			return;
+		}
+	}
+
+	_system_flag_buffer->push(system_flags);
+
+	const int64_t time_us = system_flags.time_us
+				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
+
+	// limit data rate to prevent data being lost
+	if (time_us >= static_cast<int64_t>(_system_flag_buffer->get_newest().time_us + _min_obs_interval_us)) {
+
+		systemFlagUpdate system_flags_new{system_flags};
+		system_flags_new.time_us = time_us;
+
+		_system_flag_buffer->push(system_flags_new);
+
+	} else {
+		ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
+	}
+}
+
 void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
@@ -101,6 +101,8 @@ public:

 	void setAuxVelData(const auxVelSample &auxvel_sample);

+	void setSystemFlagData(const systemFlagUpdate &system_flags);
+
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() { return &_params; }
@@ -355,9 +357,10 @@ protected:
 	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)

 	// data buffer instances
-	RingBuffer<imuSample> _imu_buffer{12};           // buffer length 12 with default parameters
-	RingBuffer<outputSample> _output_buffer{12};
-	RingBuffer<outputVert> _output_vert_buffer{12};
+	static constexpr uint8_t kBufferLengthDefault = 12;
+	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
+	RingBuffer<outputSample> _output_buffer{kBufferLengthDefault};
+	RingBuffer<outputVert> _output_vert_buffer{kBufferLengthDefault};

 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
@@ -368,6 +371,7 @@ protected:
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
 	RingBuffer<dragSample> *_drag_buffer{nullptr};
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
+	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};

 	uint64_t _time_last_gps_buffer_push{0};
 	uint64_t _time_last_gps_yaw_buffer_push{0};
@@ -92,7 +92,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 				&& _gps_checks_passed;

 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)
+				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)
 				&& _gps_checks_passed
 				&& gps_checks_passing
 				&& !gps_checks_failing;
@@ -158,7 +158,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 		}

 	} else if (_control_status.flags.gps_hgt
-		   && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)) {
+		   && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
 		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
 		stopGpsHgtFusion();
@@ -112,25 +112,24 @@ void Ekf::controlGpsFusion()
 					fuseVelocity(_aid_src_gnss_vel);
 					fuseHorizontalPosition(_aid_src_gnss_pos);

-					if (shouldResetGpsFusion()) {
-						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
+					bool do_vel_pos_reset = shouldResetGpsFusion();

-						/* A reset is not performed when getting GPS back after a significant period of no data
-						 * because the timeout could have been caused by bad GPS.
-						 * The total number of resets allowed per boot cycle is limited.
+					if (isYawFailure()
+					    && _control_status.flags.in_air
+					    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+					    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
+						/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
+						 * fails while the difference between the yaw emergency estimator and the yas estimate is large.
+						 * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
+						 * present before flight to prevent triggering due to GPS glitches or other sensor errors.
 						 */
-						if (isYawFailure()
-						    && _control_status.flags.in_air
-						    && !was_gps_signal_lost
-						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
-						    && isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
-							// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
-							// to improve its estimate if the previous reset was not successful.
-							if (resetYawToEKFGSF()) {
-								ECL_WARN("GPS emergency yaw reset");
-							}
+						if (resetYawToEKFGSF()) {
+							ECL_WARN("GPS emergency yaw reset");
+							do_vel_pos_reset = true;
 						}
+					}

+					if (do_vel_pos_reset) {
 						ECL_WARN("GPS fusion timeout, resetting velocity and position");
 						_information_events.flags.reset_vel_to_gps = true;
 						_information_events.flags.reset_pos_to_gps = true;
@@ -221,30 +220,20 @@ bool Ekf::shouldResetGpsFusion() const
 	/* We are relying on aiding to constrain drift so after a specified time
 	 * with no aiding we need to do something
 	 */
-	const bool is_reset_required = hasHorizontalAidingTimedOut()
+	const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
+						     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+						     && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
+
+	const bool is_reset_required = has_horizontal_aiding_timed_out
 				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);

-	/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
-	 * navigation casued by a bad yaw estimate.
-
-	 * A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
-	 * innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
-	 * different test criteria are used that take longer to trigger and reduce false positives. A reset is
-	 * not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
-	 * or other sensor errors.
-	 */
-	const bool is_recent_takeoff_nav_failure = _control_status.flags.in_air
-			&& isRecent(_time_last_on_ground_us, 30000000)
-			&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
-			&& (_time_last_hor_vel_fuse > _time_last_on_ground_us);
-
 	const bool is_inflight_nav_failure = _control_status.flags.in_air
 					     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
 					     && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
 					     && (_time_last_hor_vel_fuse > _time_last_on_ground_us)
 					     && (_time_last_hor_pos_fuse > _time_last_on_ground_us);

-	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
+	return (is_reset_required || is_inflight_nav_failure);
 }

 bool Ekf::isYawFailure() const
@@ -209,7 +209,6 @@ void Ekf::runInAirYawReset()
 			resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());

 			_information_events.flags.yaw_aligned_to_imu_gps = true;
-			_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;

 			// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
 			if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
@@ -45,6 +45,8 @@

 #include <mathlib/mathlib.h>
 #include <float.h>
+#include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
+#include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
 #include "utils.hpp"

 void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
@@ -52,31 +54,8 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	resetEstimatorAidStatus(aid_src);
 	aid_src.timestamp_sample = _flow_sample_delayed.time_us;

-	// get rotation matrix from earth to body
-	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
-
-	// calculate the sensor position relative to the IMU
-	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
-
-	// calculate the velocity of the sensor relative to the imu in body frame
-	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
-
-	// calculate the velocity of the sensor in the earth frame
-	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
-
-	// rotate into body frame
-	const Vector3f vel_body = earth_to_body * vel_rel_earth;
-
-	// calculate the sensor position relative to the IMU in earth frame
-	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-
-	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
-	// a positive offset in earth frame leads to a smaller height above the ground.
-	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
-
-	// calculate range from focal point to centre of image
-	const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
+	const Vector2f vel_body = predictFlowVelBody();
+	const float range = predictFlowRange();

 	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
 	// correct for gyro bias errors in the data used to do the motion compensation
@@ -106,150 +85,24 @@ void Ekf::fuseOptFlow()

 	const float R_LOS = _aid_src_optical_flow.observation_variance[0];

-	// get latest estimated orientation
-	const float q0 = _state.quat_nominal(0);
-	const float q1 = _state.quat_nominal(1);
-	const float q2 = _state.quat_nominal(2);
-	const float q3 = _state.quat_nominal(3);
-
-	// get latest velocity in earth frame
-	const float vn = _state.vel(0);
-	const float ve = _state.vel(1);
-	const float vd = _state.vel(2);
-
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
-	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
+	float range = predictFlowRange();

-	// calculate range from focal point to centre of image
-	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
-	const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
+	const Vector24f state_vector = getStateAtFusionHorizonAsVector();

-	// The derivation allows for an arbitrary body to flow sensor frame rotation which is
-	// currently not supported by the EKF, so assume sensor frame is aligned with the
-	// body frame
-	const Dcmf Tbs = matrix::eye<float, 3>();
+	Vector2f innov_var;
+	Vector24f H;
+	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);

-	// Sub Expressions
-	const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
-	const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
-	const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
-	const float HK3 = HK0*vd + HK1*ve + HK2*vn;
-	const float HK4 = 1.0F/range;
-	const float HK5 = 2*HK4;
-	const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
-	const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
-	const float HK8 = HK0*vn - HK2*vd + HK6*ve;
-	const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
-	const float HK10 = q0*q2;
-	const float HK11 = q1*q3;
-	const float HK12 = HK10 + HK11;
-	const float HK13 = 2*Tbs(1,2);
-	const float HK14 = q0*q3;
-	const float HK15 = q1*q2;
-	const float HK16 = HK14 - HK15;
-	const float HK17 = 2*Tbs(1,1);
-	const float HK18 = ecl::powf(q1, 2);
-	const float HK19 = ecl::powf(q2, 2);
-	const float HK20 = -HK19;
-	const float HK21 = ecl::powf(q0, 2);
-	const float HK22 = ecl::powf(q3, 2);
-	const float HK23 = HK21 - HK22;
-	const float HK24 = HK18 + HK20 + HK23;
-	const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
-	const float HK26 = HK14 + HK15;
-	const float HK27 = 2*Tbs(1,0);
-	const float HK28 = q0*q1;
-	const float HK29 = q2*q3;
-	const float HK30 = HK28 - HK29;
-	const float HK31 = -HK18;
-	const float HK32 = HK19 + HK23 + HK31;
-	const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
-	const float HK34 = HK28 + HK29;
-	const float HK35 = HK10 - HK11;
-	const float HK36 = HK20 + HK21 + HK22 + HK31;
-	const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
-	const float HK38 = 2*HK3;
-	const float HK39 = 2*HK7;
-	const float HK40 = 2*HK8;
-	const float HK41 = 2*HK9;
-	const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
-	const float HK43 = ecl::powf(range, -2);
-	const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
-	const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
-	const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
-	const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
-	const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
-	const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
-	// const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
-
-	// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
-	_aid_src_optical_flow.innovation_variance[0] = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
-
-	if (_aid_src_optical_flow.innovation_variance[0] < R_LOS) {
+	if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
+	    || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
+		ECL_ERR("Opt flow error - covariance reset");
 		initialiseCovariance();
 		return;
 	}
-	const float HK50 = HK4 / _aid_src_optical_flow.innovation_variance[0];
-
-	const float HK51 = Tbs(0,1)*q1;
-	const float HK52 = Tbs(0,2)*q0;
-	const float HK53 = Tbs(0,0)*q2;
-	const float HK54 = HK51 + HK52 - HK53;
-	const float HK55 = Tbs(0,0)*q3;
-	const float HK56 = Tbs(0,1)*q0;
-	const float HK57 = Tbs(0,2)*q1;
-	const float HK58 = HK55 + HK56 - HK57;
-	const float HK59 = Tbs(0,0)*q0;
-	const float HK60 = Tbs(0,2)*q2;
-	const float HK61 = Tbs(0,1)*q3;
-	const float HK62 = HK59 + HK60 - HK61;
-	const float HK63 = HK54*vd + HK58*ve + HK62*vn;
-	const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
-	const float HK65 = HK58*vd + HK64*vn;
-	const float HK66 = -HK54*ve + HK65;
-	const float HK67 = HK54*vn + HK64*ve;
-	const float HK68 = -HK62*vd + HK67;
-	const float HK69 = HK62*ve + HK64*vd;
-	const float HK70 = -HK58*vn + HK69;
-	const float HK71 = 2*Tbs(0,1);
-	const float HK72 = 2*Tbs(0,2);
-	const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
-	const float HK74 = -HK16*HK71 + HK73;
-	const float HK75 = 2*Tbs(0,0);
-	const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
-	const float HK77 = -HK30*HK72 + HK76;
-	const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
-	const float HK79 = -HK35*HK75 + HK78;
-	const float HK80 = 2*HK63;
-	const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
-	const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
-	const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
-	const float HK84 = HK71*(-HK14 + HK15) + HK73;
-	const float HK85 = HK72*(-HK28 + HK29) + HK76;
-	const float HK86 = HK75*(-HK10 + HK11) + HK78;
-	const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
-	const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
-	const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
-	const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
-	const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
-	const float HK92 = 2*HK43;
-	const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
-	const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
-	// const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
-
-	// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
-	_aid_src_optical_flow.innovation_variance[1] = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
-	if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
-		// we need to reinitialise the covariance matrix and abort this fusion step
-		initialiseCovariance();
-		return;
-	}
-
-	const float HK95 = HK4 / _aid_src_optical_flow.innovation_variance[1];

 	// run the innovation consistency check and record result
 	setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
@@ -265,75 +118,37 @@ void Ekf::fuseOptFlow()
 	bool fused[2] {false, false};

 	// fuse observation axes sequentially
-	{
-		// Optical flow observation Jacobians - axis 0
-		SparseVector24f<0,1,2,3,4,5,6> Hfusion;
-		Hfusion.at<0>() = HK3*HK5;
-		Hfusion.at<1>() = HK5*HK7;
-		Hfusion.at<2>() = HK5*HK8;
-		Hfusion.at<3>() = HK5*HK9;
-		Hfusion.at<4>() = HK25*HK4;
-		Hfusion.at<5>() = HK33*HK4;
-		Hfusion.at<6>() = HK37*HK4;
+	for (uint8_t index = 0; index <= 1; index++) {
+		if (index == 0) {
+			// everything was already computed above

-		// Optical flow Kalman gains - axis 0
-		Vector24f Kfusion;
-		Kfusion(0) = HK42*HK50;
-		Kfusion(1) = HK49*HK50;
-		Kfusion(2) = HK47*HK50;
-		Kfusion(3) = HK48*HK50;
-		Kfusion(4) = HK46*HK50;
-		Kfusion(5) = HK45*HK50;
-		Kfusion(6) = HK44*HK50;
+		} else if (index == 1) {
+			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
+			sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);

-		for (unsigned row = 7; row <= 23; row++) {
-			Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
+			// recalculate the innovation using the updated state
+			const Vector2f vel_body = predictFlowVelBody();
+			range = predictFlowRange();
+			_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
+
+			if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
+				// we need to reinitialise the covariance matrix and abort this fusion step
+				ECL_ERR("Opt flow error - covariance reset");
+				initialiseCovariance();
+				return;
+			}
 		}

-		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) {
-			fused[0] = true;
-			_fault_status.flags.bad_optflow_X = false;
+		SparseVector24f<0,1,2,3,4,5,6> Hfusion(H);
+		Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index];

-		} else {
-			_fault_status.flags.bad_optflow_X = true;
-			return;
+		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[index])) {
+			fused[index] = true;
 		}
 	}

-	{
-		// Optical flow observation Jacobians - axis 1
-		SparseVector24f<0,1,2,3,4,5,6> Hfusion;
-		Hfusion.at<0>() = -HK5*HK63;
-		Hfusion.at<1>() = -HK5*HK66;
-		Hfusion.at<2>() = -HK5*HK68;
-		Hfusion.at<3>() = -HK5*HK70;
-		Hfusion.at<4>() = -HK4*HK74;
-		Hfusion.at<5>() = -HK4*HK77;
-		Hfusion.at<6>() = -HK4*HK79;
-
-		// Optical flow Kalman gains - axis 1
-		Vector24f Kfusion;
-		Kfusion(0) = -HK87*HK95;
-		Kfusion(1) = -HK94*HK95;
-		Kfusion(2) = -HK91*HK95;
-		Kfusion(3) = -HK93*HK95;
-		Kfusion(4) = -HK90*HK95;
-		Kfusion(5) = -HK89*HK95;
-		Kfusion(6) = -HK88*HK95;
-
-		for (unsigned row = 7; row <= 23; row++) {
-			Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
-		}
-
-		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[1])) {
-			fused[1] = true;
-			_fault_status.flags.bad_optflow_Y = false;
-
-		} else {
-			_fault_status.flags.bad_optflow_Y = true;
-			return;
-		}
-	}
+	_fault_status.flags.bad_optflow_X = !fused[0];
+	_fault_status.flags.bad_optflow_Y = !fused[1];

 	if (fused[0] && fused[1]) {
 		_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
@@ -341,6 +156,39 @@ void Ekf::fuseOptFlow()
 	}
 }

+float Ekf::predictFlowRange()
+{
+	// calculate the sensor position relative to the IMU
+	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
+
+	// calculate the sensor position relative to the IMU in earth frame
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+
+	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
+	// a positive offset in earth frame leads to a smaller height above the ground.
+	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
+
+	// calculate range from focal point to centre of image
+	return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+}
+
+Vector2f Ekf::predictFlowVelBody()
+{
+	// calculate the sensor position relative to the IMU
+	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
+
+	// calculate the velocity of the sensor relative to the imu in body frame
+	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
+	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
+
+	// calculate the velocity of the sensor in the earth frame
+	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
+
+	// rotate into body frame
+	return _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
+}
+
+
 // calculate optical flow body angular rate compensation
 // returns false if bias corrected body rate data is unavailable
 bool Ekf::calcOptFlowBodyRateComp()
@@ -337,6 +337,56 @@ def compute_mag_declination_innov_innov_var_and_h(

    return (innov, innov_var, H.T)

+def predict_opt_flow(state, distance, epsilon):
+    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
+    R_to_earth = quat_to_rot(q_att)
+    R_to_body = R_to_earth.T
+
+    # Calculate earth relative velocity in a non-rotating sensor frame
+    v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
+    rel_vel_sensor = R_to_body * v
+
+    # Divide by range to get predicted angular LOS rates relative to X and Y
+    # axes. Note these are rates in a non-rotating sensor frame
+    flow_pred = sf.V2()
+    flow_pred[0] =  rel_vel_sensor[1] / distance
+    flow_pred[1] = -rel_vel_sensor[0] / distance
+    flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
+
+    return flow_pred
+
+
+def compute_flow_xy_innov_var_and_hx(
+        state: VState,
+        P: MState,
+        distance: sf.Scalar,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.V2, VState):
+    meas_pred = predict_opt_flow(state, distance, epsilon);
+
+    innov_var = sf.V2()
+    Hx = sf.V1(meas_pred[0]).jacobian(state)
+    innov_var[0] = (Hx * P * Hx.T + R)[0,0]
+    Hy = sf.V1(meas_pred[1]).jacobian(state)
+    innov_var[1] = (Hy * P * Hy.T + R)[0,0]
+
+    return (innov_var, Hx.T)
+
+def compute_flow_y_innov_var_and_h(
+        state: VState,
+        P: MState,
+        distance: sf.Scalar,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, VState):
+    meas_pred = predict_opt_flow(state, distance, epsilon);
+
+    Hy = sf.V1(meas_pred[1]).jacobian(state)
+    innov_var = (Hy * P * Hy.T + R)[0,0]
+
+    return (innov_var, Hy.T)
+
 print("Derive EKF2 equations...")
 generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
 generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
@@ -352,3 +402,5 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["
 generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
 generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])
+generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
+generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
@@ -0,0 +1,127 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_flow_xy_innov_var_and_hx
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     distance: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov_var: Matrix21
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
+                                const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
+                                const Scalar R, const Scalar epsilon,
+                                matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
+                                matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 285
+
+  // Input arrays
+
+  // Intermediate terms (29)
+  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp3 =
+      Scalar(1.0) /
+      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp4 = _tmp3 * (_tmp0 - _tmp1 + _tmp2);
+  const Scalar _tmp5 = 2 * state(3, 0);
+  const Scalar _tmp6 = _tmp5 * state(0, 0);
+  const Scalar _tmp7 = 2 * state(2, 0);
+  const Scalar _tmp8 = _tmp7 * state(1, 0);
+  const Scalar _tmp9 = _tmp3 * (-_tmp6 + _tmp8);
+  const Scalar _tmp10 = 2 * state(4, 0);
+  const Scalar _tmp11 = _tmp10 * state(0, 0);
+  const Scalar _tmp12 = 2 * state(5, 0);
+  const Scalar _tmp13 = _tmp12 * state(3, 0);
+  const Scalar _tmp14 = _tmp7 * state(6, 0);
+  const Scalar _tmp15 = _tmp3 * (-_tmp11 - _tmp13 + _tmp14);
+  const Scalar _tmp16 = 2 * state(1, 0);
+  const Scalar _tmp17 =
+      _tmp3 * (-_tmp10 * state(3, 0) + _tmp12 * state(0, 0) + _tmp16 * state(6, 0));
+  const Scalar _tmp18 = _tmp7 * state(4, 0);
+  const Scalar _tmp19 = _tmp12 * state(1, 0);
+  const Scalar _tmp20 = 2 * state(0, 0) * state(6, 0);
+  const Scalar _tmp21 = _tmp3 * (_tmp18 - _tmp19 + _tmp20);
+  const Scalar _tmp22 = _tmp3 * (_tmp10 * state(1, 0) + _tmp5 * state(6, 0) + _tmp7 * state(5, 0));
+  const Scalar _tmp23 = _tmp3 * (_tmp16 * state(0, 0) + _tmp7 * state(3, 0));
+  const Scalar _tmp24 = _tmp3 * (-_tmp0 + _tmp1 + _tmp2);
+  const Scalar _tmp25 = _tmp3 * (_tmp6 + _tmp8);
+  const Scalar _tmp26 = _tmp3 * (_tmp16 * state(3, 0) - _tmp7 * state(0, 0));
+  const Scalar _tmp27 = _tmp3 * (_tmp11 + _tmp13 - _tmp14);
+  const Scalar _tmp28 = _tmp3 * (-_tmp18 + _tmp19 - _tmp20);
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
+
+    _innov_var(0, 0) =
+        R +
+        _tmp15 * (P(0, 3) * _tmp17 + P(1, 3) * _tmp21 + P(2, 3) * _tmp22 + P(3, 3) * _tmp15 +
+                  P(4, 3) * _tmp9 + P(5, 3) * _tmp4 + P(6, 3) * _tmp23) +
+        _tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp21 + P(2, 0) * _tmp22 + P(3, 0) * _tmp15 +
+                  P(4, 0) * _tmp9 + P(5, 0) * _tmp4 + P(6, 0) * _tmp23) +
+        _tmp21 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp21 + P(2, 1) * _tmp22 + P(3, 1) * _tmp15 +
+                  P(4, 1) * _tmp9 + P(5, 1) * _tmp4 + P(6, 1) * _tmp23) +
+        _tmp22 * (P(0, 2) * _tmp17 + P(1, 2) * _tmp21 + P(2, 2) * _tmp22 + P(3, 2) * _tmp15 +
+                  P(4, 2) * _tmp9 + P(5, 2) * _tmp4 + P(6, 2) * _tmp23) +
+        _tmp23 * (P(0, 6) * _tmp17 + P(1, 6) * _tmp21 + P(2, 6) * _tmp22 + P(3, 6) * _tmp15 +
+                  P(4, 6) * _tmp9 + P(5, 6) * _tmp4 + P(6, 6) * _tmp23) +
+        _tmp4 * (P(0, 5) * _tmp17 + P(1, 5) * _tmp21 + P(2, 5) * _tmp22 + P(3, 5) * _tmp15 +
+                 P(4, 5) * _tmp9 + P(5, 5) * _tmp4 + P(6, 5) * _tmp23) +
+        _tmp9 * (P(0, 4) * _tmp17 + P(1, 4) * _tmp21 + P(2, 4) * _tmp22 + P(3, 4) * _tmp15 +
+                 P(4, 4) * _tmp9 + P(5, 4) * _tmp4 + P(6, 4) * _tmp23);
+    _innov_var(1, 0) =
+        R -
+        _tmp17 * (-P(0, 3) * _tmp27 - P(1, 3) * _tmp22 - P(2, 3) * _tmp28 - P(3, 3) * _tmp17 -
+                  P(4, 3) * _tmp24 - P(5, 3) * _tmp25 - P(6, 3) * _tmp26) -
+        _tmp22 * (-P(0, 1) * _tmp27 - P(1, 1) * _tmp22 - P(2, 1) * _tmp28 - P(3, 1) * _tmp17 -
+                  P(4, 1) * _tmp24 - P(5, 1) * _tmp25 - P(6, 1) * _tmp26) -
+        _tmp24 * (-P(0, 4) * _tmp27 - P(1, 4) * _tmp22 - P(2, 4) * _tmp28 - P(3, 4) * _tmp17 -
+                  P(4, 4) * _tmp24 - P(5, 4) * _tmp25 - P(6, 4) * _tmp26) -
+        _tmp25 * (-P(0, 5) * _tmp27 - P(1, 5) * _tmp22 - P(2, 5) * _tmp28 - P(3, 5) * _tmp17 -
+                  P(4, 5) * _tmp24 - P(5, 5) * _tmp25 - P(6, 5) * _tmp26) -
+        _tmp26 * (-P(0, 6) * _tmp27 - P(1, 6) * _tmp22 - P(2, 6) * _tmp28 - P(3, 6) * _tmp17 -
+                  P(4, 6) * _tmp24 - P(5, 6) * _tmp25 - P(6, 6) * _tmp26) -
+        _tmp27 * (-P(0, 0) * _tmp27 - P(1, 0) * _tmp22 - P(2, 0) * _tmp28 - P(3, 0) * _tmp17 -
+                  P(4, 0) * _tmp24 - P(5, 0) * _tmp25 - P(6, 0) * _tmp26) -
+        _tmp28 * (-P(0, 2) * _tmp27 - P(1, 2) * _tmp22 - P(2, 2) * _tmp28 - P(3, 2) * _tmp17 -
+                  P(4, 2) * _tmp24 - P(5, 2) * _tmp25 - P(6, 2) * _tmp26);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(0, 0) = _tmp17;
+    _H(1, 0) = _tmp21;
+    _H(2, 0) = _tmp22;
+    _H(3, 0) = _tmp15;
+    _H(4, 0) = _tmp9;
+    _H(5, 0) = _tmp4;
+    _H(6, 0) = _tmp23;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -0,0 +1,95 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_flow_y_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     distance: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
+                              const Scalar R, const Scalar epsilon,
+                              Scalar* const innov_var = nullptr,
+                              matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 171
+
+  // Input arrays
+
+  // Intermediate terms (13)
+  const Scalar _tmp0 =
+      Scalar(1.0) /
+      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp1 =
+      _tmp0 * (std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) -
+               std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)));
+  const Scalar _tmp2 = 2 * state(0, 0);
+  const Scalar _tmp3 = 2 * state(1, 0);
+  const Scalar _tmp4 = _tmp0 * (_tmp2 * state(3, 0) + _tmp3 * state(2, 0));
+  const Scalar _tmp5 = _tmp0 * (-_tmp2 * state(2, 0) + _tmp3 * state(3, 0));
+  const Scalar _tmp6 = 2 * state(4, 0);
+  const Scalar _tmp7 = 2 * state(6, 0);
+  const Scalar _tmp8 = _tmp0 * (_tmp2 * state(5, 0) - _tmp6 * state(3, 0) + _tmp7 * state(1, 0));
+  const Scalar _tmp9 = 2 * state(5, 0);
+  const Scalar _tmp10 = _tmp0 * (_tmp2 * state(4, 0) - _tmp7 * state(2, 0) + _tmp9 * state(3, 0));
+  const Scalar _tmp11 = _tmp0 * (_tmp3 * state(4, 0) + _tmp7 * state(3, 0) + _tmp9 * state(2, 0));
+  const Scalar _tmp12 = _tmp0 * (_tmp3 * state(5, 0) - _tmp6 * state(2, 0) - _tmp7 * state(0, 0));
+
+  // Output terms (2)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R -
+                 _tmp1 * (-P(0, 4) * _tmp10 - P(1, 4) * _tmp11 - P(2, 4) * _tmp12 -
+                          P(3, 4) * _tmp8 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
+                 _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp11 - P(2, 0) * _tmp12 -
+                           P(3, 0) * _tmp8 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
+                 _tmp11 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp11 - P(2, 1) * _tmp12 -
+                           P(3, 1) * _tmp8 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
+                 _tmp12 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp11 - P(2, 2) * _tmp12 -
+                           P(3, 2) * _tmp8 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
+                 _tmp4 * (-P(0, 5) * _tmp10 - P(1, 5) * _tmp11 - P(2, 5) * _tmp12 -
+                          P(3, 5) * _tmp8 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
+                 _tmp5 * (-P(0, 6) * _tmp10 - P(1, 6) * _tmp11 - P(2, 6) * _tmp12 -
+                          P(3, 6) * _tmp8 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
+                 _tmp8 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp11 - P(2, 3) * _tmp12 -
+                          P(3, 3) * _tmp8 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(0, 0) = -_tmp10;
+    _H(1, 0) = -_tmp11;
+    _H(2, 0) = -_tmp12;
+    _H(3, 0) = -_tmp8;
+    _H(4, 0) = -_tmp1;
+    _H(5, 0) = -_tmp4;
+    _H(6, 0) = -_tmp5;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -1,640 +0,0 @@
-#include <math.h>
-#include <stdio.h>
-#include <cstdlib>
-#include "../../../../../matrix/matrix/math.hpp"
-#include "util.h"
-
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
-
-template<int ... Idxs>
-using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
-
-int main()
-{
-    // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
-
-	SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
-    Vector24f Kfusion; // Optical flow observation Kalman gains
-
-    Vector24f Hfusion_sympy;
-    Vector24f Kfusion_sympy;
-
-    Vector24f Hfusion_matlab;
-    Vector24f Kfusion_matlab;
-
-	const float R_LOS = sq(0.15f);
-
-    float flow_innov_var;
-
-    // quaternion inputs must be normalised
-    float q0 = 2.0f * ((float)rand() - 0.5f);
-    float q1 = 2.0f * ((float)rand() - 0.5f);
-    float q2 = 2.0f * ((float)rand() - 0.5f);
-    float q3 = 2.0f * ((float)rand() - 0.5f);
-    const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
-    q0 /= length;
-    q1 /= length;
-    q2 /= length;
-    q3 /= length;
-
-	// get latest velocity in earth frame
-	const float vn = 10.0f * 2.0f * ((float)rand() - 0.5f);
-	const float ve = 10.0f * 2.0f * ((float)rand() - 0.5f);
-	const float vd = 2.0f * ((float)rand() - 0.5f);
-
-    const float range = 5.0f;
-
-    matrix::Dcmf Tbs;
-	Tbs.identity();
-
-    // create a symmetrical positive definite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
-    for (int col=0; col<=23; col++) {
-        for (int row=0; row<=col; row++) {
-            if (row == col) {
-                P(row,col) = (float)rand();
-            } else {
-                P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
-            }
-        }
-    }
-
-    // evaluate sub expressions used by sympy code
-    const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
-    const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
-    const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
-    const float HK3 = HK0*vd + HK1*ve + HK2*vn;
-    const float HK4 = 1.0F/range;
-    const float HK5 = 2*HK4;
-    const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
-    const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
-    const float HK8 = HK0*vn - HK2*vd + HK6*ve;
-    const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
-    const float HK10 = q0*q2;
-    const float HK11 = q1*q3;
-    const float HK12 = HK10 + HK11;
-    const float HK13 = 2*Tbs(1,2);
-    const float HK14 = q0*q3;
-    const float HK15 = q1*q2;
-    const float HK16 = HK14 - HK15;
-    const float HK17 = 2*Tbs(1,1);
-    const float HK18 = ecl::powf(q1, 2);
-    const float HK19 = ecl::powf(q2, 2);
-    const float HK20 = -HK19;
-    const float HK21 = ecl::powf(q0, 2);
-    const float HK22 = ecl::powf(q3, 2);
-    const float HK23 = HK21 - HK22;
-    const float HK24 = HK18 + HK20 + HK23;
-    const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
-    const float HK26 = HK14 + HK15;
-    const float HK27 = 2*Tbs(1,0);
-    const float HK28 = q0*q1;
-    const float HK29 = q2*q3;
-    const float HK30 = HK28 - HK29;
-    const float HK31 = -HK18;
-    const float HK32 = HK19 + HK23 + HK31;
-    const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
-    const float HK34 = HK28 + HK29;
-    const float HK35 = HK10 - HK11;
-    const float HK36 = HK20 + HK21 + HK22 + HK31;
-    const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
-    const float HK38 = 2*HK3;
-    const float HK39 = 2*HK7;
-    const float HK40 = 2*HK8;
-    const float HK41 = 2*HK9;
-    const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
-    const float HK43 = ecl::powf(range, -2);
-    const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
-    const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
-    const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
-    const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
-    const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
-    const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
-    const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
-
-    const float HK51 = Tbs(0,1)*q1;
-    const float HK52 = Tbs(0,2)*q0;
-    const float HK53 = Tbs(0,0)*q2;
-    const float HK54 = HK51 + HK52 - HK53;
-    const float HK55 = Tbs(0,0)*q3;
-    const float HK56 = Tbs(0,1)*q0;
-    const float HK57 = Tbs(0,2)*q1;
-    const float HK58 = HK55 + HK56 - HK57;
-    const float HK59 = Tbs(0,0)*q0;
-    const float HK60 = Tbs(0,2)*q2;
-    const float HK61 = Tbs(0,1)*q3;
-    const float HK62 = HK59 + HK60 - HK61;
-    const float HK63 = HK54*vd + HK58*ve + HK62*vn;
-    const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
-    const float HK65 = HK58*vd + HK64*vn;
-    const float HK66 = -HK54*ve + HK65;
-    const float HK67 = HK54*vn + HK64*ve;
-    const float HK68 = -HK62*vd + HK67;
-    const float HK69 = HK62*ve + HK64*vd;
-    const float HK70 = -HK58*vn + HK69;
-    const float HK71 = 2*Tbs(0,1);
-    const float HK72 = 2*Tbs(0,2);
-    const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
-    const float HK74 = -HK16*HK71 + HK73;
-    const float HK75 = 2*Tbs(0,0);
-    const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
-    const float HK77 = -HK30*HK72 + HK76;
-    const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
-    const float HK79 = -HK35*HK75 + HK78;
-    const float HK80 = 2*HK63;
-    const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
-    const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
-    const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
-    const float HK84 = HK71*(-HK14 + HK15) + HK73;
-    const float HK85 = HK72*(-HK28 + HK29) + HK76;
-    const float HK86 = HK75*(-HK10 + HK11) + HK78;
-    const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
-    const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
-    const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
-    const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
-    const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
-    const float HK92 = 2*HK43;
-    const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
-    const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
-    const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
-
-
-    // Compare X axis equations
-    {
-        // evaluate sympy genrated equations for observatio Jacobians and Kalman gains
-        {
-            // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
-            flow_innov_var = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
-
-            const float HK50 = HK4/flow_innov_var;
-
-            // Observation Jacobians - axis 0
-            Hfusion.at<0>() = HK3*HK5;
-            Hfusion.at<1>() = HK5*HK7;
-            Hfusion.at<2>() = HK5*HK8;
-            Hfusion.at<3>() = HK5*HK9;
-            Hfusion.at<4>() = HK25*HK4;
-            Hfusion.at<5>() = HK33*HK4;
-            Hfusion.at<6>() = HK37*HK4;
-
-			// Kalman gains - axis 0
-			Kfusion(0) = HK42*HK50;
-			Kfusion(1) = HK49*HK50;
-			Kfusion(2) = HK47*HK50;
-			Kfusion(3) = HK48*HK50;
-			Kfusion(4) = HK46*HK50;
-			Kfusion(5) = HK45*HK50;
-			Kfusion(6) = HK44*HK50;
-
-			for (unsigned row = 7; row <= 23; row++) {
-				Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
-			}
-
-            // copy to arrays used for comparison
-            for (int row=0; row<7; row++) {
-                Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
-            }
-            for (int row=0; row<24; row++) {
-                Kfusion_sympy(row) = Kfusion(row);
-            }
-
-        }
-
-        // repeat calculation using matlab generated equations
-
-        {
-            // calculate X axis observation Jacobian
-            float t2 = 1.0f / range;
-            float H_LOS[24] = {};
-            H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
-            H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
-            H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
-            H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
-            H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
-            H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
-            H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
-
-            // calculate intermediate variables for the X observation innovatoin variance and Kalman gains
-            float t3 = q1*vd*2.0f;
-            float t4 = q0*ve*2.0f;
-            float t11 = q3*vn*2.0f;
-            float t5 = t3+t4-t11;
-            float t6 = q0*q3*2.0f;
-            float t29 = q1*q2*2.0f;
-            float t7 = t6-t29;
-            float t8 = q0*q1*2.0f;
-            float t9 = q2*q3*2.0f;
-            float t10 = t8+t9;
-            float t12 = P(0,0)*t2*t5;
-            float t13 = q0*vd*2.0f;
-            float t14 = q2*vn*2.0f;
-            float t28 = q1*ve*2.0f;
-            float t15 = t13+t14-t28;
-            float t16 = q3*vd*2.0f;
-            float t17 = q2*ve*2.0f;
-            float t18 = q1*vn*2.0f;
-            float t19 = t16+t17+t18;
-            float t20 = q3*ve*2.0f;
-            float t21 = q0*vn*2.0f;
-            float t30 = q2*vd*2.0f;
-            float t22 = t20+t21-t30;
-            float t23 = q0*q0;
-            float t24 = q1*q1;
-            float t25 = q2*q2;
-            float t26 = q3*q3;
-            float t27 = t23-t24+t25-t26;
-            float t31 = P(1,1)*t2*t15;
-            float t32 = P(6,0)*t2*t10;
-            float t33 = P(1,0)*t2*t15;
-            float t34 = P(2,0)*t2*t19;
-            float t35 = P(5,0)*t2*t27;
-            float t79 = P(4,0)*t2*t7;
-            float t80 = P(3,0)*t2*t22;
-            float t36 = t12+t32+t33+t34+t35-t79-t80;
-            float t37 = t2*t5*t36;
-            float t38 = P(6,1)*t2*t10;
-            float t39 = P(0,1)*t2*t5;
-            float t40 = P(2,1)*t2*t19;
-            float t41 = P(5,1)*t2*t27;
-            float t81 = P(4,1)*t2*t7;
-            float t82 = P(3,1)*t2*t22;
-            float t42 = t31+t38+t39+t40+t41-t81-t82;
-            float t43 = t2*t15*t42;
-            float t44 = P(6,2)*t2*t10;
-            float t45 = P(0,2)*t2*t5;
-            float t46 = P(1,2)*t2*t15;
-            float t47 = P(2,2)*t2*t19;
-            float t48 = P(5,2)*t2*t27;
-            float t83 = P(4,2)*t2*t7;
-            float t84 = P(3,2)*t2*t22;
-            float t49 = t44+t45+t46+t47+t48-t83-t84;
-            float t50 = t2*t19*t49;
-            float t51 = P(6,3)*t2*t10;
-            float t52 = P(0,3)*t2*t5;
-            float t53 = P(1,3)*t2*t15;
-            float t54 = P(2,3)*t2*t19;
-            float t55 = P(5,3)*t2*t27;
-            float t85 = P(4,3)*t2*t7;
-            float t86 = P(3,3)*t2*t22;
-            float t56 = t51+t52+t53+t54+t55-t85-t86;
-            float t57 = P(6,5)*t2*t10;
-            float t58 = P(0,5)*t2*t5;
-            float t59 = P(1,5)*t2*t15;
-            float t60 = P(2,5)*t2*t19;
-            float t61 = P(5,5)*t2*t27;
-            float t88 = P(4,5)*t2*t7;
-            float t89 = P(3,5)*t2*t22;
-            float t62 = t57+t58+t59+t60+t61-t88-t89;
-            float t63 = t2*t27*t62;
-            float t64 = P(6,4)*t2*t10;
-            float t65 = P(0,4)*t2*t5;
-            float t66 = P(1,4)*t2*t15;
-            float t67 = P(2,4)*t2*t19;
-            float t68 = P(5,4)*t2*t27;
-            float t90 = P(4,4)*t2*t7;
-            float t91 = P(3,4)*t2*t22;
-            float t69 = t64+t65+t66+t67+t68-t90-t91;
-            float t70 = P(6,6)*t2*t10;
-            float t71 = P(0,6)*t2*t5;
-            float t72 = P(1,6)*t2*t15;
-            float t73 = P(2,6)*t2*t19;
-            float t74 = P(5,6)*t2*t27;
-            float t93 = P(4,6)*t2*t7;
-            float t94 = P(3,6)*t2*t22;
-            float t75 = t70+t71+t72+t73+t74-t93-t94;
-            float t76 = t2*t10*t75;
-            float t87 = t2*t22*t56;
-            float t92 = t2*t7*t69;
-            float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
-            float t78 = 1.0f / t77;
-            flow_innov_var = t77;
-
-            // calculate Kalman gains for X-axis observation
-            float Kfusion[24];
-            Kfusion[0] = t78*(t12-P(0,4)*t2*t7+P(0,1)*t2*t15+P(0,6)*t2*t10+P(0,2)*t2*t19-P(0,3)*t2*t22+P(0,5)*t2*t27);
-            Kfusion[1] = t78*(t31+P(1,0)*t2*t5-P(1,4)*t2*t7+P(1,6)*t2*t10+P(1,2)*t2*t19-P(1,3)*t2*t22+P(1,5)*t2*t27);
-            Kfusion[2] = t78*(t47+P(2,0)*t2*t5-P(2,4)*t2*t7+P(2,1)*t2*t15+P(2,6)*t2*t10-P(2,3)*t2*t22+P(2,5)*t2*t27);
-            Kfusion[3] = t78*(-t86+P(3,0)*t2*t5-P(3,4)*t2*t7+P(3,1)*t2*t15+P(3,6)*t2*t10+P(3,2)*t2*t19+P(3,5)*t2*t27);
-            Kfusion[4] = t78*(-t90+P(4,0)*t2*t5+P(4,1)*t2*t15+P(4,6)*t2*t10+P(4,2)*t2*t19-P(4,3)*t2*t22+P(4,5)*t2*t27);
-            Kfusion[5] = t78*(t61+P(5,0)*t2*t5-P(5,4)*t2*t7+P(5,1)*t2*t15+P(5,6)*t2*t10+P(5,2)*t2*t19-P(5,3)*t2*t22);
-            Kfusion[6] = t78*(t70+P(6,0)*t2*t5-P(6,4)*t2*t7+P(6,1)*t2*t15+P(6,2)*t2*t19-P(6,3)*t2*t22+P(6,5)*t2*t27);
-            Kfusion[7] = t78*(P(7,0)*t2*t5-P(7,4)*t2*t7+P(7,1)*t2*t15+P(7,6)*t2*t10+P(7,2)*t2*t19-P(7,3)*t2*t22+P(7,5)*t2*t27);
-            Kfusion[8] = t78*(P(8,0)*t2*t5-P(8,4)*t2*t7+P(8,1)*t2*t15+P(8,6)*t2*t10+P(8,2)*t2*t19-P(8,3)*t2*t22+P(8,5)*t2*t27);
-            Kfusion[9] = t78*(P(9,0)*t2*t5-P(9,4)*t2*t7+P(9,1)*t2*t15+P(9,6)*t2*t10+P(9,2)*t2*t19-P(9,3)*t2*t22+P(9,5)*t2*t27);
-            Kfusion[10] = t78*(P(10,0)*t2*t5-P(10,4)*t2*t7+P(10,1)*t2*t15+P(10,6)*t2*t10+P(10,2)*t2*t19-P(10,3)*t2*t22+P(10,5)*t2*t27);
-            Kfusion[11] = t78*(P(11,0)*t2*t5-P(11,4)*t2*t7+P(11,1)*t2*t15+P(11,6)*t2*t10+P(11,2)*t2*t19-P(11,3)*t2*t22+P(11,5)*t2*t27);
-            Kfusion[12] = t78*(P(12,0)*t2*t5-P(12,4)*t2*t7+P(12,1)*t2*t15+P(12,6)*t2*t10+P(12,2)*t2*t19-P(12,3)*t2*t22+P(12,5)*t2*t27);
-            Kfusion[13] = t78*(P(13,0)*t2*t5-P(13,4)*t2*t7+P(13,1)*t2*t15+P(13,6)*t2*t10+P(13,2)*t2*t19-P(13,3)*t2*t22+P(13,5)*t2*t27);
-            Kfusion[14] = t78*(P(14,0)*t2*t5-P(14,4)*t2*t7+P(14,1)*t2*t15+P(14,6)*t2*t10+P(14,2)*t2*t19-P(14,3)*t2*t22+P(14,5)*t2*t27);
-            Kfusion[15] = t78*(P(15,0)*t2*t5-P(15,4)*t2*t7+P(15,1)*t2*t15+P(15,6)*t2*t10+P(15,2)*t2*t19-P(15,3)*t2*t22+P(15,5)*t2*t27);
-            Kfusion[16] = t78*(P(16,0)*t2*t5-P(16,4)*t2*t7+P(16,1)*t2*t15+P(16,6)*t2*t10+P(16,2)*t2*t19-P(16,3)*t2*t22+P(16,5)*t2*t27);
-            Kfusion[17] = t78*(P(17,0)*t2*t5-P(17,4)*t2*t7+P(17,1)*t2*t15+P(17,6)*t2*t10+P(17,2)*t2*t19-P(17,3)*t2*t22+P(17,5)*t2*t27);
-            Kfusion[18] = t78*(P(18,0)*t2*t5-P(18,4)*t2*t7+P(18,1)*t2*t15+P(18,6)*t2*t10+P(18,2)*t2*t19-P(18,3)*t2*t22+P(18,5)*t2*t27);
-            Kfusion[19] = t78*(P(19,0)*t2*t5-P(19,4)*t2*t7+P(19,1)*t2*t15+P(19,6)*t2*t10+P(19,2)*t2*t19-P(19,3)*t2*t22+P(19,5)*t2*t27);
-            Kfusion[20] = t78*(P(20,0)*t2*t5-P(20,4)*t2*t7+P(20,1)*t2*t15+P(20,6)*t2*t10+P(20,2)*t2*t19-P(20,3)*t2*t22+P(20,5)*t2*t27);
-            Kfusion[21] = t78*(P(21,0)*t2*t5-P(21,4)*t2*t7+P(21,1)*t2*t15+P(21,6)*t2*t10+P(21,2)*t2*t19-P(21,3)*t2*t22+P(21,5)*t2*t27);
-            Kfusion[22] = t78*(P(22,0)*t2*t5-P(22,4)*t2*t7+P(22,1)*t2*t15+P(22,6)*t2*t10+P(22,2)*t2*t19-P(22,3)*t2*t22+P(22,5)*t2*t27);
-            Kfusion[23] = t78*(P(23,0)*t2*t5-P(23,4)*t2*t7+P(23,1)*t2*t15+P(23,6)*t2*t10+P(23,2)*t2*t19-P(23,3)*t2*t22+P(23,5)*t2*t27);
-
-            for (int row=0; row<24; row++) {
-                Hfusion_matlab(row) = H_LOS[row];
-                Kfusion_matlab(row) = Kfusion[row];
-            }
-        }
-
-        // find largest observation variance difference as a fraction of the matlab value
-        float max_diff_fraction = 0.0f;
-        int max_row;
-        float max_old, max_new;
-        for (int row=0; row<24; row++) {
-            float diff_fraction;
-            if (Hfusion_matlab(row) != 0.0f) {
-                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
-            } else if (Hfusion_sympy(row) != 0.0f) {
-                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
-            } else {
-                diff_fraction = 0.0f;
-            }
-            if (diff_fraction > max_diff_fraction) {
-                max_diff_fraction = diff_fraction;
-                max_row = row;
-                max_old = Hfusion_matlab(row);
-                max_new = Hfusion_sympy(row);
-            }
-        }
-
-        if (max_diff_fraction > 1e-5f) {
-            printf("Fail: Optical Flow X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
-        } else {
-            printf("Pass: Optical Flow X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
-        }
-
-        // find largest Kalman gain difference as a fraction of the matlab value
-        max_diff_fraction = 0.0f;
-        for (int row=0; row<24; row++) {
-            float diff_fraction;
-            if (Kfusion_matlab(row) != 0.0f) {
-                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
-            } else if (Hfusion_sympy(row) != 0.0f) {
-                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row));
-            } else {
-                diff_fraction = 0.0f;
-            }
-            if (diff_fraction > max_diff_fraction) {
-                max_diff_fraction = diff_fraction;
-                max_row = row;
-                max_old = Kfusion_matlab(row);
-                max_new = Kfusion_sympy(row);
-            }
-        }
-
-        if (max_diff_fraction > 1e-5f) {
-            printf("Fail: Optical Flow X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
-        } else {
-            printf("Pass: Optical Flow X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
-        }
-
-    }
-
-    // Compare Y axis equations
-
-    {
-        // evaluate sympy genrated equations for observatio Jacobians and Kalman gains
-        {
-            // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
-            flow_innov_var = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
-
-            const float HK95 = HK4/flow_innov_var;
-
-			// Observation Jacobians - axis 1
-			Hfusion.at<0>() = -HK5*HK63;
-			Hfusion.at<1>() = -HK5*HK66;
-			Hfusion.at<2>() = -HK5*HK68;
-			Hfusion.at<3>() = -HK5*HK70;
-			Hfusion.at<4>() = -HK4*HK74;
-			Hfusion.at<5>() = -HK4*HK77;
-			Hfusion.at<6>() = -HK4*HK79;
-
-			// Kalman gains - axis 1
-			Kfusion(0) = -HK87*HK95;
-			Kfusion(1) = -HK94*HK95;
-			Kfusion(2) = -HK91*HK95;
-			Kfusion(3) = -HK93*HK95;
-			Kfusion(4) = -HK90*HK95;
-			Kfusion(5) = -HK89*HK95;
-			Kfusion(6) = -HK88*HK95;
-
-			for (unsigned row = 7; row <= 23; row++) {
-				Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
-			}
-
-            // copy to arrays used for comparison
-            for (int row=0; row<7; row++) {
-                Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
-            }
-            for (int row=0; row<24; row++) {
-                Kfusion_sympy(row) = Kfusion(row);
-            }
-
-        }
-
-        // repeat calculation using matlab generated equations
-
-        {
-			// calculate Y axis observation Jacobian
-			float t2 = 1.0f / range;
-            float H_LOS[24] = {};
-			H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
-			H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
-			H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
-			H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
-			H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
-			H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
-			H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
-
-			// calculate intermediate variables for the Y observation innovatoin variance and Kalman gains
-			float t3 = q3*ve*2.0f;
-			float t4 = q0*vn*2.0f;
-			float t11 = q2*vd*2.0f;
-			float t5 = t3+t4-t11;
-			float t6 = q0*q3*2.0f;
-			float t7 = q1*q2*2.0f;
-			float t8 = t6+t7;
-			float t9 = q0*q2*2.0f;
-			float t28 = q1*q3*2.0f;
-			float t10 = t9-t28;
-			float t12 = P(0,0)*t2*t5;
-			float t13 = q3*vd*2.0f;
-			float t14 = q2*ve*2.0f;
-			float t15 = q1*vn*2.0f;
-			float t16 = t13+t14+t15;
-			float t17 = q0*vd*2.0f;
-			float t18 = q2*vn*2.0f;
-			float t29 = q1*ve*2.0f;
-			float t19 = t17+t18-t29;
-			float t20 = q1*vd*2.0f;
-			float t21 = q0*ve*2.0f;
-			float t30 = q3*vn*2.0f;
-			float t22 = t20+t21-t30;
-			float t23 = q0*q0;
-			float t24 = q1*q1;
-			float t25 = q2*q2;
-			float t26 = q3*q3;
-			float t27 = t23+t24-t25-t26;
-			float t31 = P(1,1)*t2*t16;
-			float t32 = P(5,0)*t2*t8;
-			float t33 = P(1,0)*t2*t16;
-			float t34 = P(3,0)*t2*t22;
-			float t35 = P(4,0)*t2*t27;
-			float t80 = P(6,0)*t2*t10;
-			float t81 = P(2,0)*t2*t19;
-			float t36 = t12+t32+t33+t34+t35-t80-t81;
-			float t37 = t2*t5*t36;
-			float t38 = P(5,1)*t2*t8;
-			float t39 = P(0,1)*t2*t5;
-			float t40 = P(3,1)*t2*t22;
-			float t41 = P(4,1)*t2*t27;
-			float t82 = P(6,1)*t2*t10;
-			float t83 = P(2,1)*t2*t19;
-			float t42 = t31+t38+t39+t40+t41-t82-t83;
-			float t43 = t2*t16*t42;
-			float t44 = P(5,2)*t2*t8;
-			float t45 = P(0,2)*t2*t5;
-			float t46 = P(1,2)*t2*t16;
-			float t47 = P(3,2)*t2*t22;
-			float t48 = P(4,2)*t2*t27;
-			float t79 = P(2,2)*t2*t19;
-			float t84 = P(6,2)*t2*t10;
-			float t49 = t44+t45+t46+t47+t48-t79-t84;
-			float t50 = P(5,3)*t2*t8;
-			float t51 = P(0,3)*t2*t5;
-			float t52 = P(1,3)*t2*t16;
-			float t53 = P(3,3)*t2*t22;
-			float t54 = P(4,3)*t2*t27;
-			float t86 = P(6,3)*t2*t10;
-			float t87 = P(2,3)*t2*t19;
-			float t55 = t50+t51+t52+t53+t54-t86-t87;
-			float t56 = t2*t22*t55;
-			float t57 = P(5,4)*t2*t8;
-			float t58 = P(0,4)*t2*t5;
-			float t59 = P(1,4)*t2*t16;
-			float t60 = P(3,4)*t2*t22;
-			float t61 = P(4,4)*t2*t27;
-			float t88 = P(6,4)*t2*t10;
-			float t89 = P(2,4)*t2*t19;
-			float t62 = t57+t58+t59+t60+t61-t88-t89;
-			float t63 = t2*t27*t62;
-			float t64 = P(5,5)*t2*t8;
-			float t65 = P(0,5)*t2*t5;
-			float t66 = P(1,5)*t2*t16;
-			float t67 = P(3,5)*t2*t22;
-			float t68 = P(4,5)*t2*t27;
-			float t90 = P(6,5)*t2*t10;
-			float t91 = P(2,5)*t2*t19;
-			float t69 = t64+t65+t66+t67+t68-t90-t91;
-			float t70 = t2*t8*t69;
-			float t71 = P(5,6)*t2*t8;
-			float t72 = P(0,6)*t2*t5;
-			float t73 = P(1,6)*t2*t16;
-			float t74 = P(3,6)*t2*t22;
-			float t75 = P(4,6)*t2*t27;
-			float t92 = P(6,6)*t2*t10;
-			float t93 = P(2,6)*t2*t19;
-			float t76 = t71+t72+t73+t74+t75-t92-t93;
-			float t85 = t2*t19*t49;
-			float t94 = t2*t10*t76;
-			float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
-
-			float t78 = 1.0f / t77;
-			flow_innov_var = t77;
-
-			// calculate Kalman gains for Y-axis observation
-            float Kfusion[24];
-			Kfusion[0] = -t78*(t12+P(0,5)*t2*t8-P(0,6)*t2*t10+P(0,1)*t2*t16-P(0,2)*t2*t19+P(0,3)*t2*t22+P(0,4)*t2*t27);
-			Kfusion[1] = -t78*(t31+P(1,0)*t2*t5+P(1,5)*t2*t8-P(1,6)*t2*t10-P(1,2)*t2*t19+P(1,3)*t2*t22+P(1,4)*t2*t27);
-			Kfusion[2] = -t78*(-t79+P(2,0)*t2*t5+P(2,5)*t2*t8-P(2,6)*t2*t10+P(2,1)*t2*t16+P(2,3)*t2*t22+P(2,4)*t2*t27);
-			Kfusion[3] = -t78*(t53+P(3,0)*t2*t5+P(3,5)*t2*t8-P(3,6)*t2*t10+P(3,1)*t2*t16-P(3,2)*t2*t19+P(3,4)*t2*t27);
-			Kfusion[4] = -t78*(t61+P(4,0)*t2*t5+P(4,5)*t2*t8-P(4,6)*t2*t10+P(4,1)*t2*t16-P(4,2)*t2*t19+P(4,3)*t2*t22);
-			Kfusion[5] = -t78*(t64+P(5,0)*t2*t5-P(5,6)*t2*t10+P(5,1)*t2*t16-P(5,2)*t2*t19+P(5,3)*t2*t22+P(5,4)*t2*t27);
-			Kfusion[6] = -t78*(-t92+P(6,0)*t2*t5+P(6,5)*t2*t8+P(6,1)*t2*t16-P(6,2)*t2*t19+P(6,3)*t2*t22+P(6,4)*t2*t27);
-			Kfusion[7] = -t78*(P(7,0)*t2*t5+P(7,5)*t2*t8-P(7,6)*t2*t10+P(7,1)*t2*t16-P(7,2)*t2*t19+P(7,3)*t2*t22+P(7,4)*t2*t27);
-			Kfusion[8] = -t78*(P(8,0)*t2*t5+P(8,5)*t2*t8-P(8,6)*t2*t10+P(8,1)*t2*t16-P(8,2)*t2*t19+P(8,3)*t2*t22+P(8,4)*t2*t27);
-			Kfusion[9] = -t78*(P(9,0)*t2*t5+P(9,5)*t2*t8-P(9,6)*t2*t10+P(9,1)*t2*t16-P(9,2)*t2*t19+P(9,3)*t2*t22+P(9,4)*t2*t27);
-			Kfusion[10] = -t78*(P(10,0)*t2*t5+P(10,5)*t2*t8-P(10,6)*t2*t10+P(10,1)*t2*t16-P(10,2)*t2*t19+P(10,3)*t2*t22+P(10,4)*t2*t27);
-			Kfusion[11] = -t78*(P(11,0)*t2*t5+P(11,5)*t2*t8-P(11,6)*t2*t10+P(11,1)*t2*t16-P(11,2)*t2*t19+P(11,3)*t2*t22+P(11,4)*t2*t27);
-			Kfusion[12] = -t78*(P(12,0)*t2*t5+P(12,5)*t2*t8-P(12,6)*t2*t10+P(12,1)*t2*t16-P(12,2)*t2*t19+P(12,3)*t2*t22+P(12,4)*t2*t27);
-			Kfusion[13] = -t78*(P(13,0)*t2*t5+P(13,5)*t2*t8-P(13,6)*t2*t10+P(13,1)*t2*t16-P(13,2)*t2*t19+P(13,3)*t2*t22+P(13,4)*t2*t27);
-			Kfusion[14] = -t78*(P(14,0)*t2*t5+P(14,5)*t2*t8-P(14,6)*t2*t10+P(14,1)*t2*t16-P(14,2)*t2*t19+P(14,3)*t2*t22+P(14,4)*t2*t27);
-			Kfusion[15] = -t78*(P(15,0)*t2*t5+P(15,5)*t2*t8-P(15,6)*t2*t10+P(15,1)*t2*t16-P(15,2)*t2*t19+P(15,3)*t2*t22+P(15,4)*t2*t27);
-			Kfusion[16] = -t78*(P(16,0)*t2*t5+P(16,5)*t2*t8-P(16,6)*t2*t10+P(16,1)*t2*t16-P(16,2)*t2*t19+P(16,3)*t2*t22+P(16,4)*t2*t27);
-			Kfusion[17] = -t78*(P(17,0)*t2*t5+P(17,5)*t2*t8-P(17,6)*t2*t10+P(17,1)*t2*t16-P(17,2)*t2*t19+P(17,3)*t2*t22+P(17,4)*t2*t27);
-			Kfusion[18] = -t78*(P(18,0)*t2*t5+P(18,5)*t2*t8-P(18,6)*t2*t10+P(18,1)*t2*t16-P(18,2)*t2*t19+P(18,3)*t2*t22+P(18,4)*t2*t27);
-			Kfusion[19] = -t78*(P(19,0)*t2*t5+P(19,5)*t2*t8-P(19,6)*t2*t10+P(19,1)*t2*t16-P(19,2)*t2*t19+P(19,3)*t2*t22+P(19,4)*t2*t27);
-			Kfusion[20] = -t78*(P(20,0)*t2*t5+P(20,5)*t2*t8-P(20,6)*t2*t10+P(20,1)*t2*t16-P(20,2)*t2*t19+P(20,3)*t2*t22+P(20,4)*t2*t27);
-			Kfusion[21] = -t78*(P(21,0)*t2*t5+P(21,5)*t2*t8-P(21,6)*t2*t10+P(21,1)*t2*t16-P(21,2)*t2*t19+P(21,3)*t2*t22+P(21,4)*t2*t27);
-			Kfusion[22] = -t78*(P(22,0)*t2*t5+P(22,5)*t2*t8-P(22,6)*t2*t10+P(22,1)*t2*t16-P(22,2)*t2*t19+P(22,3)*t2*t22+P(22,4)*t2*t27);
-			Kfusion[23] = -t78*(P(23,0)*t2*t5+P(23,5)*t2*t8-P(23,6)*t2*t10+P(23,1)*t2*t16-P(23,2)*t2*t19+P(23,3)*t2*t22+P(23,4)*t2*t27);
-
-            for (int row=0; row<24; row++) {
-                Hfusion_matlab(row) = H_LOS[row];
-                Kfusion_matlab(row) = Kfusion[row];
-            }
-        }
-
-        // find largest observation variance difference as a fraction of the matlab value
-        float max_diff_fraction = 0.0f;
-        int max_row;
-        float max_old, max_new;
-        for (int row=0; row<24; row++) {
-            float diff_fraction;
-            if (Hfusion_matlab(row) != 0.0f) {
-                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
-            } else if (Hfusion_sympy(row) != 0.0f) {
-                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
-            } else {
-                diff_fraction = 0.0f;
-            }
-            if (diff_fraction > max_diff_fraction) {
-                max_diff_fraction = diff_fraction;
-                max_row = row;
-                max_old = Hfusion_matlab(row);
-                max_new = Hfusion_sympy(row);
-            }
-        }
-
-        if (max_diff_fraction > 1e-5f) {
-            printf("Fail: Optical Flow Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
-        } else {
-            printf("Pass: Optical Flow Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
-        }
-
-        // find largest Kalman gain difference as a fraction of the matlab value
-        max_diff_fraction = 0.0f;
-        for (int row=0; row<24; row++) {
-            float diff_fraction;
-            if (Kfusion_matlab(row) != 0.0f) {
-                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
-            } else if (Hfusion_sympy(row) != 0.0f) {
-                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row));
-            } else {
-                diff_fraction = 0.0f;
-            }
-            if (diff_fraction > max_diff_fraction) {
-                max_diff_fraction = diff_fraction;
-                max_row = row;
-                max_old = Kfusion_matlab(row);
-                max_new = Kfusion_sympy(row);
-            }
-        }
-
-        if (max_diff_fraction > 1e-5f) {
-            printf("Fail: Optical Flow Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
-        } else {
-            printf("Pass: Optical Flow Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
-        }
-
-    }
-
-    return 0;
-}
@@ -1,184 +0,0 @@
-// X Axis Equations
-// Sub Expressions
-const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
-const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
-const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
-const float HK3 = HK0*vd + HK1*ve + HK2*vn;
-const float HK4 = 1.0F/(range);
-const float HK5 = 2*HK4;
-const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
-const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
-const float HK8 = HK0*vn - HK2*vd + HK6*ve;
-const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
-const float HK10 = q0*q2;
-const float HK11 = q1*q3;
-const float HK12 = 2*Tbs(1,2);
-const float HK13 = q0*q3;
-const float HK14 = q1*q2;
-const float HK15 = 2*Tbs(1,1);
-const float HK16 = (q1)*(q1);
-const float HK17 = (q2)*(q2);
-const float HK18 = -HK17;
-const float HK19 = (q0)*(q0);
-const float HK20 = (q3)*(q3);
-const float HK21 = HK19 - HK20;
-const float HK22 = HK12*(HK10 + HK11) - HK15*(HK13 - HK14) + Tbs(1,0)*(HK16 + HK18 + HK21);
-const float HK23 = 2*Tbs(1,0);
-const float HK24 = q0*q1;
-const float HK25 = q2*q3;
-const float HK26 = -HK16;
-const float HK27 = -HK12*(HK24 - HK25) + HK23*(HK13 + HK14) + Tbs(1,1)*(HK17 + HK21 + HK26);
-const float HK28 = HK15*(HK24 + HK25) - HK23*(HK10 - HK11) + Tbs(1,2)*(HK18 + HK19 + HK20 + HK26);
-const float HK29 = 2*HK3;
-const float HK30 = 2*HK7;
-const float HK31 = 2*HK8;
-const float HK32 = 2*HK9;
-const float HK33 = HK22*P(0,4) + HK27*P(0,5) + HK28*P(0,6) + HK29*P(0,0) + HK30*P(0,1) + HK31*P(0,2) + HK32*P(0,3);
-const float HK34 = 1.0F/((range)*(range));
-const float HK35 = HK22*P(4,6) + HK27*P(5,6) + HK28*P(6,6) + HK29*P(0,6) + HK30*P(1,6) + HK31*P(2,6) + HK32*P(3,6);
-const float HK36 = HK22*P(4,5) + HK27*P(5,5) + HK28*P(5,6) + HK29*P(0,5) + HK30*P(1,5) + HK31*P(2,5) + HK32*P(3,5);
-const float HK37 = HK22*P(4,4) + HK27*P(4,5) + HK28*P(4,6) + HK29*P(0,4) + HK30*P(1,4) + HK31*P(2,4) + HK32*P(3,4);
-const float HK38 = HK22*P(2,4) + HK27*P(2,5) + HK28*P(2,6) + HK29*P(0,2) + HK30*P(1,2) + HK31*P(2,2) + HK32*P(2,3);
-const float HK39 = HK22*P(3,4) + HK27*P(3,5) + HK28*P(3,6) + HK29*P(0,3) + HK30*P(1,3) + HK31*P(2,3) + HK32*P(3,3);
-const float HK40 = HK22*P(1,4) + HK27*P(1,5) + HK28*P(1,6) + HK29*P(0,1) + HK30*P(1,1) + HK31*P(1,2) + HK32*P(1,3);
-const float HK41 = HK4/(HK22*HK34*HK37 + HK27*HK34*HK36 + HK28*HK34*HK35 + HK29*HK33*HK34 + HK30*HK34*HK40 + HK31*HK34*HK38 + HK32*HK34*HK39 + R_LOS);
-
-
-// Observation Jacobians
-Hfusion.at<0>() = HK3*HK5;
-Hfusion.at<1>() = HK5*HK7;
-Hfusion.at<2>() = HK5*HK8;
-Hfusion.at<3>() = HK5*HK9;
-Hfusion.at<4>() = HK22*HK4;
-Hfusion.at<5>() = HK27*HK4;
-Hfusion.at<6>() = HK28*HK4;
-
-
-// Kalman gains
-Kfusion(0) = HK33*HK41;
-Kfusion(1) = HK40*HK41;
-Kfusion(2) = HK38*HK41;
-Kfusion(3) = HK39*HK41;
-Kfusion(4) = HK37*HK41;
-Kfusion(5) = HK36*HK41;
-Kfusion(6) = HK35*HK41;
-Kfusion(7) = HK41*(HK22*P(4,7) + HK27*P(5,7) + HK28*P(6,7) + HK29*P(0,7) + HK30*P(1,7) + HK31*P(2,7) + HK32*P(3,7));
-Kfusion(8) = HK41*(HK22*P(4,8) + HK27*P(5,8) + HK28*P(6,8) + HK29*P(0,8) + HK30*P(1,8) + HK31*P(2,8) + HK32*P(3,8));
-Kfusion(9) = HK41*(HK22*P(4,9) + HK27*P(5,9) + HK28*P(6,9) + HK29*P(0,9) + HK30*P(1,9) + HK31*P(2,9) + HK32*P(3,9));
-Kfusion(10) = HK41*(HK22*P(4,10) + HK27*P(5,10) + HK28*P(6,10) + HK29*P(0,10) + HK30*P(1,10) + HK31*P(2,10) + HK32*P(3,10));
-Kfusion(11) = HK41*(HK22*P(4,11) + HK27*P(5,11) + HK28*P(6,11) + HK29*P(0,11) + HK30*P(1,11) + HK31*P(2,11) + HK32*P(3,11));
-Kfusion(12) = HK41*(HK22*P(4,12) + HK27*P(5,12) + HK28*P(6,12) + HK29*P(0,12) + HK30*P(1,12) + HK31*P(2,12) + HK32*P(3,12));
-Kfusion(13) = HK41*(HK22*P(4,13) + HK27*P(5,13) + HK28*P(6,13) + HK29*P(0,13) + HK30*P(1,13) + HK31*P(2,13) + HK32*P(3,13));
-Kfusion(14) = HK41*(HK22*P(4,14) + HK27*P(5,14) + HK28*P(6,14) + HK29*P(0,14) + HK30*P(1,14) + HK31*P(2,14) + HK32*P(3,14));
-Kfusion(15) = HK41*(HK22*P(4,15) + HK27*P(5,15) + HK28*P(6,15) + HK29*P(0,15) + HK30*P(1,15) + HK31*P(2,15) + HK32*P(3,15));
-Kfusion(16) = HK41*(HK22*P(4,16) + HK27*P(5,16) + HK28*P(6,16) + HK29*P(0,16) + HK30*P(1,16) + HK31*P(2,16) + HK32*P(3,16));
-Kfusion(17) = HK41*(HK22*P(4,17) + HK27*P(5,17) + HK28*P(6,17) + HK29*P(0,17) + HK30*P(1,17) + HK31*P(2,17) + HK32*P(3,17));
-Kfusion(18) = HK41*(HK22*P(4,18) + HK27*P(5,18) + HK28*P(6,18) + HK29*P(0,18) + HK30*P(1,18) + HK31*P(2,18) + HK32*P(3,18));
-Kfusion(19) = HK41*(HK22*P(4,19) + HK27*P(5,19) + HK28*P(6,19) + HK29*P(0,19) + HK30*P(1,19) + HK31*P(2,19) + HK32*P(3,19));
-Kfusion(20) = HK41*(HK22*P(4,20) + HK27*P(5,20) + HK28*P(6,20) + HK29*P(0,20) + HK30*P(1,20) + HK31*P(2,20) + HK32*P(3,20));
-Kfusion(21) = HK41*(HK22*P(4,21) + HK27*P(5,21) + HK28*P(6,21) + HK29*P(0,21) + HK30*P(1,21) + HK31*P(2,21) + HK32*P(3,21));
-Kfusion(22) = HK41*(HK22*P(4,22) + HK27*P(5,22) + HK28*P(6,22) + HK29*P(0,22) + HK30*P(1,22) + HK31*P(2,22) + HK32*P(3,22));
-Kfusion(23) = HK41*(HK22*P(4,23) + HK27*P(5,23) + HK28*P(6,23) + HK29*P(0,23) + HK30*P(1,23) + HK31*P(2,23) + HK32*P(3,23));
-
-
-// Predicted observation
-
-
-// Innovation variance
-
-
-// Y Axis Equations
-// Sub Expressions
-const float HK0 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
-const float HK1 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
-const float HK2 = Tbs(0,0)*q0 - Tbs(0,1)*q3 + Tbs(0,2)*q2;
-const float HK3 = HK0*vd + HK1*ve + HK2*vn;
-const float HK4 = 1.0F/(range);
-const float HK5 = 2*HK4;
-const float HK6 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
-const float HK7 = HK1*vd + HK6*vn;
-const float HK8 = HK0*vn + HK6*ve;
-const float HK9 = HK2*ve + HK6*vd;
-const float HK10 = q0*q3;
-const float HK11 = q1*q2;
-const float HK12 = HK10 - HK11;
-const float HK13 = 2*Tbs(0,1);
-const float HK14 = (q1)*(q1);
-const float HK15 = (q2)*(q2);
-const float HK16 = -HK15;
-const float HK17 = (q0)*(q0);
-const float HK18 = (q3)*(q3);
-const float HK19 = HK17 - HK18;
-const float HK20 = q0*q2;
-const float HK21 = q1*q3;
-const float HK22 = 2*Tbs(0,2);
-const float HK23 = HK22*(HK20 + HK21) + Tbs(0,0)*(HK14 + HK16 + HK19);
-const float HK24 = q0*q1;
-const float HK25 = q2*q3;
-const float HK26 = HK24 - HK25;
-const float HK27 = -HK14;
-const float HK28 = 2*Tbs(0,0);
-const float HK29 = HK28*(HK10 + HK11) + Tbs(0,1)*(HK15 + HK19 + HK27);
-const float HK30 = HK20 - HK21;
-const float HK31 = HK13*(HK24 + HK25) + Tbs(0,2)*(HK16 + HK17 + HK18 + HK27);
-const float HK32 = 2*HK3;
-const float HK33 = -2*HK0*ve + 2*HK7;
-const float HK34 = -2*HK2*vd + 2*HK8;
-const float HK35 = -2*HK1*vn + 2*HK9;
-const float HK36 = -HK12*HK13 + HK23;
-const float HK37 = -HK22*HK26 + HK29;
-const float HK38 = -HK28*HK30 + HK31;
-const float HK39 = HK32*P(0,0) + HK33*P(0,1) + HK34*P(0,2) + HK35*P(0,3) + HK36*P(0,4) + HK37*P(0,5) + HK38*P(0,6);
-const float HK40 = 1.0F/((range)*(range));
-const float HK41 = HK32*P(0,6) + HK33*P(1,6) + HK34*P(2,6) + HK35*P(3,6) + HK36*P(4,6) + HK37*P(5,6) + HK38*P(6,6);
-const float HK42 = HK32*P(0,5) + HK33*P(1,5) + HK34*P(2,5) + HK35*P(3,5) + HK36*P(4,5) + HK37*P(5,5) + HK38*P(5,6);
-const float HK43 = HK32*P(0,4) + HK33*P(1,4) + HK34*P(2,4) + HK35*P(3,4) + HK36*P(4,4) + HK37*P(4,5) + HK38*P(4,6);
-const float HK44 = HK32*P(0,2) + HK33*P(1,2) + HK34*P(2,2) + HK35*P(2,3) + HK36*P(2,4) + HK37*P(2,5) + HK38*P(2,6);
-const float HK45 = HK32*P(0,3) + HK33*P(1,3) + HK34*P(2,3) + HK35*P(3,3) + HK36*P(3,4) + HK37*P(3,5) + HK38*P(3,6);
-const float HK46 = HK32*P(0,1) + HK33*P(1,1) + HK34*P(1,2) + HK35*P(1,3) + HK36*P(1,4) + HK37*P(1,5) + HK38*P(1,6);
-const float HK47 = HK4/(HK32*HK39*HK40 + HK33*HK40*HK46 + HK34*HK40*HK44 + HK35*HK40*HK45 + HK36*HK40*HK43 + HK37*HK40*HK42 + HK38*HK40*HK41 + R_LOS);
-
-
-// Observation Jacobians
-Hfusion.at<0>() = -HK3*HK5;
-Hfusion.at<1>() = -HK5*(-HK0*ve + HK7);
-Hfusion.at<2>() = -HK5*(-HK2*vd + HK8);
-Hfusion.at<3>() = -HK5*(-HK1*vn + HK9);
-Hfusion.at<4>() = -HK4*(-HK12*HK13 + HK23);
-Hfusion.at<5>() = -HK4*(-HK22*HK26 + HK29);
-Hfusion.at<6>() = -HK4*(-HK28*HK30 + HK31);
-
-
-// Kalman gains
-Kfusion(0) = -HK39*HK47;
-Kfusion(1) = -HK46*HK47;
-Kfusion(2) = -HK44*HK47;
-Kfusion(3) = -HK45*HK47;
-Kfusion(4) = -HK43*HK47;
-Kfusion(5) = -HK42*HK47;
-Kfusion(6) = -HK41*HK47;
-Kfusion(7) = -HK47*(HK32*P(0,7) + HK33*P(1,7) + HK34*P(2,7) + HK35*P(3,7) + HK36*P(4,7) + HK37*P(5,7) + HK38*P(6,7));
-Kfusion(8) = -HK47*(HK32*P(0,8) + HK33*P(1,8) + HK34*P(2,8) + HK35*P(3,8) + HK36*P(4,8) + HK37*P(5,8) + HK38*P(6,8));
-Kfusion(9) = -HK47*(HK32*P(0,9) + HK33*P(1,9) + HK34*P(2,9) + HK35*P(3,9) + HK36*P(4,9) + HK37*P(5,9) + HK38*P(6,9));
-Kfusion(10) = -HK47*(HK32*P(0,10) + HK33*P(1,10) + HK34*P(2,10) + HK35*P(3,10) + HK36*P(4,10) + HK37*P(5,10) + HK38*P(6,10));
-Kfusion(11) = -HK47*(HK32*P(0,11) + HK33*P(1,11) + HK34*P(2,11) + HK35*P(3,11) + HK36*P(4,11) + HK37*P(5,11) + HK38*P(6,11));
-Kfusion(12) = -HK47*(HK32*P(0,12) + HK33*P(1,12) + HK34*P(2,12) + HK35*P(3,12) + HK36*P(4,12) + HK37*P(5,12) + HK38*P(6,12));
-Kfusion(13) = -HK47*(HK32*P(0,13) + HK33*P(1,13) + HK34*P(2,13) + HK35*P(3,13) + HK36*P(4,13) + HK37*P(5,13) + HK38*P(6,13));
-Kfusion(14) = -HK47*(HK32*P(0,14) + HK33*P(1,14) + HK34*P(2,14) + HK35*P(3,14) + HK36*P(4,14) + HK37*P(5,14) + HK38*P(6,14));
-Kfusion(15) = -HK47*(HK32*P(0,15) + HK33*P(1,15) + HK34*P(2,15) + HK35*P(3,15) + HK36*P(4,15) + HK37*P(5,15) + HK38*P(6,15));
-Kfusion(16) = -HK47*(HK32*P(0,16) + HK33*P(1,16) + HK34*P(2,16) + HK35*P(3,16) + HK36*P(4,16) + HK37*P(5,16) + HK38*P(6,16));
-Kfusion(17) = -HK47*(HK32*P(0,17) + HK33*P(1,17) + HK34*P(2,17) + HK35*P(3,17) + HK36*P(4,17) + HK37*P(5,17) + HK38*P(6,17));
-Kfusion(18) = -HK47*(HK32*P(0,18) + HK33*P(1,18) + HK34*P(2,18) + HK35*P(3,18) + HK36*P(4,18) + HK37*P(5,18) + HK38*P(6,18));
-Kfusion(19) = -HK47*(HK32*P(0,19) + HK33*P(1,19) + HK34*P(2,19) + HK35*P(3,19) + HK36*P(4,19) + HK37*P(5,19) + HK38*P(6,19));
-Kfusion(20) = -HK47*(HK32*P(0,20) + HK33*P(1,20) + HK34*P(2,20) + HK35*P(3,20) + HK36*P(4,20) + HK37*P(5,20) + HK38*P(6,20));
-Kfusion(21) = -HK47*(HK32*P(0,21) + HK33*P(1,21) + HK34*P(2,21) + HK35*P(3,21) + HK36*P(4,21) + HK37*P(5,21) + HK38*P(6,21));
-Kfusion(22) = -HK47*(HK32*P(0,22) + HK33*P(1,22) + HK34*P(2,22) + HK35*P(3,22) + HK36*P(4,22) + HK37*P(5,22) + HK38*P(6,22));
-Kfusion(23) = -HK47*(HK32*P(0,23) + HK33*P(1,23) + HK34*P(2,23) + HK35*P(3,23) + HK36*P(4,23) + HK37*P(5,23) + HK38*P(6,23));
-
-
-// Predicted observation
-
-
-// Innovation variance
-
-
@@ -1,157 +0,0 @@
-// Sub Expressions
-const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
-const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
-const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
-const float HK3 = HK0*vd + HK1*ve + HK2*vn;
-const float HK4 = 1.0F/(range);
-const float HK5 = 2*HK4;
-const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
-const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
-const float HK8 = HK0*vn - HK2*vd + HK6*ve;
-const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
-const float HK10 = q0*q2;
-const float HK11 = q1*q3;
-const float HK12 = HK10 + HK11;
-const float HK13 = 2*Tbs(1,2);
-const float HK14 = q0*q3;
-const float HK15 = q1*q2;
-const float HK16 = HK14 - HK15;
-const float HK17 = 2*Tbs(1,1);
-const float HK18 = (q1)*(q1);
-const float HK19 = (q2)*(q2);
-const float HK20 = -HK19;
-const float HK21 = (q0)*(q0);
-const float HK22 = (q3)*(q3);
-const float HK23 = HK21 - HK22;
-const float HK24 = HK18 + HK20 + HK23;
-const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
-const float HK26 = HK14 + HK15;
-const float HK27 = 2*Tbs(1,0);
-const float HK28 = q0*q1;
-const float HK29 = q2*q3;
-const float HK30 = HK28 - HK29;
-const float HK31 = -HK18;
-const float HK32 = HK19 + HK23 + HK31;
-const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
-const float HK34 = HK28 + HK29;
-const float HK35 = HK10 - HK11;
-const float HK36 = HK20 + HK21 + HK22 + HK31;
-const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
-const float HK38 = 2*HK3;
-const float HK39 = 2*HK7;
-const float HK40 = 2*HK8;
-const float HK41 = 2*HK9;
-const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
-const float HK43 = 1.0F/((range)*(range));
-const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
-const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
-const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
-const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
-const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
-const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
-const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
-const float HK51 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
-const float HK52 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
-const float HK53 = Tbs(0,0)*q0 - Tbs(0,1)*q3 + Tbs(0,2)*q2;
-const float HK54 = HK51*vd + HK52*ve + HK53*vn;
-const float HK55 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
-const float HK56 = HK52*vd + HK55*vn;
-const float HK57 = HK51*vn + HK55*ve;
-const float HK58 = HK53*ve + HK55*vd;
-const float HK59 = 2*Tbs(0,1);
-const float HK60 = 2*Tbs(0,2);
-const float HK61 = HK12*HK60 + HK24*Tbs(0,0);
-const float HK62 = 2*Tbs(0,0);
-const float HK63 = HK26*HK62 + HK32*Tbs(0,1);
-const float HK64 = HK34*HK59 + HK36*Tbs(0,2);
-const float HK65 = 2*HK54;
-const float HK66 = -2*HK51*ve + 2*HK56;
-const float HK67 = -2*HK53*vd + 2*HK57;
-const float HK68 = -2*HK52*vn + 2*HK58;
-const float HK69 = -HK16*HK59 + HK61;
-const float HK70 = -HK30*HK60 + HK63;
-const float HK71 = -HK35*HK62 + HK64;
-const float HK72 = HK65*P(0,0) + HK66*P(0,1) + HK67*P(0,2) + HK68*P(0,3) + HK69*P(0,4) + HK70*P(0,5) + HK71*P(0,6);
-const float HK73 = HK65*P(0,6) + HK66*P(1,6) + HK67*P(2,6) + HK68*P(3,6) + HK69*P(4,6) + HK70*P(5,6) + HK71*P(6,6);
-const float HK74 = HK65*P(0,5) + HK66*P(1,5) + HK67*P(2,5) + HK68*P(3,5) + HK69*P(4,5) + HK70*P(5,5) + HK71*P(5,6);
-const float HK75 = HK65*P(0,4) + HK66*P(1,4) + HK67*P(2,4) + HK68*P(3,4) + HK69*P(4,4) + HK70*P(4,5) + HK71*P(4,6);
-const float HK76 = HK65*P(0,2) + HK66*P(1,2) + HK67*P(2,2) + HK68*P(2,3) + HK69*P(2,4) + HK70*P(2,5) + HK71*P(2,6);
-const float HK77 = HK65*P(0,3) + HK66*P(1,3) + HK67*P(2,3) + HK68*P(3,3) + HK69*P(3,4) + HK70*P(3,5) + HK71*P(3,6);
-const float HK78 = HK65*P(0,1) + HK66*P(1,1) + HK67*P(1,2) + HK68*P(1,3) + HK69*P(1,4) + HK70*P(1,5) + HK71*P(1,6);
-const float HK79 = HK4/(HK43*HK65*HK72 + HK43*HK66*HK78 + HK43*HK67*HK76 + HK43*HK68*HK77 + HK43*HK69*HK75 + HK43*HK70*HK74 + HK43*HK71*HK73 + R_LOS);
-
-
-// Observation Jacobians - axis 0
-Hfusion.at<0>() = HK3*HK5;
-Hfusion.at<1>() = HK5*HK7;
-Hfusion.at<2>() = HK5*HK8;
-Hfusion.at<3>() = HK5*HK9;
-Hfusion.at<4>() = HK25*HK4;
-Hfusion.at<5>() = HK33*HK4;
-Hfusion.at<6>() = HK37*HK4;
-
-
-// Kalman gains - axis 0
-Kfusion(0) = HK42*HK50;
-Kfusion(1) = HK49*HK50;
-Kfusion(2) = HK47*HK50;
-Kfusion(3) = HK48*HK50;
-Kfusion(4) = HK46*HK50;
-Kfusion(5) = HK45*HK50;
-Kfusion(6) = HK44*HK50;
-Kfusion(7) = HK50*(HK25*P(4,7) + HK33*P(5,7) + HK37*P(6,7) + HK38*P(0,7) + HK39*P(1,7) + HK40*P(2,7) + HK41*P(3,7));
-Kfusion(8) = HK50*(HK25*P(4,8) + HK33*P(5,8) + HK37*P(6,8) + HK38*P(0,8) + HK39*P(1,8) + HK40*P(2,8) + HK41*P(3,8));
-Kfusion(9) = HK50*(HK25*P(4,9) + HK33*P(5,9) + HK37*P(6,9) + HK38*P(0,9) + HK39*P(1,9) + HK40*P(2,9) + HK41*P(3,9));
-Kfusion(10) = HK50*(HK25*P(4,10) + HK33*P(5,10) + HK37*P(6,10) + HK38*P(0,10) + HK39*P(1,10) + HK40*P(2,10) + HK41*P(3,10));
-Kfusion(11) = HK50*(HK25*P(4,11) + HK33*P(5,11) + HK37*P(6,11) + HK38*P(0,11) + HK39*P(1,11) + HK40*P(2,11) + HK41*P(3,11));
-Kfusion(12) = HK50*(HK25*P(4,12) + HK33*P(5,12) + HK37*P(6,12) + HK38*P(0,12) + HK39*P(1,12) + HK40*P(2,12) + HK41*P(3,12));
-Kfusion(13) = HK50*(HK25*P(4,13) + HK33*P(5,13) + HK37*P(6,13) + HK38*P(0,13) + HK39*P(1,13) + HK40*P(2,13) + HK41*P(3,13));
-Kfusion(14) = HK50*(HK25*P(4,14) + HK33*P(5,14) + HK37*P(6,14) + HK38*P(0,14) + HK39*P(1,14) + HK40*P(2,14) + HK41*P(3,14));
-Kfusion(15) = HK50*(HK25*P(4,15) + HK33*P(5,15) + HK37*P(6,15) + HK38*P(0,15) + HK39*P(1,15) + HK40*P(2,15) + HK41*P(3,15));
-Kfusion(16) = HK50*(HK25*P(4,16) + HK33*P(5,16) + HK37*P(6,16) + HK38*P(0,16) + HK39*P(1,16) + HK40*P(2,16) + HK41*P(3,16));
-Kfusion(17) = HK50*(HK25*P(4,17) + HK33*P(5,17) + HK37*P(6,17) + HK38*P(0,17) + HK39*P(1,17) + HK40*P(2,17) + HK41*P(3,17));
-Kfusion(18) = HK50*(HK25*P(4,18) + HK33*P(5,18) + HK37*P(6,18) + HK38*P(0,18) + HK39*P(1,18) + HK40*P(2,18) + HK41*P(3,18));
-Kfusion(19) = HK50*(HK25*P(4,19) + HK33*P(5,19) + HK37*P(6,19) + HK38*P(0,19) + HK39*P(1,19) + HK40*P(2,19) + HK41*P(3,19));
-Kfusion(20) = HK50*(HK25*P(4,20) + HK33*P(5,20) + HK37*P(6,20) + HK38*P(0,20) + HK39*P(1,20) + HK40*P(2,20) + HK41*P(3,20));
-Kfusion(21) = HK50*(HK25*P(4,21) + HK33*P(5,21) + HK37*P(6,21) + HK38*P(0,21) + HK39*P(1,21) + HK40*P(2,21) + HK41*P(3,21));
-Kfusion(22) = HK50*(HK25*P(4,22) + HK33*P(5,22) + HK37*P(6,22) + HK38*P(0,22) + HK39*P(1,22) + HK40*P(2,22) + HK41*P(3,22));
-Kfusion(23) = HK50*(HK25*P(4,23) + HK33*P(5,23) + HK37*P(6,23) + HK38*P(0,23) + HK39*P(1,23) + HK40*P(2,23) + HK41*P(3,23));
-
-
-// Observation Jacobians - axis 1
-Hfusion.at<0>() = -HK5*HK54;
-Hfusion.at<1>() = -HK5*(-HK51*ve + HK56);
-Hfusion.at<2>() = -HK5*(-HK53*vd + HK57);
-Hfusion.at<3>() = -HK5*(-HK52*vn + HK58);
-Hfusion.at<4>() = -HK4*(-HK16*HK59 + HK61);
-Hfusion.at<5>() = -HK4*(-HK30*HK60 + HK63);
-Hfusion.at<6>() = -HK4*(-HK35*HK62 + HK64);
-
-
-// Kalman gains - axis 1
-Kfusion(0) = -HK72*HK79;
-Kfusion(1) = -HK78*HK79;
-Kfusion(2) = -HK76*HK79;
-Kfusion(3) = -HK77*HK79;
-Kfusion(4) = -HK75*HK79;
-Kfusion(5) = -HK74*HK79;
-Kfusion(6) = -HK73*HK79;
-Kfusion(7) = -HK79*(HK65*P(0,7) + HK66*P(1,7) + HK67*P(2,7) + HK68*P(3,7) + HK69*P(4,7) + HK70*P(5,7) + HK71*P(6,7));
-Kfusion(8) = -HK79*(HK65*P(0,8) + HK66*P(1,8) + HK67*P(2,8) + HK68*P(3,8) + HK69*P(4,8) + HK70*P(5,8) + HK71*P(6,8));
-Kfusion(9) = -HK79*(HK65*P(0,9) + HK66*P(1,9) + HK67*P(2,9) + HK68*P(3,9) + HK69*P(4,9) + HK70*P(5,9) + HK71*P(6,9));
-Kfusion(10) = -HK79*(HK65*P(0,10) + HK66*P(1,10) + HK67*P(2,10) + HK68*P(3,10) + HK69*P(4,10) + HK70*P(5,10) + HK71*P(6,10));
-Kfusion(11) = -HK79*(HK65*P(0,11) + HK66*P(1,11) + HK67*P(2,11) + HK68*P(3,11) + HK69*P(4,11) + HK70*P(5,11) + HK71*P(6,11));
-Kfusion(12) = -HK79*(HK65*P(0,12) + HK66*P(1,12) + HK67*P(2,12) + HK68*P(3,12) + HK69*P(4,12) + HK70*P(5,12) + HK71*P(6,12));
-Kfusion(13) = -HK79*(HK65*P(0,13) + HK66*P(1,13) + HK67*P(2,13) + HK68*P(3,13) + HK69*P(4,13) + HK70*P(5,13) + HK71*P(6,13));
-Kfusion(14) = -HK79*(HK65*P(0,14) + HK66*P(1,14) + HK67*P(2,14) + HK68*P(3,14) + HK69*P(4,14) + HK70*P(5,14) + HK71*P(6,14));
-Kfusion(15) = -HK79*(HK65*P(0,15) + HK66*P(1,15) + HK67*P(2,15) + HK68*P(3,15) + HK69*P(4,15) + HK70*P(5,15) + HK71*P(6,15));
-Kfusion(16) = -HK79*(HK65*P(0,16) + HK66*P(1,16) + HK67*P(2,16) + HK68*P(3,16) + HK69*P(4,16) + HK70*P(5,16) + HK71*P(6,16));
-Kfusion(17) = -HK79*(HK65*P(0,17) + HK66*P(1,17) + HK67*P(2,17) + HK68*P(3,17) + HK69*P(4,17) + HK70*P(5,17) + HK71*P(6,17));
-Kfusion(18) = -HK79*(HK65*P(0,18) + HK66*P(1,18) + HK67*P(2,18) + HK68*P(3,18) + HK69*P(4,18) + HK70*P(5,18) + HK71*P(6,18));
-Kfusion(19) = -HK79*(HK65*P(0,19) + HK66*P(1,19) + HK67*P(2,19) + HK68*P(3,19) + HK69*P(4,19) + HK70*P(5,19) + HK71*P(6,19));
-Kfusion(20) = -HK79*(HK65*P(0,20) + HK66*P(1,20) + HK67*P(2,20) + HK68*P(3,20) + HK69*P(4,20) + HK70*P(5,20) + HK71*P(6,20));
-Kfusion(21) = -HK79*(HK65*P(0,21) + HK66*P(1,21) + HK67*P(2,21) + HK68*P(3,21) + HK69*P(4,21) + HK70*P(5,21) + HK71*P(6,21));
-Kfusion(22) = -HK79*(HK65*P(0,22) + HK66*P(1,22) + HK67*P(2,22) + HK68*P(3,22) + HK69*P(4,22) + HK70*P(5,22) + HK71*P(6,22));
-Kfusion(23) = -HK79*(HK65*P(0,23) + HK66*P(1,23) + HK67*P(2,23) + HK68*P(3,23) + HK69*P(4,23) + HK70*P(5,23) + HK71*P(6,23));
-
-
@@ -45,15 +45,16 @@ void Ekf::controlZeroVelocityUpdate()

 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
-				&& _control_status_prev.flags.vehicle_at_rest;
+				&& _control_status_prev.flags.vehicle_at_rest
+				&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift

 		if (continuing_conditions_passing) {
 			Vector3f vel_obs{0, 0, 0};
 			Vector3f innovation = _state.vel - vel_obs;

-			// Set a low variance initially for faster accel bias learning and higher
+			// Set a low variance initially for faster leveling and higher
 			// later to let the states follow the measurements
-			const float obs_var = _NED_origin_initialised ? sq(0.2f) : sq(0.001f);
+			const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f);
 			Vector3f innov_var{
 				P(4, 4) + obs_var,
 				P(5, 5) + obs_var,
@@ -530,53 +530,6 @@ void EKF2::Run()
 			_last_time_slip_us = 0;
 		}

-		// update all other topics if they have new data
-		if (_status_sub.updated()) {
-			vehicle_status_s vehicle_status;
-
-			if (_status_sub.copy(&vehicle_status)) {
-				const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-
-				// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
-				_ekf.set_is_fixed_wing(is_fixed_wing);
-
-				_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
-						vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
-			}
-		}
-
-		if (_vehicle_land_detected_sub.updated()) {
-			vehicle_land_detected_s vehicle_land_detected;
-
-			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
-
-				_ekf.set_vehicle_at_rest(vehicle_land_detected.at_rest);
-
-				if (vehicle_land_detected.in_ground_effect) {
-					_ekf.set_gnd_effect();
-				}
-
-				const bool was_in_air = _ekf.control_status_flags().in_air;
-				const bool in_air = !vehicle_land_detected.landed;
-
-				if (!was_in_air && in_air) {
-					// takeoff
-					_ekf.set_in_air_status(true);
-
-					// reset learned sensor calibrations on takeoff
-					_accel_cal = {};
-					_gyro_cal = {};
-					_mag_cal = {};
-
-					_preflt_checker.reset();
-
-				} else if (was_in_air && !in_air) {
-					// landed
-					_ekf.set_in_air_status(false);
-				}
-			}
-		}
-
 		// ekf2_timestamps (using 0.1 ms relative timestamps)
 		ekf2_timestamps_s ekf2_timestamps {
 			.timestamp = now,
@@ -595,6 +548,7 @@ void EKF2::Run()
 		UpdateGpsSample(ekf2_timestamps);
 		UpdateMagSample(ekf2_timestamps);
 		UpdateRangeSample(ekf2_timestamps);
+		UpdateSystemFlagsSample(ekf2_timestamps);

 		vehicle_odometry_s ev_odom;
 		const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
@@ -1037,6 +991,11 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_estimator_innovations_pub.publish(innovations);

 	// calculate noise filtered velocity innovations which are used for pre-flight checking
+	if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) {
+		// fully reset on takeoff or landing
+		_preflt_checker.reset();
+	}
+
 	if (!_ekf.control_status_flags().in_air) {
 		// TODO: move to run before publications
 		_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
@@ -1049,6 +1008,8 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 		_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
 		_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);

+		_preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
+
 		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
 	}
 }
@@ -1379,11 +1340,11 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 	_ekf.get_ekf_soln_status(&status.solution_status_flags);

 	// reset counters
-	status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
-	status.reset_count_vel_d = _ekf.state_reset_status().velD_counter;
-	status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter;
-	status.reset_count_pod_d = _ekf.state_reset_status().posD_counter;
-	status.reset_count_quat = _ekf.state_reset_status().quat_counter;
+	status.reset_count_vel_ne = _ekf.state_reset_status().reset_count.velNE;
+	status.reset_count_vel_d = _ekf.state_reset_status().reset_count.velD;
+	status.reset_count_pos_ne = _ekf.state_reset_status().reset_count.posNE;
+	status.reset_count_pod_d = _ekf.state_reset_status().reset_count.posD;
+	status.reset_count_quat = _ekf.state_reset_status().reset_count.quat;

 	status.time_slip = _last_time_slip_us * 1e-6f;

@@ -2107,9 +2068,50 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }

+void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	// EKF system flags
+	if (_status_sub.updated() || _vehicle_land_detected_sub.updated()) {
+
+		systemFlagUpdate flags{};
+		flags.time_us = ekf2_timestamps.timestamp;
+
+		// vehicle_status
+		vehicle_status_s vehicle_status;
+
+		if (_status_sub.copy(&vehicle_status)
+		    && (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) {
+
+			// initially set in_air from arming_state (will be overridden if land detector is available)
+			flags.in_air = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+			// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
+			flags.is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+		}
+
+		// vehicle_land_detected
+		vehicle_land_detected_s vehicle_land_detected;
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
+		    && (ekf2_timestamps.timestamp < vehicle_land_detected.timestamp + 3_s)) {
+
+			flags.at_rest = vehicle_land_detected.at_rest;
+			flags.in_air = !vehicle_land_detected.landed;
+			flags.gnd_effect = vehicle_land_detected.in_ground_effect;
+		}
+
+		_ekf.setSystemFlagData(flags);
+	}
+}
+
 void EKF2::UpdateCalibration(const hrt_abstime &timestamp, InFlightCalibration &cal, const matrix::Vector3f &bias,
 			     const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid)
 {
+	// reset existing cal on takeoff
+	if (!_ekf.control_status_prev_flags().in_air && _ekf.control_status_flags().in_air) {
+		cal = {};
+	}
+
 	// Check if conditions are OK for learning of accelerometer bias values
 	// the EKF is operating in the correct mode and there are no filter faults
 	static constexpr float max_var_allowed = 1e-3f;
@@ -170,6 +170,7 @@ private:
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps);

 	// Used to check, save and use learned accel/gyro/mag biases
 	struct InFlightCalibration {
@@ -43,6 +43,7 @@ px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_covariance_prediction_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
+px4_add_unit_gtest(SRC test_EKF_opt_flow_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
@@ -138,7 +138,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 13590000,0.7,0.00081,-0.014,0.71,7.7e-05,0.0061,-0.021,0.0024,0.001,-3.7e+02,-1.2e-05,-5.9e-05,-3.4e-06,1.5e-06,2e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.0002,0.0002,9.4e-05,0.053,0.053,0.028,0.049,0.049,0.05,3.6e-09,3.6e-09,2e-09,3.6e-06,3.6e-06,8.3e-07,0,0,0,0,0,0,0,0
 13690000,0.7,0.00079,-0.014,0.71,0.00075,0.0078,-0.025,0.0024,0.0017,-3.7e+02,-1.1e-05,-5.9e-05,-2.7e-06,1.8e-06,2e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00021,0.00021,9.4e-05,0.061,0.061,0.029,0.056,0.056,0.05,3.6e-09,3.6e-09,2e-09,3.6e-06,3.6e-06,8.3e-07,0,0,0,0,0,0,0,0
 13790000,0.7,0.00067,-0.014,0.71,0.0014,0.0036,-0.027,0.0035,-0.00062,-3.7e+02,-1.1e-05,-6e-05,-2.7e-06,-3e-07,1.8e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.0002,0.0002,9.3e-05,0.051,0.051,0.029,0.048,0.048,0.049,3.4e-09,3.4e-09,1.9e-09,3.6e-06,3.6e-06,7.9e-07,0,0,0,0,0,0,0,0
-13890000,0.7,0.00063,-0.014,0.71,0.0019,0.0035,-0.031,0.0036,-0.00029,-3.7e+02,-1.1e-05,-6e-05,-2.3e-06,1e-08,1.8e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00021,0.00021,9.3e-05,0.058,0.058,0.03,0.056,0.056,0.05,3.4e-09,3.4e-09,1.9e-09,3.6e-06,3.6e-06,7.8e-07,0,0,0,0,0,0,0,0
+13890000,0.7,0.00063,-0.014,0.71,0.0019,0.0035,-0.031,0.0036,-0.00029,-3.7e+02,-1.1e-05,-6e-05,-2.3e-06,9.9e-09,1.8e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,0.00021,0.00021,9.3e-05,0.058,0.058,0.03,0.056,0.056,0.05,3.4e-09,3.4e-09,1.9e-09,3.6e-06,3.6e-06,7.8e-07,0,0,0,0,0,0,0,0
 13990000,0.7,0.00056,-0.014,0.71,0.0022,0.001,-0.03,0.0045,-0.002,-3.7e+02,-1.1e-05,-6e-05,-2.2e-06,-2.6e-06,1.7e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,0.0002,0.0002,9.3e-05,0.049,0.049,0.03,0.048,0.048,0.05,3.2e-09,3.2e-09,1.8e-09,3.6e-06,3.6e-06,7.4e-07,0,0,0,0,0,0,0,0
 14090000,0.7,0.00054,-0.014,0.71,0.0023,0.0012,-0.031,0.0047,-0.0019,-3.7e+02,-1.1e-05,-6e-05,-1.5e-06,-2.5e-06,1.7e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,0.0002,0.0002,9.3e-05,0.055,0.055,0.031,0.056,0.056,0.05,3.2e-09,3.2e-09,1.8e-09,3.6e-06,3.6e-06,7.3e-07,0,0,0,0,0,0,0,0
 14190000,0.7,0.00044,-0.014,0.71,0.0057,0.00062,-0.033,0.0068,-0.0016,-3.7e+02,-1.1e-05,-6e-05,-1.1e-06,-2.3e-06,1.4e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,0.0002,0.0002,9.3e-05,0.047,0.047,0.031,0.048,0.048,0.05,3.1e-09,3.1e-09,1.8e-09,3.6e-06,3.6e-06,6.9e-07,0,0,0,0,0,0,0,0
@@ -198,7 +198,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 19590000,0.71,0.00019,-0.012,0.71,0.0097,-0.0013,0.0081,0.0075,-0.0003,-3.7e+02,-1.4e-05,-6.1e-05,5.9e-06,-1.8e-05,6.8e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.2e-05,0.00011,0.00011,8e-05,0.027,0.027,0.011,0.044,0.044,0.042,3e-10,3e-10,5.4e-10,3e-06,3e-06,5.4e-08,0,0,0,0,0,0,0,0
 19690000,0.71,0.00019,-0.012,0.71,0.01,-0.0035,0.0096,0.0085,-0.00055,-3.7e+02,-1.4e-05,-6.1e-05,5.7e-06,-1.8e-05,6.8e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.2e-05,0.00011,0.00011,8e-05,0.029,0.029,0.011,0.05,0.05,0.042,3e-10,3e-10,5.3e-10,3e-06,3e-06,5.2e-08,0,0,0,0,0,0,0,0
 19790000,0.71,0.00026,-0.012,0.71,0.0078,-0.0044,0.01,0.0068,-0.00044,-3.7e+02,-1.4e-05,-6.1e-05,5.8e-06,-1.8e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.2e-05,0.00011,0.00011,7.9e-05,0.026,0.026,0.011,0.044,0.044,0.042,2.7e-10,2.7e-10,5.2e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
-19890000,0.71,0.0002,-0.012,0.71,0.0065,-0.0047,0.011,0.0076,-0.00091,-3.7e+02,-1.4e-05,-6.1e-05,6.2e-06,-1.8e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.2e-05,0.00011,0.00011,7.9e-05,0.029,0.029,0.011,0.05,0.05,0.042,2.7e-10,2.7e-10,5.1e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
+19890000,0.71,0.0002,-0.012,0.71,0.0065,-0.0047,0.011,0.0076,-0.00091,-3.7e+02,-1.4e-05,-6.1e-05,6.1e-06,-1.8e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.2e-05,0.00011,0.00011,7.9e-05,0.029,0.029,0.011,0.05,0.05,0.042,2.7e-10,2.7e-10,5.1e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 19990000,0.71,0.00019,-0.012,0.71,0.0041,-0.0053,0.014,0.0062,-0.00076,-3.7e+02,-1.4e-05,-6.1e-05,6.7e-06,-1.7e-05,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.1e-05,0.00011,0.00011,7.9e-05,0.026,0.026,0.01,0.044,0.044,0.041,2.5e-10,2.5e-10,5e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20090000,0.71,0.00018,-0.012,0.71,0.0039,-0.0073,0.014,0.0065,-0.0014,-3.7e+02,-1.4e-05,-6.1e-05,7.1e-06,-1.7e-05,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.1e-05,0.00011,0.00011,7.9e-05,0.028,0.028,0.01,0.05,0.05,0.042,2.5e-10,2.5e-10,4.9e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
 20190000,0.71,0.00029,-0.012,0.71,0.0015,-0.008,0.017,0.0042,-0.0011,-3.7e+02,-1.4e-05,-6e-05,7.3e-06,-1.6e-05,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,8.1e-05,0.0001,0.0001,7.9e-05,0.025,0.025,0.01,0.044,0.044,0.041,2.3e-10,2.3e-10,4.8e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0
@@ -217,7 +217,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 21490000,0.71,0.00052,-0.012,0.71,-0.0053,-0.018,0.015,0.0023,-0.0052,-3.7e+02,-1.4e-05,-6e-05,7.8e-06,-2.5e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.7e-05,9.6e-05,7.6e-05,0.023,0.023,0.0085,0.048,0.048,0.038,1.4e-10,1.4e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21590000,0.71,0.00054,-0.012,0.71,-0.0058,-0.015,0.015,0.0019,-0.0032,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,9.1e-07,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.5e-05,9.5e-05,7.6e-05,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-10,1.3e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21690000,0.71,0.00055,-0.012,0.71,-0.0057,-0.016,0.017,0.0014,-0.0048,-3.7e+02,-1.4e-05,-6e-05,7.8e-06,8.9e-07,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.5e-05,9.5e-05,7.6e-05,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-10,1.3e-10,3.7e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
-21790000,0.71,0.00057,-0.012,0.71,-0.0063,-0.011,0.015,9e-05,-0.00074,-3.7e+02,-1.4e-05,-5.9e-05,7.6e-06,6.1e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.4e-05,9.4e-05,7.5e-05,0.021,0.021,0.0082,0.042,0.042,0.038,1.3e-10,1.3e-10,3.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
+21790000,0.71,0.00057,-0.012,0.71,-0.0063,-0.011,0.015,9.1e-05,-0.00074,-3.7e+02,-1.4e-05,-5.9e-05,7.6e-06,6.1e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.8e-05,9.4e-05,9.4e-05,7.5e-05,0.021,0.021,0.0082,0.042,0.042,0.038,1.3e-10,1.3e-10,3.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21890000,0.71,0.00057,-0.012,0.71,-0.0063,-0.012,0.016,-0.00054,-0.0019,-3.7e+02,-1.4e-05,-5.9e-05,7.5e-06,6e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.7e-05,9.4e-05,9.4e-05,7.5e-05,0.022,0.022,0.0082,0.047,0.047,0.038,1.3e-10,1.3e-10,3.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21990000,0.71,0.00062,-0.012,0.71,-0.0068,-0.0091,0.016,-0.0015,0.0015,-3.7e+02,-1.4e-05,-5.9e-05,7.5e-06,1e-05,7.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.7e-05,9.3e-05,9.3e-05,7.5e-05,0.02,0.02,0.0081,0.042,0.042,0.038,1.2e-10,1.2e-10,3.5e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 22090000,0.71,0.00064,-0.012,0.71,-0.0071,-0.0082,0.015,-0.0021,0.00064,-3.7e+02,-1.4e-05,-5.9e-05,7.4e-06,1e-05,7.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,7.7e-05,9.4e-05,9.3e-05,7.5e-05,0.022,0.022,0.0081,0.047,0.047,0.038,1.2e-10,1.2e-10,3.5e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
@@ -286,7 +286,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 28390000,0.71,0.011,0.023,0.7,-2.8,-1.3,0.77,-4.7,-2.6,-3.7e+02,-8.2e-06,-5.8e-05,4.8e-06,4.3e-05,-0.00013,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.7e-05,7.7e-05,6.5e-05,0.022,0.021,0.0087,0.1,0.099,0.036,3.8e-11,3.8e-11,1.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 28490000,0.71,0.0026,0.0045,0.7,-2.7,-1.3,1.1,-5,-2.7,-3.7e+02,-8.2e-06,-5.8e-05,4.7e-06,4e-05,-0.00012,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.8e-05,7.7e-05,6.5e-05,0.023,0.022,0.0088,0.11,0.11,0.036,3.8e-11,3.8e-11,1.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 28590000,0.71,0.00085,0.001,0.7,-2.6,-1.3,0.96,-5.3,-2.8,-3.7e+02,-8.2e-06,-5.8e-05,4.7e-06,3.7e-05,-0.00012,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.8e-05,7.7e-05,6.5e-05,0.024,0.023,0.0089,0.12,0.12,0.036,3.8e-11,3.8e-11,1.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
-28690000,0.71,0.00014,8.3e-05,0.7,-2.6,-1.2,0.96,-5.5,-2.9,-3.7e+02,-8.2e-06,-5.8e-05,4.6e-06,3.3e-05,-0.0001,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.8e-05,7.7e-05,6.5e-05,0.025,0.025,0.009,0.13,0.12,0.036,3.8e-11,3.8e-11,1.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
+28690000,0.71,0.00014,8.2e-05,0.7,-2.6,-1.2,0.96,-5.5,-2.9,-3.7e+02,-8.2e-06,-5.8e-05,4.6e-06,3.3e-05,-0.0001,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.6e-05,7.8e-05,7.7e-05,6.5e-05,0.025,0.025,0.009,0.13,0.12,0.036,3.8e-11,3.8e-11,1.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 28790000,0.71,-0.00015,-0.00017,0.71,-2.5,-1.2,0.97,-5.8,-3,-3.7e+02,-8.9e-06,-5.8e-05,4.6e-06,1.4e-06,-0.00018,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.5e-05,7.8e-05,7.7e-05,6.5e-05,0.024,0.024,0.0089,0.13,0.13,0.036,3.7e-11,3.7e-11,1.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 28890000,0.71,-0.00017,5.3e-05,0.71,-2.5,-1.2,0.96,-6.1,-3.2,-3.7e+02,-8.9e-06,-5.8e-05,4.6e-06,-3e-06,-0.00016,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.5e-05,7.8e-05,7.8e-05,6.5e-05,0.025,0.025,0.009,0.14,0.13,0.036,3.7e-11,3.7e-11,1.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
 28990000,0.71,2.5e-05,0.0005,0.71,-2.4,-1.2,0.95,-6.4,-3.3,-3.7e+02,-9.8e-06,-5.8e-05,4.5e-06,-2.4e-05,-0.00025,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,6.5e-05,7.8e-05,7.8e-05,6.4e-05,0.024,0.024,0.009,0.14,0.14,0.036,3.7e-11,3.6e-11,1.3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0
@@ -215,7 +215,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 21290000,0.98,-0.00055,-0.0072,0.19,-0.049,0.054,-0.63,-0.0062,0.0089,-0.082,-1.4e-05,-5.9e-05,2.2e-06,0.00014,4.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.6e-05,9.4e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.6e-10,1.6e-10,4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21390000,0.98,-0.002,-0.0079,0.19,-0.044,0.05,-0.75,-0.005,0.011,-0.15,-1.4e-05,-5.9e-05,2.2e-06,0.00015,1.5e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.3e-05,9.2e-05,7.1e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-10,1.4e-10,3.9e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0093,0.016,-0.23,-1.4e-05,-5.9e-05,2.3e-06,0.00015,4.7e-07,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.4e-05,9.3e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-10,1.4e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
-21590000,0.98,-0.0033,-0.0083,0.19,-0.031,0.043,-1,-0.0079,0.017,-0.32,-1.4e-05,-5.9e-05,2.3e-06,0.00015,4.4e-08,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.2e-05,9e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-10,1.3e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
+21590000,0.98,-0.0033,-0.0083,0.19,-0.031,0.043,-1,-0.0079,0.017,-0.32,-1.4e-05,-5.9e-05,2.3e-06,0.00015,4.5e-08,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.2e-05,9e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-10,1.3e-10,3.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21690000,0.98,-0.0037,-0.0082,0.19,-0.029,0.039,-1.1,-0.011,0.021,-0.44,-1.4e-05,-5.9e-05,2.5e-06,0.00015,-1.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9.2e-05,9.1e-05,7e-05,0.025,0.025,0.0083,0.048,0.048,0.036,1.3e-10,1.3e-10,3.7e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21790000,0.98,-0.004,-0.0084,0.19,-0.021,0.033,-1.3,-0.0038,0.018,-0.56,-1.4e-05,-5.8e-05,2.7e-06,0.00015,-3.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9e-05,8.8e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
 21890000,0.98,-0.0043,-0.0085,0.19,-0.018,0.029,-1.4,-0.0057,0.021,-0.7,-1.4e-05,-5.8e-05,2.5e-06,0.00016,-5.1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-06,9e-05,8.9e-05,6.9e-05,0.025,0.025,0.0082,0.048,0.048,0.036,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0
@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "test_helper/comparison_helper.h"
+
+#include "../EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
+#include "../EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
+
+using namespace matrix;
+
+void sympyFlowInnovVarHAndK(float q0, float q1, float q2, float q3, float vn, float ve, float vd, float range,
+			    const SquareMatrix24f &P, float R_LOS, Vector2f &innov_var,
+			    Vector24f &Kfusion_x, Vector24f &Kfusion_y, Vector24f &H_x, Vector24f &H_y)
+{
+	const Dcmf Tbs;
+
+	const float HK0 = -Tbs(1, 0) * q2 + Tbs(1, 1) * q1 + Tbs(1, 2) * q0;
+	const float HK1 = Tbs(1, 0) * q3 + Tbs(1, 1) * q0 - Tbs(1, 2) * q1;
+	const float HK2 = Tbs(1, 0) * q0 - Tbs(1, 1) * q3 + Tbs(1, 2) * q2;
+	const float HK3 = HK0 * vd + HK1 * ve + HK2 * vn;
+	const float HK4 = 1.0F / range;
+	const float HK5 = 2 * HK4;
+	const float HK6 = Tbs(1, 0) * q1 + Tbs(1, 1) * q2 + Tbs(1, 2) * q3;
+	const float HK7 = -HK0 * ve + HK1 * vd + HK6 * vn;
+	const float HK8 = HK0 * vn - HK2 * vd + HK6 * ve;
+	const float HK9 = -HK1 * vn + HK2 * ve + HK6 * vd;
+	const float HK10 = q0 * q2;
+	const float HK11 = q1 * q3;
+	const float HK12 = HK10 + HK11;
+	const float HK13 = 2 * Tbs(1, 2);
+	const float HK14 = q0 * q3;
+	const float HK15 = q1 * q2;
+	const float HK16 = HK14 - HK15;
+	const float HK17 = 2 * Tbs(1, 1);
+	const float HK18 = ecl::powf(q1, 2);
+	const float HK19 = ecl::powf(q2, 2);
+	const float HK20 = -HK19;
+	const float HK21 = ecl::powf(q0, 2);
+	const float HK22 = ecl::powf(q3, 2);
+	const float HK23 = HK21 - HK22;
+	const float HK24 = HK18 + HK20 + HK23;
+	const float HK25 = HK12 * HK13 - HK16 * HK17 + HK24 * Tbs(1, 0);
+	const float HK26 = HK14 + HK15;
+	const float HK27 = 2 * Tbs(1, 0);
+	const float HK28 = q0 * q1;
+	const float HK29 = q2 * q3;
+	const float HK30 = HK28 - HK29;
+	const float HK31 = -HK18;
+	const float HK32 = HK19 + HK23 + HK31;
+	const float HK33 = -HK13 * HK30 + HK26 * HK27 + HK32 * Tbs(1, 1);
+	const float HK34 = HK28 + HK29;
+	const float HK35 = HK10 - HK11;
+	const float HK36 = HK20 + HK21 + HK22 + HK31;
+	const float HK37 = HK17 * HK34 - HK27 * HK35 + HK36 * Tbs(1, 2);
+	const float HK38 = 2 * HK3;
+	const float HK39 = 2 * HK7;
+	const float HK40 = 2 * HK8;
+	const float HK41 = 2 * HK9;
+	const float HK42 = HK25 * P(0, 4) + HK33 * P(0, 5) + HK37 * P(0, 6) + HK38 * P(0, 0) + HK39 * P(0, 1) + HK40 * P(0,
+			   2) + HK41 * P(0, 3);
+	const float HK43 = ecl::powf(range, -2);
+	const float HK44 = HK25 * P(4, 6) + HK33 * P(5, 6) + HK37 * P(6, 6) + HK38 * P(0, 6) + HK39 * P(1, 6) + HK40 * P(2,
+			   6) + HK41 * P(3, 6);
+	const float HK45 = HK25 * P(4, 5) + HK33 * P(5, 5) + HK37 * P(5, 6) + HK38 * P(0, 5) + HK39 * P(1, 5) + HK40 * P(2,
+			   5) + HK41 * P(3, 5);
+	const float HK46 = HK25 * P(4, 4) + HK33 * P(4, 5) + HK37 * P(4, 6) + HK38 * P(0, 4) + HK39 * P(1, 4) + HK40 * P(2,
+			   4) + HK41 * P(3, 4);
+	const float HK47 = HK25 * P(2, 4) + HK33 * P(2, 5) + HK37 * P(2, 6) + HK38 * P(0, 2) + HK39 * P(1, 2) + HK40 * P(2,
+			   2) + HK41 * P(2, 3);
+	const float HK48 = HK25 * P(3, 4) + HK33 * P(3, 5) + HK37 * P(3, 6) + HK38 * P(0, 3) + HK39 * P(1, 3) + HK40 * P(2,
+			   3) + HK41 * P(3, 3);
+	const float HK49 = HK25 * P(1, 4) + HK33 * P(1, 5) + HK37 * P(1, 6) + HK38 * P(0, 1) + HK39 * P(1, 1) + HK40 * P(1,
+			   2) + HK41 * P(1, 3);
+
+	const float HK51 = Tbs(0, 1) * q1;
+	const float HK52 = Tbs(0, 2) * q0;
+	const float HK53 = Tbs(0, 0) * q2;
+	const float HK54 = HK51 + HK52 - HK53;
+	const float HK55 = Tbs(0, 0) * q3;
+	const float HK56 = Tbs(0, 1) * q0;
+	const float HK57 = Tbs(0, 2) * q1;
+	const float HK58 = HK55 + HK56 - HK57;
+	const float HK59 = Tbs(0, 0) * q0;
+	const float HK60 = Tbs(0, 2) * q2;
+	const float HK61 = Tbs(0, 1) * q3;
+	const float HK62 = HK59 + HK60 - HK61;
+	const float HK63 = HK54 * vd + HK58 * ve + HK62 * vn;
+	const float HK64 = Tbs(0, 0) * q1 + Tbs(0, 1) * q2 + Tbs(0, 2) * q3;
+	const float HK65 = HK58 * vd + HK64 * vn;
+	const float HK66 = -HK54 * ve + HK65;
+	const float HK67 = HK54 * vn + HK64 * ve;
+	const float HK68 = -HK62 * vd + HK67;
+	const float HK69 = HK62 * ve + HK64 * vd;
+	const float HK70 = -HK58 * vn + HK69;
+	const float HK71 = 2 * Tbs(0, 1);
+	const float HK72 = 2 * Tbs(0, 2);
+	const float HK73 = HK12 * HK72 + HK24 * Tbs(0, 0);
+	const float HK74 = -HK16 * HK71 + HK73;
+	const float HK75 = 2 * Tbs(0, 0);
+	const float HK76 = HK26 * HK75 + HK32 * Tbs(0, 1);
+	const float HK77 = -HK30 * HK72 + HK76;
+	const float HK78 = HK34 * HK71 + HK36 * Tbs(0, 2);
+	const float HK79 = -HK35 * HK75 + HK78;
+	const float HK80 = 2 * HK63;
+	const float HK81 = 2 * HK65 + 2 * ve * (-HK51 - HK52 + HK53);
+	const float HK82 = 2 * HK67 + 2 * vd * (-HK59 - HK60 + HK61);
+	const float HK83 = 2 * HK69 + 2 * vn * (-HK55 - HK56 + HK57);
+	const float HK84 = HK71 * (-HK14 + HK15) + HK73;
+	const float HK85 = HK72 * (-HK28 + HK29) + HK76;
+	const float HK86 = HK75 * (-HK10 + HK11) + HK78;
+	const float HK87 = HK80 * P(0, 0) + HK81 * P(0, 1) + HK82 * P(0, 2) + HK83 * P(0, 3) + HK84 * P(0, 4) + HK85 * P(0,
+			   5) + HK86 * P(0, 6);
+	const float HK88 = HK80 * P(0, 6) + HK81 * P(1, 6) + HK82 * P(2, 6) + HK83 * P(3, 6) + HK84 * P(4, 6) + HK85 * P(5,
+			   6) + HK86 * P(6, 6);
+	const float HK89 = HK80 * P(0, 5) + HK81 * P(1, 5) + HK82 * P(2, 5) + HK83 * P(3, 5) + HK84 * P(4, 5) + HK85 * P(5,
+			   5) + HK86 * P(5, 6);
+	const float HK90 = HK80 * P(0, 4) + HK81 * P(1, 4) + HK82 * P(2, 4) + HK83 * P(3, 4) + HK84 * P(4, 4) + HK85 * P(4,
+			   5) + HK86 * P(4, 6);
+	const float HK91 = HK80 * P(0, 2) + HK81 * P(1, 2) + HK82 * P(2, 2) + HK83 * P(2, 3) + HK84 * P(2, 4) + HK85 * P(2,
+			   5) + HK86 * P(2, 6);
+	const float HK92 = 2 * HK43;
+	const float HK93 = HK80 * P(0, 3) + HK81 * P(1, 3) + HK82 * P(2, 3) + HK83 * P(3, 3) + HK84 * P(3, 4) + HK85 * P(3,
+			   5) + HK86 * P(3, 6);
+	const float HK94 = HK80 * P(0, 1) + HK81 * P(1, 1) + HK82 * P(1, 2) + HK83 * P(1, 3) + HK84 * P(1, 4) + HK85 * P(1,
+			   5) + HK86 * P(1, 6);
+
+	// X-axis
+	innov_var(0) = (HK25 * HK43 * HK46 + HK33 * HK43 * HK45 + HK37 * HK43 * HK44 + HK38 * HK42 * HK43 + HK39 * HK43 * HK49 +
+			HK40 * HK43 * HK47 + HK41 * HK43 * HK48 + R_LOS);
+
+	{
+		const float HK50 = HK4 / innov_var(0);
+
+		// Observation Jacobians - axis 0
+		SparseVector24f<0, 1, 2, 3, 4, 5, 6> Hfusion;
+		Hfusion.at<0>() = HK3 * HK5;
+		Hfusion.at<1>() = HK5 * HK7;
+		Hfusion.at<2>() = HK5 * HK8;
+		Hfusion.at<3>() = HK5 * HK9;
+		Hfusion.at<4>() = HK25 * HK4;
+		Hfusion.at<5>() = HK33 * HK4;
+		Hfusion.at<6>() = HK37 * HK4;
+
+		// Kalman gains - axis 0
+		Vector24f Kfusion;
+		Kfusion(0) = HK42 * HK50;
+		Kfusion(1) = HK49 * HK50;
+		Kfusion(2) = HK47 * HK50;
+		Kfusion(3) = HK48 * HK50;
+		Kfusion(4) = HK46 * HK50;
+		Kfusion(5) = HK45 * HK50;
+		Kfusion(6) = HK44 * HK50;
+
+		for (unsigned row = 7; row <= 23; row++) {
+			Kfusion(row) = HK50 * (HK25 * P(4, row) + HK33 * P(5, row) + HK37 * P(6, row) + HK38 * P(0, row) + HK39 * P(1,
+					       row) + HK40 * P(2, row) + HK41 * P(3, row));
+		}
+
+		// copy to arrays used for comparison
+		for (int row = 0; row < 7; row++) {
+			H_x(row) = Hfusion.atCompressedIndex(row);
+		}
+
+		for (int row = 0; row < 24; row++) {
+			Kfusion_x(row) = Kfusion(row);
+		}
+	}
+
+	// Y-axis
+	innov_var(1) = (HK43 * HK74 * HK90 + HK43 * HK77 * HK89 + HK43 * HK79 * HK88 + HK43 * HK80 * HK87 + HK66 * HK92 * HK94 +
+			HK68 * HK91 * HK92 + HK70 * HK92 * HK93 + R_LOS);
+
+	{
+		const float HK95 = HK4 / innov_var(1);
+
+		// Observation Jacobians - axis 1
+		SparseVector24f<0, 1, 2, 3, 4, 5, 6> Hfusion;
+		Hfusion.at<0>() = -HK5 * HK63;
+		Hfusion.at<1>() = -HK5 * HK66;
+		Hfusion.at<2>() = -HK5 * HK68;
+		Hfusion.at<3>() = -HK5 * HK70;
+		Hfusion.at<4>() = -HK4 * HK74;
+		Hfusion.at<5>() = -HK4 * HK77;
+		Hfusion.at<6>() = -HK4 * HK79;
+
+		// Kalman gains - axis 1
+		Vector24f Kfusion;
+		Kfusion(0) = -HK87 * HK95;
+		Kfusion(1) = -HK94 * HK95;
+		Kfusion(2) = -HK91 * HK95;
+		Kfusion(3) = -HK93 * HK95;
+		Kfusion(4) = -HK90 * HK95;
+		Kfusion(5) = -HK89 * HK95;
+		Kfusion(6) = -HK88 * HK95;
+
+		for (unsigned row = 7; row <= 23; row++) {
+			Kfusion(row) = -HK95 * (HK80 * P(0, row) + HK81 * P(1, row) + HK82 * P(2, row) + HK83 * P(3, row) + HK84 * P(4,
+						row) + HK85 * P(5, row) + HK86 * P(6, row));
+		}
+
+		// copy to arrays used for comparison
+		for (int row = 0; row < 7; row++) {
+			H_y(row) = Hfusion.atCompressedIndex(row);
+		}
+
+		for (int row = 0; row < 24; row++) {
+			Kfusion_y(row) = Kfusion(row);
+		}
+	}
+}
+
+TEST(OptFlowFusionGenerated, SympyVsSymforce)
+{
+	// Compare calculation of observation Jacobians and Kalman gains for sympy and symforce generated equations
+	const Quatf q(Eulerf(-M_PI_F / 2.f, M_PI_F / 3.f, M_PI_F * 4.f / 5.f));
+	const float q0 = q(0);
+	const float q1 = q(1);
+	const float q2 = q(2);
+	const float q3 = q(3);
+
+	const float vn = 10.0f * 2.0f * ((float)randf() - 0.5f);
+	const float ve = 10.0f * 2.0f * ((float)randf() - 0.5f);
+	const float vd = 2.0f * ((float)randf() - 0.5f);
+
+	const float range = 5.0f;
+
+	Vector24f state_vector{};
+	state_vector(0) = q0;
+	state_vector(1) = q1;
+	state_vector(2) = q2;
+	state_vector(3) = q3;
+	state_vector(4) = vn;
+	state_vector(5) = ve;
+	state_vector(6) = vd;
+
+	const float R_LOS = sq(0.15f);
+
+	SquareMatrix24f P = createRandomCovarianceMatrix24f();
+
+	Vector24f Hfusion_sympy_x;
+	Vector24f Hfusion_sympy_y;
+	Vector24f Kfusion_sympy_x;
+	Vector24f Kfusion_sympy_y;
+	Vector2f innov_var_sympy;
+
+	Vector24f Hfusion_symforce;
+	Vector24f Kfusion_symforce;
+	Vector2f innov_var_symforce;
+
+	sympyFlowInnovVarHAndK(q0, q1, q2, q3, vn, ve, vd, range, P, R_LOS, innov_var_sympy,
+			       Kfusion_sympy_x, Kfusion_sympy_y, Hfusion_sympy_x, Hfusion_sympy_y);
+
+	for (int i = 0; i < 2; i++) {
+		Vector24f &Hfusion_sympy = Hfusion_sympy_x;
+		Vector24f &Kfusion_sympy = Kfusion_sympy_x;
+
+		if (i == 0) {
+			sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var_symforce, &Hfusion_symforce);
+
+		} else {
+			Hfusion_sympy = Hfusion_sympy_y;
+			Kfusion_sympy = Kfusion_sympy_y;
+			sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var_symforce(1), &Hfusion_symforce);
+		}
+
+		// K isn't generated from symbolic anymore to save flash space
+		Kfusion_symforce = P * Hfusion_symforce / innov_var_symforce(i);
+
+		DiffRatioReport report = computeDiffRatioVector24f(Hfusion_sympy, Hfusion_symforce);
+		EXPECT_LT(report.max_diff_fraction, 1e-5f) << "i = " << i << "Hfusion max diff fraction = " <<
+				report.max_diff_fraction <<
+				" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
+
+		report = computeDiffRatioVector24f(Kfusion_sympy, Kfusion_symforce);
+		EXPECT_LT(report.max_diff_fraction, 1e-5f) << "i = " << i << "Kfusion max diff fraction = " <<
+				report.max_diff_fraction <<
+				" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
+		EXPECT_NEAR(innov_var_sympy(i), innov_var_symforce(i), 1e-5f) << "i = " << i;
+	}
+}
@@ -71,13 +71,6 @@ set(python_args
 	-f ${files_to_generate}
 )

-# add the additional tasks for the python script (if there are any)
-if(flight_tasks_to_add)
-	list(APPEND python_args
-		-s ${flight_tasks_to_add}
-	)
-endif()
-
 # generate the files using the python script and template
 add_custom_command(
 	OUTPUT
@@ -114,6 +114,8 @@ void FlightModeManager::Run()
 			handleCommand();
 		}

+		tryApplyCommandIfAny();
+
 		if (isAnyTaskActive()) {
 			generateTrajectorySetpoint(dt, vehicle_local_position);
 		}
@@ -231,6 +233,19 @@ void FlightModeManager::start_flight_task()

 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
 		should_disable_task = false;
+
+		if (!_command_failed) {
+			FlightTaskError error = FlightTaskError::InvalidTask;
+
+#if !defined(CONSTRAINED_FLASH)
+			error = switchTask(FlightTaskIndex::Orbit);
+#endif // !CONSTRAINED_FLASH
+
+			if (error != FlightTaskError::NoError) {
+				task_failure = true;
+			}
+
+		}
 	}

 	// check task failure
@@ -265,47 +280,39 @@ void FlightModeManager::start_flight_task()

 }

+void FlightModeManager::tryApplyCommandIfAny()
+{
+	if (isAnyTaskActive() && _current_command.command != 0 && hrt_absolute_time() < _current_command.timestamp + 200_ms) {
+		bool success = false;
+
+		if (_current_task.task->applyCommandParameters(_current_command, success)) {
+			_current_command.command = 0;
+
+			if (!success) {
+				switchTask(FlightTaskIndex::Failsafe);
+				_command_failed = true;
+			}
+		}
+	}
+}
+
 void FlightModeManager::handleCommand()
 {
 	// get command
 	vehicle_command_s command;

 	while (_vehicle_command_sub.update(&command)) {
-		// check what command it is
-		FlightTaskIndex desired_task = switchVehicleCommand(command.command);

-		// ignore all unknown commands
-		if (desired_task != FlightTaskIndex::None
-		    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			// switch to the commanded task
-			bool switch_succeeded = (switchTask(desired_task) == FlightTaskError::NoError);
-			uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+		switch (command.command) {
+		case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
+			// The command might trigger a mode switch, and the mode switch can happen before or
+			// after we receive the command here, so we store it for later.
+			memcpy(&_current_command, &command, sizeof(vehicle_command_s));
+			_command_failed = false;
+			break;
+		}

-			// if we are in/switched to the desired task
-			if (switch_succeeded) {
-				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
-				// if the task is running apply parameters to it and see if it rejects
-				if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
-					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
-
-					// if we just switched and parameters are not accepted, go to failsafe
-					if (switch_succeeded) {
-						switchTask(FlightTaskIndex::Failsafe);
-					}
-				}
-			}
-
-			// send back acknowledgment
-			vehicle_command_ack_s command_ack{};
-			command_ack.command = command.command;
-			command_ack.result = cmd_result;
-			command_ack.target_system = command.source_system;
-			command_ack.target_component = command.source_component;
-			command_ack.timestamp = hrt_absolute_time();
-			_vehicle_command_ack_pub.publish(command_ack);
-
-		} else if (_current_task.task) {
+		if (_current_task.task) {
 			// check for other commands not related to task switching
 			if ((command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED)
 			    && (static_cast<uint8_t>(command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_GROUNDSPEED)
@@ -419,6 +426,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 	}

 	_current_task.task->setResetCounters(last_reset_counters);
+	_command_failed = false;

 	return FlightTaskError::NoError;
 }
@@ -50,7 +50,6 @@
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -112,9 +111,10 @@ private:
 	 */
 	bool isAnyTaskActive() const { return _current_task.task; }

+	void tryApplyCommandIfAny();
+
 	// generated
 	int _initTask(FlightTaskIndex task_index);
-	FlightTaskIndex switchVehicleCommand(const int command);

 	/**
 	 * Union with all existing tasks: we use it to make sure that only the memory of the largest existing
@@ -136,6 +136,9 @@ private:
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; ///< loop duration performance counter
 	hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration

+	vehicle_command_s _current_command{};
+	bool _command_failed{false};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

 	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
@@ -150,7 +153,6 @@ private:

 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
-	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};

 	DEFINE_PARAMETERS(
@@ -75,23 +75,3 @@ int FlightModeManager::_initTask(FlightTaskIndex task_index)
 	_current_task.index = task_index;
 	return 0;
 }
-
-FlightTaskIndex FlightModeManager::switchVehicleCommand(const int command)
-{
-    switch (command) {
-@# loop through all additional tasks
-@[if tasks_add]@
-@[for task in tasks_add]@
-@{
-upperCase = lambda s: s[:].upper() if s else ''
-}@
-	case vehicle_command_s::VEHICLE_CMD_DO_@(upperCase(task)) :
-		return FlightTaskIndex::@(task);
-		break;
-
-@[end for]@
-@[end if]@
-	// ignore all unknown commands
-	default : return FlightTaskIndex::None;
-	}
-}
@@ -6,7 +6,6 @@ import argparse

 parser = argparse.ArgumentParser()
 parser.add_argument("-t", "--tasks", dest='tasks_all', nargs='+', required=True, help="All tasks to be generated")
-parser.add_argument("-s", "--tasks_additional", dest='tasks_add', nargs='+', help="Additional tasks to be generated (on top of the core)")
 parser.add_argument("-i", "--input_directory", dest='directory_in', required=True, help="Output directory")
 parser.add_argument("-o", "--output_directory", dest='directory_out', required=True, help="Input directory")
 parser.add_argument("-f", "--files", dest='gen_files', nargs='+', required=True, help="Files to generate")
@@ -20,7 +19,6 @@ for gen_file in args.gen_files:
    # need to specify the em_globals inside the loop -> em.Error: interpreter globals collision
    em_globals = {
        "tasks": args.tasks_all,
-        "tasks_add": args.tasks_add,
    }
    interpreter = em.Interpreter(output=output_file, globals=em_globals)
    interpreter.file(open(args.directory_in + "/" + gen_file + ".em"))
@@ -91,9 +91,10 @@ public:
 	/**
 	 * To be called to adopt parameters from an arrived vehicle command
 	 * @param command received command message containing the parameters
-	 * @return true if accepted, false if declined
+	 * @param success set to true if it was successfully applied, false on error
+	 * @return true if handled
 	 */
-	virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; }
+	virtual bool applyCommandParameters(const vehicle_command_s &command, bool &success) { return false; }

 	/**
 	 * Call before activate() or update()
@@ -49,9 +49,13 @@ FlightTaskOrbit::FlightTaskOrbit()
 	_sticks_data_required = false;
 }

-bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
+bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, bool &success)
 {
-	bool ret = true;
+	if (command.command != vehicle_command_s::VEHICLE_CMD_DO_ORBIT) {
+		return false;
+	}
+
+	success = true;
 	// save previous velocity and rotation direction
 	bool new_is_clockwise = _orbit_velocity > 0;
 	float new_radius = _orbit_radius;
@@ -81,7 +85,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 	} else {
 		mavlink_log_critical(&_mavlink_log_pub, "Orbit radius limit exceeded\t");
 		events::send(events::ID("orbit_radius_exceeded"), events::Log::Alert, "Orbit radius limit exceeded");
-		ret = false;
+		success = false;
 	}

 	// commanded heading behaviour
@@ -98,7 +102,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 			_center.xy() = _geo_projection.project(command.param5, command.param6);

 		} else {
-			ret = false;
+			success = false;
 		}
 	}

@@ -108,7 +112,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 			_center(2) = _global_local_alt0 - command.param7;

 		} else {
-			ret = false;
+			success = false;
 		}
 	}

@@ -118,7 +122,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 		_position_smoothing.reset(_acceleration_setpoint, _velocity_setpoint, _position);
 	}

-	return ret;
+	return true;
 }

 bool FlightTaskOrbit::sendTelemetry()
@@ -56,7 +56,7 @@ public:
 	FlightTaskOrbit();
 	virtual ~FlightTaskOrbit() = default;

-	bool applyCommandParameters(const vehicle_command_s &command) override;
+	bool applyCommandParameters(const vehicle_command_s &command, bool &success) override;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;

@@ -305,10 +305,7 @@ void FixedwingAttitudeControl::Run()
 		float pitchspeed = angular_velocity.xyz[1];
 		float yawspeed = angular_velocity.xyz[2];
 		const Vector3f rates(rollspeed, pitchspeed, yawspeed);
-
-		vehicle_angular_acceleration_s angular_acceleration{};
-		_vehicle_angular_acceleration_sub.copy(&angular_acceleration);
-		const Vector3f angular_accel{angular_acceleration.xyz};
+		const Vector3f angular_accel{angular_velocity.xyz_derivative};

 		if (_vehicle_status.is_vtol_tailsitter) {
 			/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
@@ -67,7 +67,6 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
-#include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -127,7 +126,6 @@ private:
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};		/**< vehicle land detected subscription */
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};				/**< vehicle status subscription */
 	uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};

 	uORB::SubscriptionMultiArray<control_allocator_status_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};

@@ -616,10 +616,12 @@ PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);
 *
 * @min 45
 * @max 720
- * @unit deg
+ * @decimal 1
+ * @increment 5
+ * @unit deg/s
 * @group FW Attitude Control
 */
-PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
+PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90.f);

 /**
 * Acro body y max rate.
@@ -629,10 +631,12 @@ PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
 *
 * @min 45
 * @max 720
- * @unit deg
+ * @decimal 1
+ * @increment 5
+ * @unit deg/s
 * @group FW Attitude Control
 */
-PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
+PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90.f);

 /**
 * Acro body z max rate.
@@ -642,10 +646,12 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
 *
 * @min 10
 * @max 180
- * @unit deg
+ * @decimal 1
+ * @increment 5
+ * @unit deg/s
 * @group FW Attitude Control
 */
-PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45);
+PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45.f);

 /**
 * Roll trim increment at minimum airspeed
@@ -189,15 +189,15 @@ FixedwingPositionControl::parameters_update()
 		check_ret = PX4_ERROR;
 	}

-	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) {
-		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min\t");
+	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: FW_AIRSPD_STALL airspeed higher than FW_AIRSPD_MIN\t");
 		/* EVENT
 		 * @description
 		 * - <param>FW_AIRSPD_MIN</param>: {1:.1}
 		 * - <param>FW_AIRSPD_STALL</param>: {2:.1}
 		 */
 		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error,
-					   "Invalid configuration: Stall airspeed higher than 90% of minimum airspeed",
+					   "Invalid configuration: FW_AIRSPD_STALL higher FW_AIRSPD_MIN",
 					   _param_fw_airspd_min.get(), _param_fw_airspd_stall.get());
 		check_ret = PX4_ERROR;
 	}
@@ -428,7 +428,7 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interva
 	float load_factor = 1.0f;

 	if (PX4_ISFINITE(_att_sp.roll_body)) {
-		load_factor = 1.0f / sqrtf(cosf(_att_sp.roll_body));
+		load_factor = 1.0f / cosf(_att_sp.roll_body);
 	}

 	float weight_ratio = 1.0f;
@@ -80,6 +80,7 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 *
 * @unit deg/s
 * @min 0
+ * @decimal 0
 * @increment 1
 * @group FW L1 Control
 */
@@ -250,6 +251,8 @@ PARAM_DEFINE_FLOAT(FW_THR_TRIM, 0.6f);
 *
 * @min 0.0
 * @max 1.0
+ * @decimal 2
+ * @increment 0.01
 * @group FW TECS
 */
 PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
@@ -79,6 +79,8 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
+	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
+	_paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL");
 	_minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s);
 }

@@ -118,6 +120,19 @@ void MulticopterLandDetector::_update_params()
 {
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
+	param_get(_paramHandle.landSpeed, &_params.landSpeed);
+	param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed);
+
+	// 1.2 corresponds to the margin between the default parameters LNDMC_Z_VEL_MAX = MPC_LAND_CRWL / 1.2
+	const float lndmc_upper_threshold = math::min(_params.crawlSpeed, _params.landSpeed) / 1.2f;
+
+	if (_param_lndmc_z_vel_max.get() > lndmc_upper_threshold) {
+		PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_CRWL or MPC_LAND_SPEED, updating %.3f -> %.3f",
+			(double)_param_lndmc_z_vel_max.get(), (double)(lndmc_upper_threshold));
+
+		_param_lndmc_z_vel_max.set(lndmc_upper_threshold);
+		_param_lndmc_z_vel_max.commit_no_notification();
+	}

 	int32_t use_hover_thrust_estimate = 0;
 	param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
@@ -87,12 +87,13 @@ private:
 	/** Distance above ground below which entering ground contact state is possible when distance to ground is available. */
 	static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f;

-	/** Handles for interesting parameters. **/
 	struct {
 		param_t minThrottle;
 		param_t hoverThrottle;
 		param_t minManThrottle;
 		param_t useHoverThrustEstimate;
+		param_t landSpeed;
+		param_t crawlSpeed;
 	} _paramHandle{};

 	struct {
@@ -100,6 +101,8 @@ private:
 		float hoverThrottle;
 		float minManThrottle;
 		bool useHoverThrustEstimate;
+		float landSpeed;
+		float crawlSpeed;
 	} _params{};

 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
@@ -51,12 +51,13 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f);
 * Multicopter vertical velocity threshold
 *
 * Vertical velocity threshold to detect landing.
- * Should be set lower than the expected minimal speed for landing
- * so MPC_LAND_SPEED for autonomous landing and MPC_LAND_CRWL
- * if distance sensor is present and slowing down close to ground.
+ * Has to be set lower than the expected minimal speed for landing,
+ * which is either MPC_LAND_SPEED or MPC_LAND_CRWL.
+ * This is enforced by an automatic check.
 *
 * @unit m/s
- * @decimal 1
+ * @min 0
+ * @decimal 2
 *
 * @group Land Detector
 */
@@ -52,7 +52,6 @@ PARAM_DEFINE_FLOAT(LPE_FLW_RR, 7.0f);
 * @group Local Position Estimator
 * @min 0
 * @max 255
- * @decimal 0
 */
 PARAM_DEFINE_INT32(LPE_FLW_QMIN, 150);

@@ -216,7 +216,6 @@ void LoggedTopics::add_default_topics()
 	// additional control allocation logging
 	add_topic("actuator_motors", 100);
 	add_topic("actuator_servos", 100);
-	add_topic("vehicle_angular_acceleration", 20);
 	add_topic_multi("vehicle_thrust_setpoint", 20, 1);
 	add_topic_multi("vehicle_torque_setpoint", 20, 2);

@@ -237,7 +236,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("fw_virtual_attitude_setpoint");
 	add_topic("mc_virtual_attitude_setpoint");
 	add_topic("time_offset");
-	add_topic("vehicle_angular_acceleration", 10);
 	add_topic("vehicle_angular_velocity", 10);
 	add_topic("vehicle_angular_velocity_groundtruth", 10);
 	add_topic("vehicle_attitude_groundtruth", 10);
@@ -294,7 +292,6 @@ void LoggedTopics::add_high_rate_topics()
 	add_topic("manual_control_setpoint");
 	add_topic("rate_ctrl_status", 20);
 	add_topic("sensor_combined");
-	add_topic("vehicle_angular_acceleration");
 	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_attitude");
 	add_topic("vehicle_attitude_setpoint");
@@ -377,7 +374,7 @@ void LoggedTopics::add_system_identification_topics()
 	add_topic("actuator_controls_0");
 	add_topic("actuator_controls_1");
 	add_topic("sensor_combined");
-	add_topic("vehicle_angular_acceleration");
+	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_torque_setpoint");
 }

@@ -308,6 +308,10 @@ Mavlink::instance_count()
 Mavlink *
 Mavlink::get_instance_for_device(const char *device_name)
 {
+	if (device_name == nullptr) {
+		return nullptr;
+	}
+
 	LockGuard lg{mavlink_module_mutex};

 	for (Mavlink *inst : mavlink_module_instances) {
@@ -417,6 +421,10 @@ Mavlink::get_status_all_instances(bool show_streams_status)
 bool
 Mavlink::serial_instance_exists(const char *device_name, Mavlink *self)
 {
+	if (device_name == nullptr) {
+		return false;
+	}
+
 	LockGuard lg{mavlink_module_mutex};

 	for (Mavlink *inst : mavlink_module_instances) {
@@ -979,7 +987,7 @@ const in_addr Mavlink::compute_broadcast_addr(const in_addr &host_addr, const in
 	return broadcast_addr;
 }

-void Mavlink::init_udp()
+bool Mavlink::init_udp()
 {
 	PX4_DEBUG("Setting up UDP with port %hu", _network_port);

@@ -988,13 +996,13 @@ void Mavlink::init_udp()
 	_myaddr.sin_port = htons(_network_port);

 	if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
-		PX4_WARN("create socket failed: %s", strerror(errno));
-		return;
+		PX4_ERR("create socket failed: %s", strerror(errno));
+		return false;
 	}

 	if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
-		PX4_WARN("bind failed: %s", strerror(errno));
-		return;
+		PX4_ERR("bind failed: %s", strerror(errno));
+		return false;
 	}

 	/* set default target address, but not for onboard mode (will be set on first received packet) */
@@ -1004,6 +1012,8 @@ void Mavlink::init_udp()
 	}

 	_src_addr.sin_port = htons(_remote_port);
+
+	return true;
 }
 #endif // MAVLINK_UDP

@@ -1061,6 +1071,7 @@ Mavlink::send_autopilot_capabilities()
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
@@ -1855,12 +1866,6 @@ int
 Mavlink::task_main(int argc, char *argv[])
 {
 	int ch;
-	_baudrate = 57600;
-	_datarate = 0;
-	_mode = MAVLINK_MODE_COUNT;
-	FLOW_CONTROL_MODE _flow_control = FLOW_CONTROL_AUTO;
-
-	_interface_name = nullptr;

 	// We don't care about the name and verb at this point.
 	argc -= 2;
@@ -1875,6 +1880,8 @@ Mavlink::task_main(int argc, char *argv[])
 	int temp_int_arg;
 #endif

+	FLOW_CONTROL_MODE flow_control = FLOW_CONTROL_AUTO;
+
 	while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
@@ -2001,7 +2008,7 @@ Mavlink::task_main(int argc, char *argv[])

 		case 'm': {

-				int mode;
+				int mode = -1;

 				if (px4_get_parameter_value(myoptarg, mode) == 0) {
 					if (mode >= 0 && mode < (int)MAVLINK_MODE_COUNT) {
@@ -2080,11 +2087,11 @@ Mavlink::task_main(int argc, char *argv[])
 			break;

 		case 'z':
-			_flow_control = FLOW_CONTROL_ON;
+			flow_control = FLOW_CONTROL_ON;
 			break;

 		case 'Z':
-			_flow_control = FLOW_CONTROL_OFF;
+			flow_control = FLOW_CONTROL_OFF;
 			break;

 		default:
@@ -2098,29 +2105,6 @@ Mavlink::task_main(int argc, char *argv[])
 		return PX4_ERROR;
 	}

-	/* USB serial is indicated by /dev/ttyACMx */
-	if (strncmp(_device_name, "/dev/ttyACM", 11) == 0) {
-		if (_datarate == 0) {
-			_datarate = 100000;
-		}
-
-		/* USB has no baudrate, but use a magic number for 'fast' */
-		_baudrate = 2000000;
-
-		if (_mode == MAVLINK_MODE_COUNT) {
-			_mode = MAVLINK_MODE_CONFIG;
-		}
-
-		_ftp_on = true;
-		_is_usb_uart = true;
-
-		set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
-	}
-
-	if (_mode == MAVLINK_MODE_COUNT) {
-		_mode = MAVLINK_MODE_NORMAL;
-	}
-
 	if (_datarate == 0) {
 		/* convert bits to bytes and use 1/2 of bandwidth by default */
 		_datarate = _baudrate / 20;
@@ -2131,16 +2115,56 @@ Mavlink::task_main(int argc, char *argv[])
 	}

 	if (get_protocol() == Protocol::SERIAL) {
-		if (Mavlink::serial_instance_exists(_device_name, this)) {
-			PX4_ERR("%s already running", _device_name);
+		if (_device_name) {
+
+			if (Mavlink::serial_instance_exists(_device_name, this)) {
+				PX4_ERR("%s already running", _device_name);
+				return PX4_ERROR;
+			}
+
+			if (access(_device_name, F_OK) != 0) {
+				PX4_ERR("invalid device (-d) %s", _device_name);
+				return PX4_ERROR;
+
+			} else if (access(_device_name, R_OK | W_OK) != 0) {
+				PX4_ERR("unable to access device %s", _device_name);
+				return PX4_ERROR;
+			}
+
+			// USB serial is indicated by /dev/ttyACMx
+			if (strncmp(_device_name, "/dev/ttyACM", 11) == 0) {
+				if (_datarate == 0) {
+					_datarate = 100000;
+				}
+
+				/* USB has no baudrate, but use a magic number for 'fast' */
+				_baudrate = 2000000;
+
+				if (_mode == MAVLINK_MODE_COUNT) {
+					_mode = MAVLINK_MODE_CONFIG;
+				}
+
+				_is_usb_uart = true;
+
+				set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
+			}
+
+		} else {
+			PX4_ERR("serial device not set");
+			usage();
+			return PX4_ERROR;
+		}
+
+		// open the UART device after setting the instance, as it might block */
+		_uart_fd = mavlink_open_uart(_baudrate, _device_name, flow_control);
+
+		if (_uart_fd < 0) {
+			PX4_ERR("could not open %s", _device_name);
 			return PX4_ERROR;
 		}

 		PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
 			 mavlink_mode_str(_mode), _datarate, _device_name, _baudrate);
-
-		/* flush stdout in case MAVLink is about to take it over */
-		fflush(stdout);
 	}

 #if defined(MAVLINK_UDP)
@@ -2151,6 +2175,11 @@ Mavlink::task_main(int argc, char *argv[])
 			return PX4_ERROR;
 		}

+		// init socket
+		if (!init_udp()) {
+			return PX4_ERROR;
+		}
+
 		PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu",
 			 mavlink_mode_str(_mode), _datarate, _network_port, _remote_port);
 	}
@@ -2194,7 +2223,6 @@ Mavlink::task_main(int argc, char *argv[])

 	/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
 	_transmitting_enabled = true;
-	_transmitting_enabled_commanded = true;

 	if (_mode == MAVLINK_MODE_IRIDIUM) {
 		_transmitting_enabled_commanded = false;
@@ -2219,36 +2247,7 @@ Mavlink::task_main(int argc, char *argv[])
 	}

 	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
-
-	/* hard limit to 1000 Hz at max */
-	if (_main_loop_delay < MAVLINK_MIN_INTERVAL) {
-		_main_loop_delay = MAVLINK_MIN_INTERVAL;
-	}
-
-	/* hard limit to 100 Hz at least */
-	if (_main_loop_delay > MAVLINK_MAX_INTERVAL) {
-		_main_loop_delay = MAVLINK_MAX_INTERVAL;
-	}
-
-	/* open the UART device after setting the instance, as it might block */
-	if (get_protocol() == Protocol::SERIAL) {
-		_uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control);
-
-		if (_uart_fd < 0) {
-			PX4_ERR("could not open %s", _device_name);
-			return PX4_ERROR;
-		}
-	}
-
-#if defined(MAVLINK_UDP)
-
-	/* init socket if necessary */
-	if (get_protocol() == Protocol::UDP) {
-		init_udp();
-	}
-
-#endif // MAVLINK_UDP
+	_main_loop_delay = math::constrain((MAIN_LOOP_DELAY * 1000) / _datarate, MAVLINK_MIN_INTERVAL, MAVLINK_MAX_INTERVAL);

 	_task_id = px4_getpid();

@@ -3131,7 +3130,7 @@ Mavlink::stop_command(int argc, char *argv[])
 int
 Mavlink::stream_command(int argc, char *argv[])
 {
-	const char *device_name = DEFAULT_DEVICE_NAME;
+	const char *device_name = nullptr;
 	float rate = -1.0f;
 	const char *stream_name = nullptr;
 #ifdef MAVLINK_UDP
@@ -86,7 +86,6 @@
 #include "mavlink_ulog.h"

 #define DEFAULT_BAUD_RATE       57600
-#define DEFAULT_DEVICE_NAME     "/dev/ttyS1"

 #define HASH_PARAM              "_HASH_CHECK"

@@ -195,7 +194,7 @@ public:
 	 */
 	int			get_component_id() const { return mavlink_system.compid; }

-	const char *_device_name{DEFAULT_DEVICE_NAME};
+	const char *_device_name{nullptr};

 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
@@ -535,7 +534,7 @@ private:
 	px4::atomic_bool	_task_running{false};

 	bool			_transmitting_enabled{true};
-	bool			_transmitting_enabled_commanded{false};
+	bool			_transmitting_enabled_commanded{true};
 	bool			_first_heartbeat_sent{false};

 	orb_advert_t		_mavlink_log_pub{nullptr};
@@ -687,8 +686,7 @@ private:

 	void			mavlink_update_parameters();

-	int mavlink_open_uart(const int baudrate = DEFAULT_BAUD_RATE,
-			      const char *uart_name = DEFAULT_DEVICE_NAME,
+	int mavlink_open_uart(const int baudrate, const char *uart_name,
 			      const FLOW_CONTROL_MODE flow_control = FLOW_CONTROL_AUTO);

 	static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
@@ -747,7 +745,7 @@ private:
 #if defined(MAVLINK_UDP)
 	void find_broadcast_address();

-	void init_udp();
+	bool init_udp();
 #endif // MAVLINK_UDP


@@ -122,18 +122,14 @@ MulticopterRateControl::Run()

 	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {

-		// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
-		vehicle_angular_acceleration_s v_angular_acceleration{};
-		_vehicle_angular_acceleration_sub.copy(&v_angular_acceleration);
-
 		const hrt_abstime now = angular_velocity.timestamp_sample;

 		// Guard against too small (< 0.125ms) and too large (> 20ms) dt's.
 		const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f);
 		_last_run = now;

-		const Vector3f angular_accel{v_angular_acceleration.xyz};
 		const Vector3f rates{angular_velocity.xyz};
+		const Vector3f angular_accel{angular_velocity.xyz_derivative};

 		/* check for updates in other topics */
 		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
@@ -54,7 +54,6 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
-#include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_land_detected.h>
@@ -101,7 +100,6 @@ private:
 	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
 	uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
@@ -33,6 +33,7 @@

 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/cli.h>
+#include <px4_platform_common/posix.h>

 #include "microdds_client.h"

@@ -154,43 +154,6 @@ PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADM, 500);
 */
 PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADU, 10);

-/**
- * Airfield home Lat
- *
- * Latitude of airfield home waypoint
- *
- * @unit deg*1e7
- * @min -900000000
- * @max 900000000
- * @group Data Link Loss
- */
-PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
-
-/**
- * Airfield home Lon
- *
- * Longitude of airfield home waypoint
- *
- * @unit deg*1e7
- * @min -1800000000
- * @max 1800000000
- * @group Data Link Loss
- */
-PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
-
-/**
- * Airfield home alt
- *
- * Altitude of airfield home waypoint
- *
- * @unit m
- * @min -50
- * @decimal 1
- * @increment 0.5
- * @group Data Link Loss
- */
-PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
-
 /**
 * Force VTOL mode takeoff and land
 *
@@ -46,7 +46,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	_vehicle_angular_acceleration_pub.advertise();
 	_vehicle_angular_velocity_pub.advertise();
 }

@@ -918,17 +917,6 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sa
 {
 	if (timestamp_sample >= _last_publish + _publish_interval_min_us) {

-		// Publish vehicle_angular_acceleration
-		vehicle_angular_acceleration_s angular_acceleration;
-		angular_acceleration.timestamp_sample = timestamp_sample;
-
-		// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
-		_angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated;
-		_angular_acceleration.copyTo(angular_acceleration.xyz);
-
-		angular_acceleration.timestamp = hrt_absolute_time();
-		_vehicle_angular_acceleration_pub.publish(angular_acceleration);
-
 		// Publish vehicle_angular_velocity
 		vehicle_angular_velocity_s angular_velocity;
 		angular_velocity.timestamp_sample = timestamp_sample;
@@ -937,6 +925,10 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sa
 		_angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias;
 		_angular_velocity.copyTo(angular_velocity.xyz);

+		// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
+		_angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated;
+		_angular_acceleration.copyTo(angular_velocity.xyz_derivative);
+
 		angular_velocity.timestamp = hrt_absolute_time();
 		_vehicle_angular_velocity_pub.publish(angular_velocity);

@@ -55,7 +55,6 @@
 #include <uORB/topics/sensor_gyro_fft.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_selection.h>
-#include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>

 using namespace time_literals;
@@ -99,7 +98,6 @@ private:

 	static constexpr int MAX_SENSOR_COUNT = 4;

-	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Publication<vehicle_angular_velocity_s>     _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};

 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
@@ -231,13 +231,14 @@ void Standard::update_transition_state()
 	}

 	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
-		if (_param_vt_psher_rmp_dt.get() <= 0.0f) {
+		if (_param_vt_psher_slew.get() <= FLT_EPSILON) {
 			// just set the final target throttle value
 			_pusher_throttle = _param_vt_f_trans_thr.get();

 		} else if (_pusher_throttle <= _param_vt_f_trans_thr.get()) {
 			// ramp up throttle to the target throttle value
-			_pusher_throttle = _param_vt_f_trans_thr.get() * time_since_trans_start / _param_vt_psher_rmp_dt.get();
+			_pusher_throttle = math::min(_pusher_throttle +
+						     _param_vt_psher_slew.get() * _dt, _param_vt_f_trans_thr.get());
 		}

 		_airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get();
@@ -291,9 +292,8 @@ void Standard::update_transition_state()

 		if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
 			// Handle throttle reversal for active breaking
-			float thrscale = (time_since_trans_start - _param_vt_b_rev_del.get()) / (_param_vt_psher_rmp_dt.get());
-			thrscale = math::constrain(thrscale, 0.0f, 1.0f);
-			_pusher_throttle = thrscale * _param_vt_b_trans_thr.get();
+			_pusher_throttle = math::constrain((time_since_trans_start - _param_vt_b_rev_del.get())
+							   * _param_vt_psher_slew.get(), 0.0f, _param_vt_b_trans_thr.get());
 		}

 		// continually increase mc attitude control as we transition back to mc mode
@@ -84,7 +84,7 @@ private:
 	void parameters_update() override;

 	DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
-					(ParamFloat<px4::params::VT_PSHER_RMP_DT>) _param_vt_psher_rmp_dt,
+					(ParamFloat<px4::params::VT_PSHER_SLEW>) _param_vt_psher_slew,
 					(ParamFloat<px4::params::VT_B_TRANS_RAMP>) _param_vt_b_trans_ramp,
 					(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
 					(ParamFloat<px4::params::VT_B_REV_OUT>) _param_vt_b_rev_out,
@@ -108,7 +108,7 @@ PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f);
 *
 * Set this to a value greater than 0 to give the motor time to spin down.
 *
- * unit s
+ * @unit s
 * @min 0
 * @max 10
 * @increment 1
@@ -118,14 +118,16 @@ PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f);
 PARAM_DEFINE_FLOAT(VT_B_REV_DEL, 0.0f);

 /**
- * Pusher throttle ramp up window
+ * Pusher throttle ramp up slew rate
 *
- * Defines the time window during which the pusher throttle will be ramped up linearly to VT_F_TRANS_THR during a transition
- * to fixed wing mode. Zero or negative values will produce an instant throttle rise to VT_F_TRANS_THR.
+ * Defines the slew rate of the puller/pusher throttle during transitions.
+ * Zero will deactivate the slew rate limiting and thus produce an instant throttle
+ * rise to the transition throttle VT_F_TRANS_THR.
 *
- * @max 20
+ * @unit 1/s
+ * @min 0
 * @increment 0.01
 * @decimal 2
 * @group VTOL Attitude Control
 */
-PARAM_DEFINE_FLOAT(VT_PSHER_RMP_DT, 3.0f);
+PARAM_DEFINE_FLOAT(VT_PSHER_SLEW, 0.33f);
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	33b5437eee	mavlink: remove default device and consolidate serial vs UDP init handling	2022-11-18 11:53:56 -05:00
alexklimaj	64768f1cda	Increase allowed rtk injections to 8 for moving base. Update GPS submodule.	2022-11-18 11:04:17 -05:00
alexklimaj	8b61b22da6	Fix CANNODE_SUB_MBD typo	2022-11-18 11:04:17 -05:00
Jukka Laitinen	f2607335ac	rc/crsf_rc/CrsfRc.cpp: Include fcntl.h for "open" Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-11-18 07:10:27 +01:00
Jukka Laitinen	9081238dc5	microdds_client.cpp: Include posix.h for PX4_STACK_ADJUSTED macro Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-11-18 07:10:27 +01:00
Jukka Laitinen	9ce234ece8	SafetyButton.cpp: Include board_config.h Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-11-18 07:10:27 +01:00
Jukka Laitinen	ff3a3dac01	cdc_acm_check.cpp: Add missing #includes - Include board_config.h for BOARD_GET_EXTERNAL_LOCKOUT_STATE etc. macros - Include fcntl.h for "open" Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-11-18 07:10:27 +01:00
Jukka Laitinen	966560edc0	Fix overflows in abstime_to_ts Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2022-11-18 07:10:05 +01:00
Zachary Lowell	52b16d062c	uORB Remote Manager Update (#20623 )	2022-11-17 13:51:01 -08:00
Silvan Fuhrer	8b7c074680	FW Position Control: remove 0.9 mergin factor on stall vs min param comparison Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-11-17 13:33:10 +01:00
Silvan Fuhrer	2e0c8da7ef	FW Position Control: fix load factor calculation Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-11-17 13:33:10 +01:00
Silvan Fuhrer	798cc4f01c	LandDetectorMC: enforce that LNDMC_Z_VEL_MAX is larger than MPC_LAND_CRWL/MPC_LAND_SPEED (#20614 ) * LandDetectorMC: enforce that LNDMC_Z_VEL_MAX * 1.2 is below MPC_LAND_CRWL/MPC_LAND_SPEED Otherwise the _in_descend flag doesn't get set correctly during the last part of the landing, where the descend speed is at MPC_LAND_CRWL or LAND_SPEED. The _in_descend flag is set it the velocity setpoint is >1.1LNDMC_Z_VEL_MAX. Signed-off-by: Silvan Fuhrer <silvan@auterion.com> Co-authored-by: Matthias Grob <maetugr@gmail.com>	2022-11-17 10:56:26 +01:00
Matthias Grob	e58ad581a0	modalai_esc_params: remove unused parameter	2022-11-17 10:50:41 +01:00
Matthias Grob	fd4d4e001d	PULL_REQUEST_TEMPLATE: suggestion to make it more "concise"	2022-11-16 17:28:27 +01:00
TSC21	9c0e09c3df	mavlink_main: report in AUTOPILOT_VERSION.capabilities that FLIGHT_TERMINATION is supported	2022-11-16 16:51:47 +01:00
Matthias Grob	d4f18bda8e	navigator_params: remove deprecated parameters The usage of `NAV_AH_...` was removed in `5d33b9e999` #14307	2022-11-16 10:26:12 -05:00
Matthias Grob	fbe5024fa8	commander_params: remove deprecated COM_RCL_ACT_T It's now covered with COM_FAIL_ACT_T	2022-11-16 08:11:09 +01:00
Daniel Agar	da82757bf6	ekf2: accumulate multiple vel/pos/orientation reset deltas per update - if in a single EKF update there are multiple resets we need to track the accumulated delta so the change consumed by the controllers is correct	2022-11-15 13:20:54 -05:00
alexklimaj	2e918eba00	Revert "Update MAVLink so common includes standard (#20542 )" This reverts commit `0e2b1ee979`.	2022-11-15 13:09:56 -05:00
Eric Katzfey	796fa8bd72	boards/modalai: separate voxl2 builds into two board builds instead of single board build with different variants * Made voxl2 apps processor and slpi dsp processor builds into separate board builds so that they can more easily be configured independently. * Removed board specific link library command from px4_config.cmake and moved it to a more generic board specific solution that can be used by any board that needs custom link libraries. * Removed redundant cmake command for Qurt * Removed unused definition from Qurt cmake file * Removed unnecessary QURT_LIB cmake function * Reorganized the voxl2 build structure to avoid 4 level board directories. * Reverted cmake files to remove 4 level board naming code * Updated documentation	2022-11-15 13:09:04 -05:00
bresch	22420a7bf1	ekf2: do not fuse ZVU if other velocity source is active	2022-11-15 11:17:07 -05:00
bresch	c67f03f383	ekf2: update change indicator	2022-11-14 11:51:32 -05:00
bresch	f319cc528b	ekf2: remove old flow fusion generated code	2022-11-14 11:51:32 -05:00
bresch	2a83dbf81d	ekf2_test: compare flow fusion sympy vs symforce	2022-11-14 11:51:32 -05:00
bresch	93564baccf	ekf2_flow: recalculate innovation after fusing 1st axis The state changed so we need to recalculate the innovation of the 2nd axis after fusing the 1st one	2022-11-14 11:51:32 -05:00
bresch	4dbdf23346	ekf2: update _R_to_earth when the state quat changed through fusion	2022-11-14 11:51:32 -05:00
bresch	28458340e6	ekf2_flow: check innov variance health after fusing 1st axis	2022-11-14 11:51:32 -05:00
bresch	73a8c388e8	ekf2: fuse by looping through axes	2022-11-14 11:51:32 -05:00
bresch	b54a4417fa	ekf2: migrate flow fusion to SymForce	2022-11-14 11:51:32 -05:00
Daniel Agar	639d1ddca2	ekf2: update flags (in_air, at_rest, etc) on delayed time horizon (#20576 ) - in a lot of cases this won't really matter, although you can see it in logs (especially with things like vehicle_at_rest), but it's something we might as well do properly Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>	2022-11-14 11:35:40 -05:00
Beat Küng	b0e1cc72f7	fix orbit for mc: handle VEHICLE_CMD_DO_ORBIT command and avoid race condition Fixes a regression from `8bae4e5c0e`, where the orbit flight task wasn't an extra task (flight_tasks_to_add) anymore and therefore the command handling wasn't generated. There was a race condition that could cause several outcomes. The most severe was that flight_mode_manager gets the command, switches to orbit and then in the next iteration switches back because commander did not change nav_state yet. When commander then switches, flight_mode_manager would still be in the old mode. This is prevented by storing the command (allowing it to arrive before or after mode switch), and then apply it after the switch happens.	2022-11-14 17:31:35 +01:00
Beat Küng	c1f9824396	flight_mode_manager: remove command ack for VEHICLE_CMD_DO_ORBIT Already acked in commander	2022-11-14 17:31:35 +01:00
Daniel Agar	9e7db0ed54	merge vehicle_angular_acceleration into vehicle_angular_velocity (#20531 ) - vehicle_angular_velocity and vehicle_angular_acceleration are produced together from the same input data, consumed together, and share the the same metadata (timestamps) - individually these topics each have 16 bytes of metadata (2 timestamps) for 12 bytes of data (x,y,z float32)	2022-11-14 11:03:59 -05:00
bresch	7d1f1d0f84	ekf2: replace macro constants by typed constexpr	2022-11-14 10:45:23 -05:00
bresch	06702da003	ekf2: add GNSS yaw max interval yaw data usually comes at lower rate than vel/pos	2022-11-14 10:45:23 -05:00
bresch	9834c7917b	ekf2: use yaw emergency estimator more aggressively During the whole flight, if the difference between the yaw estimate from EKF2 and the emergency estimator is large and that the GNSS velocity fusion is failing continuously, immediately reset to the emergency yaw estimate.	2022-11-14 10:45:23 -05:00
GaspardBesacier	dfced1fe46	VTOL: Smarter pusher ramp up during front transtitions for standard VTOLs (#20394 ) New param VT_PSHER_SLEW for ramping up throttle of pusher during front/back transition, that replaces the old VT_PSHER_RMP_DT param.	2022-11-14 16:32:51 +01:00
Silvan Fuhrer	f5ecd1106f	VTOL: some parater meta data fixes/improvements Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-11-14 16:25:46 +01:00
Silvan Fuhrer	c04a67401e	Remove some @decimal and @increment from integer parameters Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-11-14 16:25:46 +01:00
Silvan Fuhrer	a018debd37	FW Position controller: fix some parameter meta data Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-11-14 16:25:46 +01:00
Silvan Fuhrer	0d7a029bfc	FW Attitude controller params: fix meta data for Acro rates Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-11-14 16:25:46 +01:00
Matthias Grob	30d74f124d	commander_params: make disarm timeouts more explicit decimals of 100ms increment	2022-11-14 16:25:46 +01:00
Silvan Fuhrer	464a7fcbed	Commander params: add @decimal to COM_SPOOLUP_TIME Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-11-14 16:25:46 +01:00