uavcan: servers handle move firmware early

Move uavcan start to end of rcS to prevent sd card read lock
ekf2: cleanup terrain estimator init (shouldn't be valid by default)
2026-05-30 06:30:05 +08:00 · 2022-11-14 09:35:13 -05:00 · 2022-11-14 09:32:10 -05:00 · 2022-11-14 09:29:46 -05:00 · 2022-11-14 09:26:14 -05:00 · 2022-11-14 11:27:23 +01:00
161 changed files with 3754 additions and 1626 deletions
@@ -5,7 +5,7 @@ labels: feature-request

 ---

-For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Slack (you can find an invite link on this project README).
+For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Discord (you can find an invite link on this project README).

 ## Describe problem solved by the proposed feature
 A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...
@@ -1,4 +1,4 @@
-Please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
+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.
@@ -94,21 +94,22 @@ pipeline {

        stage('failsafe docs') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
+            docker { image 'px4io/px4-dev-nuttx-focal:2021-08-18' }
          }
          steps {
            sh '''#!/bin/bash -l
-			echo $0;
+            echo $0;
            git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk;
            cd _emscripten_sdk;
            ./emsdk install latest;
            ./emsdk activate latest;
+            cd ..;
            . ./_emscripten_sdk/emsdk_env.sh;
-			make failsafe_web;
-			cd build/px4_sitl_default_failsafe_web;
-			mkdir -p failsafe_sim;
-			cp index.* parameters.json failsafe_sim;
-			'''
+            make failsafe_web;
+            cd build/px4_sitl_default_failsafe_web;
+            mkdir -p failsafe_sim;
+            cp index.* parameters.json failsafe_sim;
+            '''
            dir('build/px4_sitl_default_failsafe_web') {
              archiveArtifacts(artifacts: 'failsafe_sim/*')
              stash includes: 'failsafe_sim/*', name: 'failsafe_sim'
@@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

@@ -10,6 +10,7 @@
 # EKF2: Vision position and heading
 param set-default EKF2_AID_MASK 24
 param set-default EKF2_EV_DELAY 5
+param set-default EKF2_EV_CTRL 15
 param set-default EKF2_GPS_CTRL 0

 # LPE: Vision + baro
@@ -70,7 +70,7 @@ param set-default MPC_XY_VEL_D_ACC 0.1
 param set-default NAV_ACC_RAD 5

 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.5
+param set-default VT_FW_DIFTHR_S_Y 0.5
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0
@@ -22,7 +22,7 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

@@ -191,7 +191,10 @@ then
 	ist8308 -X -q start
 	ist8310 -X -q start
 	lis2mdl -X -q start
-	lis3mdl -X -q start
+	if ! lis3mdl -X -q start
+	then
+		lis3mdl -X -q -a 0x1c start
+	fi
 	qmc5883l -X -q start
 	rm3100 -X -q start

@@ -264,23 +264,6 @@ else
 		. $FCONFIG
 	fi

-	#
-	# Check if UAVCAN is enabled, default to it for ESCs.
-	#
-	if param greater -s UAVCAN_ENABLE 0
-	then
-		# Start core UAVCAN module.
-		if ! uavcan start
-		then
-			tune_control play error
-		fi
-	else
-		if param greater -s CYPHAL_ENABLE 0
-		then
-			cyphal start
-		fi
-	fi
-
 	#
 	# Start IO for PWM output or RC input if enabled
 	#
@@ -527,6 +510,23 @@ else
 	fi
 	unset BOARD_BOOTLOADER_UPGRADE

+	#
+	# Check if UAVCAN is enabled, default to it for ESCs.
+	#
+	if param greater -s UAVCAN_ENABLE 0
+	then
+		# Start core UAVCAN module.
+		if ! uavcan start
+		then
+			tune_control play error
+		fi
+	else
+		if param greater -s CYPHAL_ENABLE 0
+		then
+			cyphal start
+		fi
+	fi
+
 #
 # End of autostart.
 #
@@ -51,9 +51,9 @@ div.frame_variant td, div.frame_variant th {
  text-align : left;
 }
 </style>\n\n"""
- 
+
        type_set = set()
-        
+
        if len(image_path) > 0 and image_path[-1] != '/':
            image_path = image_path + '/'

@@ -62,7 +62,7 @@ div.frame_variant td, div.frame_variant th {
               result += '## %s\n\n' % group.GetClass()
               type_set.add(group.GetClass())

-            result += '### %s\n\n' % group.GetName()
+            result += '### %s\n\n' % group.GetType()

            # Display an image of the frame
            image_name = group.GetImageName()
@@ -73,11 +73,11 @@ div.frame_variant td, div.frame_variant th {
            # check if all outputs are equal for the group: if so, show them
            # only once
            all_outputs = {}
-            num_configs = len(group.GetParams())
-            for param in group.GetParams():
-                if not self.IsExcluded(param, board):
-                    for output_name in param.GetOutputCodes():
-                        value = param.GetOutputValue(output_name)
+            num_configs = len(group.GetAirframes())
+            for airframe in group.GetAirframes():
+                if not self.IsExcluded(airframe, board):
+                    for output_name in airframe.GetOutputCodes():
+                        value = airframe.GetOutputValue(output_name)
                        key_value_pair = (output_name, value)
                        if key_value_pair not in all_outputs:
                            all_outputs[key_value_pair] = 0
@@ -104,18 +104,17 @@ div.frame_variant td, div.frame_variant th {
            result += ' </thead>\n'
            result += '<tbody>\n'

-            for param in group.GetParams():
-                if not self.IsExcluded(param, board):
-                    #print("generating: {0} {1}".format(param.GetName(), excluded))
-                    name = param.GetName()
-                    airframe_id = param.GetId()
+            for airframe in group.GetAirframes():
+                if not self.IsExcluded(airframe, board):
+                    name = airframe.GetName()
+                    airframe_id = airframe.GetId()
                    airframe_id_entry = '<p><code>SYS_AUTOSTART</code> = %s</p>' % (airframe_id)
-                    maintainer = param.GetMaintainer()
+                    maintainer = airframe.GetMaintainer()
                    maintainer_entry = ''
                    if maintainer != '':
                        maintainer_entry = 'Maintainer: %s' % (html.escape(maintainer))
-                    url = param.GetFieldValue('url')
-                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetName(),name)
+                    url = airframe.GetFieldValue('url')
+                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetType(),name)
                    name_anchor=name_anchor.replace(' ','_').lower()
                    name_anchor=name_anchor.replace('"','_').lower()
                    name_anchor='id="%s"' % name_anchor
@@ -124,8 +123,8 @@ div.frame_variant td, div.frame_variant th {
                        name_entry = '<a href="%s">%s</a>' % (url, name)
                    outputs = '<ul>'
                    has_outputs = False
-                    for output_name in param.GetOutputCodes():
-                        value = param.GetOutputValue(output_name)
+                    for output_name in airframe.GetOutputCodes():
+                        value = airframe.GetOutputValue(output_name)
                        valstrs = value.split(";")
                        key_value_pair = (output_name, value)
                        if all_outputs[key_value_pair] < num_configs:
@@ -152,9 +151,9 @@ div.frame_variant td, div.frame_variant th {

        self.output = result

-    def IsExcluded(self, param, board):
-        for code in param.GetArchCodes():
-            if "CONFIG_ARCH_BOARD_{0}".format(code) == board and param.GetArchValue(code) == "exclude":
+    def IsExcluded(self, airframe, board):
+        for code in airframe.GetArchCodes():
+            if "CONFIG_ARCH_BOARD_{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
                return True
        return False

@@ -3,6 +3,9 @@ import codecs
 import os

 class RCOutput():
+    """
+        Generates RC scripts for the airframes
+    """
    def __init__(self, groups, board, post_start=False):

        result = (  "#\n"
@@ -34,33 +37,33 @@ class RCOutput():
        result += "set AIRFRAME none\n"
        result += "\n"
        for group in groups:
-            result += "# GROUP: %s\n\n" % group.GetName()
-            for param in group.GetParams():
+            result += "# GROUP: %s\n\n" % group.GetType()
+            for airframe in group.GetAirframes():
                excluded = False
-                for code in param.GetArchCodes():
-                    if "{0}".format(code) == board and param.GetArchValue(code) == "exclude":
+                for code in airframe.GetArchCodes():
+                    if "{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
                        excluded = True
                if excluded:
                    continue

                if post_start:
                    # Path to post-start sript
-                    path = param.GetPostPath()
+                    path = airframe.GetPostPath()
                else:
                    # Path to start script
-                    path = param.GetPath()
+                    path = airframe.GetPath()

                if not path:
                    continue

                path = os.path.split(path)[1]

-                id_val = param.GetId()
-                name = param.GetFieldValue("short_desc")
-                long_desc = param.GetFieldValue("long_desc")
+                id_val = airframe.GetId()
+                name = airframe.GetFieldValue("short_desc")
+                long_desc = airframe.GetFieldValue("long_desc")

                result +=   "#\n"
-                result +=   "# %s\n" % param.GetName()
+                result +=   "# %s\n" % airframe.GetName()
                result +=   "if param compare SYS_AUTOSTART %s\n" % id_val
                result +=   "then\n"
                result +=   "\tset AIRFRAME %s\n" % path
@@ -2,31 +2,38 @@ import sys
 import re
 import os

-class ParameterGroup(object):
+class AirframeGroup(object):
    """
-    Single parameter group
+    Airframe group
+
+    type: specific vehicle type (e.g. VTOL Tiltrotor, VTOL Quadrotor, etc.)
+    class: vehicle class (e.g. Multicopter, Fixed Wing, etc.)
    """
-    def __init__(self, name, af_class):
-        self.name = name
+    def __init__(self, type, af_class):
+        self.type = type
        self.af_class = af_class
-        self.params = []
+        self.airframes = []


-    def AddParameter(self, param):
+    def AddAirframe(self, airframe):
        """
-        Add parameter to the group
+        Add airframe to the airframe group
        """
-        self.params.append(param)
+        self.airframes.append(airframe)

-    def GetName(self):
+    def GetType(self):
        """
-        Get parameter group name
+        Get airframe group's vehicle type
+
+        e.g. VTOL Tiltrotor, VTOL Quadrotor, etc.
        """
-        return self.name
+        return self.type

    def GetClass(self):
        """
-        Get parameter group vehicle type.
+        Get airframe group's vehicle class
+
+        e.g. Multicopter, Fixed Wing, etc.
        """
        return self.af_class

@@ -34,86 +41,84 @@ class ParameterGroup(object):
        """
        Get parameter group image base name (w/o extension)
        """
-        if (self.name == "Standard Plane"):
+        if (self.type == "Standard Plane"):
            return "Plane"
-        elif (self.name == "Flying Wing"):
+        elif (self.type == "Flying Wing"):
            return "FlyingWing"
-        elif (self.name == "Quadrotor x"):
+        elif (self.type == "Quadrotor x"):
            return "QuadRotorX"
-        elif (self.name == "Quadrotor +"):
+        elif (self.type == "Quadrotor +"):
            return "QuadRotorPlus"
-        elif (self.name == "Hexarotor x"):
+        elif (self.type == "Hexarotor x"):
            return "HexaRotorX"
-        elif (self.name == "Hexarotor +"):
+        elif (self.type == "Hexarotor +"):
            return "HexaRotorPlus"
-        elif (self.name == "Octorotor +"):
+        elif (self.type == "Octorotor +"):
            return "OctoRotorPlus"
-        elif (self.name == "Octorotor x"):
+        elif (self.type == "Octorotor x"):
            return "OctoRotorX"
-        elif (self.name == "Octorotor Coaxial"):
+        elif (self.type == "Octorotor Coaxial"):
            return "OctoRotorXCoaxial"
-        elif (self.name == "Octo Coax Wide"):
+        elif (self.type == "Octo Coax Wide"):
            return "OctoRotorXCoaxial"
-        elif (self.name == "Quadrotor Wide"):
+        elif (self.type == "Quadrotor Wide"):
            return "QuadRotorWide"
-        elif (self.name == "Quadrotor H"):
+        elif (self.type == "Quadrotor H"):
            return "QuadRotorH"
-        elif (self.name == "Dodecarotor cox"):
+        elif (self.type == "Dodecarotor cox"):
            return "DodecaRotorXCoaxial"
-        elif (self.name == "Simulation"):
+        elif (self.type == "Simulation"):
            return "AirframeSimulation"
-        elif (self.name == "Plane A-Tail"):
+        elif (self.type == "Plane A-Tail"):
            return "PlaneATail"
-        elif (self.name == "Plane V-Tail"):
+        elif (self.type == "Plane V-Tail"):
            return "PlaneVTail"
-        elif (self.name == "VTOL Duo Tailsitter"):
+        elif (self.type == "VTOL Duo Tailsitter"):
            return "VTOLDuoRotorTailSitter"
-        elif (self.name == "Standard VTOL"):
+        elif (self.type == "Standard VTOL"):
            return "VTOLPlane"
-        elif (self.name == "VTOL Quad Tailsitter"):
+        elif (self.type == "VTOL Quad Tailsitter"):
            return "VTOLQuadRotorTailSitter"
-        elif (self.name == "VTOL Tiltrotor"):
+        elif (self.type == "VTOL Tiltrotor"):
            return "VTOLTiltRotor"
-        elif (self.name == "VTOL Octoplane"):
+        elif (self.type == "VTOL Octoplane"):
            return "VTOLPlaneOcto"
-        elif (self.name == "Coaxial Helicopter"):
+        elif (self.type == "Coaxial Helicopter"):
            return "HelicopterCoaxial"
-        elif (self.name == "Helicopter"):
+        elif (self.type == "Helicopter"):
            return "Helicopter"
-        elif (self.name == "Hexarotor Coaxial"):
+        elif (self.type == "Hexarotor Coaxial"):
            return "Y6B"
-        elif (self.name == "Y6A"):
+        elif (self.type == "Y6A"):
            return "Y6A"
-        elif (self.name == "Tricopter Y-"):
+        elif (self.type == "Tricopter Y-"):
            return "YMinus"
-        elif (self.name == "Tricopter Y+"):
+        elif (self.type == "Tricopter Y+"):
            return "YPlus"
-        elif (self.name == "Autogyro"):
+        elif (self.type == "Autogyro"):
            return "Autogyro"
-        elif (self.name == "Airship"):
+        elif (self.type == "Airship"):
            return "Airship"
-        elif (self.name == "Rover"):
+        elif (self.type == "Rover"):
            return "Rover"
-        elif (self.name == "Boat"):
+        elif (self.type == "Boat"):
            return "Boat"
-        elif (self.name == "Balloon"):
+        elif (self.type == "Balloon"):
            return "Balloon"
-        elif (self.name == "Vectored 6 DOF UUV"):
+        elif (self.type == "Vectored 6 DOF UUV"):
            return "Vectored6DofUUV"
        return "AirframeUnknown"

-    def GetParams(self):
+    def GetAirframes(self):
        """
-        Returns the parsed list of parameters. Every parameter is a Parameter
-        object. Note that returned object is not a copy. Modifications affect
-        state of the parser.
+        Returns the parsed list of airframes objects. Note that returned
+        object is not a copy. Modifications affect state of the parser.
        """
+        return sorted(self.airframes, key=lambda x: x.GetId())

-        return sorted(self.params, key=lambda x: x.GetId())
-
-class Parameter(object):
+class Airframe(object):
    """
-    Single parameter
+    Single Airframe definition
    """

    # Define sorting order of the fields
@@ -288,7 +293,7 @@ class SourceParser(object):
    }

    def __init__(self):
-        self.param_groups = {}
+        self.airframe_groups = {}

    def GetSupportedExtensions(self):
        """
@@ -347,10 +352,10 @@ class SourceParser(object):
                            tag, desc = m.group(1, 2)
                            if (tag == "output"):
                                key, text = desc.split(' ', 1)
-                                outputs[key] = text;
+                                outputs[key] = text
                            elif (tag == "board"):
                                key, text = desc.split(' ', 1)
-                                archs[key] = text;
+                                archs[key] = text
                            else:
                                tags[tag] = desc
                            current_tag = tag
@@ -427,7 +432,7 @@ class SourceParser(object):
            post_path = None

        # We already know this is an airframe config, so add it
-        param = Parameter(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)
+        airframe = Airframe(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)

        # Done with file, store
        for tag in tags:
@@ -440,24 +445,24 @@ class SourceParser(object):
            if tag == "name":
                airframe_name = tags[tag]
            else:
-                param.SetField(tag, tags[tag])
+                airframe.SetField(tag, tags[tag])

        # Store outputs
        for output in outputs:
-            param.SetOutput(output, outputs[output])
+            airframe.SetOutput(output, outputs[output])

        # Store outputs
        for arch in archs:
-            param.SetArch(arch, archs[arch])
+            airframe.SetArch(arch, archs[arch])

        # Store the parameter

        # Create a class-specific airframe group. This is needed to catch cases where an airframe type might cross classes (e.g. simulation)
-        class_group_identifier=airframe_type+airframe_class
-        if class_group_identifier not in self.param_groups:
-            #self.param_groups[airframe_type] = ParameterGroup(airframe_type)  #HW TEST REMOVE
-            self.param_groups[class_group_identifier] = ParameterGroup(airframe_type, airframe_class)
-        self.param_groups[class_group_identifier].AddParameter(param)
+        class_group_identifier=airframe_type + airframe_class
+        if class_group_identifier not in self.airframe_groups:
+            #self.airframe_groups[airframe_type] = ParameterGroup(airframe_type)  #HW TEST REMOVE
+            self.airframe_groups[class_group_identifier] = AirframeGroup(airframe_type, airframe_class)
+        self.airframe_groups[class_group_identifier].AddAirframe(airframe)

        return True

@@ -473,8 +478,8 @@ class SourceParser(object):
        Validates the airframe meta data.
        """
        seenParamNames = []
-        for group in self.GetParamGroups():
-            for param in group.GetParams():
+        for group in self.GetAirframeGroups():
+            for param in group.GetAirframes():
                name  = param.GetName()
                board = param.GetFieldValue("board")
                # Check for duplicates
@@ -487,27 +492,27 @@ class SourceParser(object):

        return True

-    def GetParamGroups(self):
+    def GetAirframeGroups(self):
        """
-        Returns the parsed list of parameters. Every parameter is a Parameter
+        Returns the parsed list of Airframe groups. Every Airframe is an Airframe
        object. Note that returned object is not a copy. Modifications affect
        state of the parser.
        """
-        groups = self.param_groups.values()
-        groups = sorted(groups, key=lambda x: x.GetName())
+        groups = self.airframe_groups.values()
+        groups = sorted(groups, key=lambda x: x.GetType())
        groups = sorted(groups, key=lambda x: x.GetClass())
-        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
+        groups = sorted(groups, key=lambda x: self.priority.get(x.GetType(), 0), reverse=True)

        #Rename duplicate groups to include the class (creating unique headings in page TOC)
        duplicate_test=set()
        duplicate_set=set()
        for group in groups:
-            if group.GetName() in duplicate_test:
-                duplicate_set.add(group.GetName())
+            if group.GetType() in duplicate_test:
+                duplicate_set.add(group.GetType())
            else:
-                duplicate_test.add(group.GetName() )
+                duplicate_test.add(group.GetType() )
        for group in groups:
-            if group.GetName() in duplicate_set:
-                group.name=group.GetName()+' (%s)' % group.GetClass()
+            if group.GetType() in duplicate_set:
+                group.name=group.GetType()+' (%s)' % group.GetClass()

        return groups
@@ -28,28 +28,28 @@ class XMLOutput():
        xml_version.text = "1"
        for group in groups:
            xml_group = ET.SubElement(xml_parameters, "airframe_group")
-            xml_group.attrib["name"] = group.GetName()
+            xml_group.attrib["name"] = group.GetType()
            xml_group.attrib["image"] = group.GetImageName()
-            for param in group.GetParams():
+            for airframe in group.GetAirframes():

                # check if there is an exclude tag for this airframe
                excluded = False
-                for code in param.GetArchCodes():
-                    if "CONFIG_ARCH_BOARD_{0}".format(code) == board and param.GetArchValue(code) == "exclude":
+                for code in airframe.GetArchCodes():
+                    if "CONFIG_ARCH_BOARD_{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
                        excluded = True

                if not excluded:
-                    #print("generating: {0} {1}".format(param.GetName(), excluded))
+                    #print("generating: {0} {1}".format(airframe.GetName(), excluded))
                    xml_param = ET.SubElement(xml_group, "airframe")
-                    xml_param.attrib["name"] = param.GetName()
-                    xml_param.attrib["id"] = param.GetId()
-                    xml_param.attrib["maintainer"] = param.GetMaintainer()
-                    for code in param.GetFieldCodes():
-                        value = param.GetFieldValue(code)
+                    xml_param.attrib["name"] = airframe.GetName()
+                    xml_param.attrib["id"] = airframe.GetId()
+                    xml_param.attrib["maintainer"] = airframe.GetMaintainer()
+                    for code in airframe.GetFieldCodes():
+                        value = airframe.GetFieldValue(code)
                        xml_field = ET.SubElement(xml_param, code)
                        xml_field.text = value
-                    for code in param.GetOutputCodes():
-                        value = param.GetOutputValue(code)
+                    for code in airframe.GetOutputCodes():
+                        value = airframe.GetOutputValue(code)
                        valstrs = value.split(";")
                        xml_field = ET.SubElement(xml_param, "output")
                        xml_field.attrib["name"] = code
@@ -35,12 +35,11 @@
 #
 # PX4 airframe config processor (main executable file)
 #
-# This tool scans the PX4 ROMFS code for declarations of airframes
-#
-# Currently supported formats are:
-#   * XML for the parametric UI generator
-#   * Markdown for the PX4 dev guide (https://github.com/PX4/Devguide)
+# This tool scans the PX4 ROMFS directory for declarations of airframes
 #
+# Currently supported output formats are:
+#   * XML for the parametric UI generator (Used in QGC)
+#   * Markdown for the PX4 User guide (https://github.com/PX4/PX4-user_guide)
 #

 from __future__ import print_function
@@ -104,31 +103,31 @@ def main():
    # We can't validate yet
    # if not parser.Validate():
    #     sys.exit(1)
-    param_groups = parser.GetParamGroups()
+    airframe_groups = parser.GetAirframeGroups()

    # Output to XML file
    if args.xml:
        if args.verbose: print("Creating XML file " + args.xml)
-        out = xmlout.XMLOutput(param_groups, args.board)
+        out = xmlout.XMLOutput(airframe_groups, args.board)
        out.Save(args.xml)

    # Output to markdown file
    if args.markdown:
        if args.verbose: print("Creating markdown file " + args.markdown)
-        out = markdownout.MarkdownTablesOutput(param_groups, args.board, args.image_path)
+        out = markdownout.MarkdownTablesOutput(airframe_groups, args.board, args.image_path)
        out.Save(args.markdown)

    # Output to start scripts
    if args.start_script:
        # Airframe start script
        if args.verbose: print("Creating start script " + args.start_script)
-        out = rcout.RCOutput(param_groups, args.board)
+        out = rcout.RCOutput(airframe_groups, args.board)
        out.Save(args.start_script)

        # Airframe post-start script
        post_start_script = args.start_script + '.post'
        if args.verbose: print("Creating post-start script " + post_start_script)
-        out_post = rcout.RCOutput(param_groups, args.board, post_start=True)
+        out_post = rcout.RCOutput(airframe_groups, args.board, post_start=True)
        out_post.Save(post_start_script)

    if (args.verbose): print("All done!")
@@ -582,7 +582,35 @@ class uploader(object):
        self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)

    # upload the firmware
-    def upload(self, fw, force=False, boot_delay=None):
+    def upload(self, fw_list, force=False, boot_delay=None, boot_check=False):
+        # select correct binary
+        found_suitable_firmware = False
+        for file in fw_list:
+            fw = firmware(file)
+            if self.board_type == fw.property('board_id'):
+                if len(fw_list) > 1: print("using firmware binary {}".format(file))
+                found_suitable_firmware = True
+                break
+
+        if not found_suitable_firmware:
+            msg = "Firmware not suitable for this board (Firmware board_type=%u board_id=%u)" % (
+                self.board_type, fw.property('board_id'))
+            print("WARNING: %s" % msg)
+            if force:
+                if len(fw_list) > 1:
+                    raise FirmwareNotSuitableException("force flashing failed, more than one file provided, none suitable")
+                print("FORCED WRITE, FLASHING ANYWAY!")
+            else:
+                raise FirmwareNotSuitableException(msg)
+
+        percent = fw.property('image_size') / fw.property('image_maxsize')
+        binary_size = float(fw.property('image_size'))
+        binary_max_size = float(fw.property('image_maxsize'))
+        percent = (binary_size / binary_max_size) * 100
+
+        print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%) " % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
+        print()
+
        # Make sure we are doing the right thing
        start = _time()
        if self.board_type != fw.property('board_id'):
@@ -764,7 +792,7 @@ def main():
    parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading')
    parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash')
    parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot')
-    parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
+    parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)")
    args = parser.parse_args()

    if args.use_protocol_splitter_format:
@@ -776,17 +804,7 @@ def main():
        print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)")
        print("==========================================================================================================")

-    # Load the firmware file
-    fw = firmware(args.firmware)
-
-    percent = fw.property('image_size') / fw.property('image_maxsize')
-    binary_size = float(fw.property('image_size'))
-    binary_max_size = float(fw.property('image_maxsize'))
-    percent = (binary_size / binary_max_size) * 100
-
-    print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%), waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
-    print()
-
+    print("Waiting for bootloader...")
    # tell any GCS that might be connected to the autopilot to give up
    # control of the serial port

@@ -889,7 +907,7 @@ def main():

                try:
                    # ok, we have a bootloader, try flashing it
-                    up.upload(fw, force=args.force, boot_delay=args.boot_delay)
+                    up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay)

                    # if we made this far without raising exceptions, the upload was successful
                    successful = True
@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {

 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,
@@ -2,3 +2,5 @@ CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_MODULES_MUORB_APPS=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_UORB=y
@@ -1,3 +1,4 @@
 CONFIG_PLATFORM_QURT=y
 CONFIG_BOARD_TOOLCHAIN="qurt"
 CONFIG_MODULES_MUORB_SLPI=y
+CONFIG_SYSTEMCMDS_UORB=y
@@ -41,7 +41,7 @@

 #define BOARD_HAS_NO_RESET
 #define BOARD_HAS_NO_BOOTLOADER
-
+#define ORB_COMMUNICATOR 1
 /*
 * I2C buses
 */
@@ -99,14 +99,14 @@ __BEGIN_DECLS

 /* Timer I/O PWM and capture
 *
- * ?? PWM outputs are configured.
+ * 2 PWM outputs are configured.
 * ?? Timer inputs are configured.
 *
 * Pins:
 *      Defined in board.h
 */

-#define DIRECT_PWM_OUTPUT_CHANNELS  1
+#define DIRECT_PWM_OUTPUT_CHANNELS  2


 #define BOARD_HAS_LED_PWM              1
@@ -105,11 +105,13 @@
 #define rPWMLOAD(t)    REG(t,S32K1XX_FTM_PWMLOAD_OFFSET)

 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::FTM1),
 	initIOTimer(Timer::FTM2),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::FTM2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::FTM1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin1}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -154,17 +156,26 @@ void ucans32k_timer_initialize(void)
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM0_SC) = regval;

+	regval = _REG(S32K1XX_FTM1_SC);
+	regval &= ~(FTM_SC_CLKS_MASK);
+	regval |= FTM_SC_CLKS_FTM;
+	_REG(S32K1XX_FTM1_SC) = regval;
+
 	regval = _REG(S32K1XX_FTM2_SC);
 	regval &= ~(FTM_SC_CLKS_MASK);
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM2_SC) = regval;

-	/* Enabled System Clock Gating Control for FTM0, and FTM2 */
+	/* Enabled System Clock Gating Control for FTM0, FTM1 and FTM2 */

 	regval = _REG(S32K1XX_PCC_FTM0);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM0) = regval;

+	regval = _REG(S32K1XX_PCC_FTM1);
+	regval |= PCC_CGC;
+	_REG(S32K1XX_PCC_FTM1) = regval;
+
 	regval = _REG(S32K1XX_PCC_FTM2);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM2) = regval;
@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {

 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,
@@ -65,12 +65,17 @@ static const px4_mtd_entry_t fmum_fram = {

 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,
@@ -74,6 +74,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {

 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,
@@ -66,8 +66,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -170,6 +170,9 @@ function(px4_add_module)
 		if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL)
 			target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel)
 			set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE})
+		elseif(${PX4_PLATFORM} STREQUAL "qurt")
+			target_link_libraries(${MODULE} PRIVATE px4_layer uORB)
+			set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
 		else()
 			target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB)
 			set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
@@ -5,5 +5,5 @@ float64 lat					# Latitude in degrees (WGS84)
 float64 lon					# Longitude in degrees (WGS84)
 float32 alt					# Altitude (AMSL)
 float32 ground_distance			# Altitude above ground (meters)
-float32[4] q					# Attitude of the camera, zero rotation is facing towards front of vehicle
+float32[4] q					# Attitude of the camera relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude
 int8 result					# 1 for success, 0 for failure, -1 if camera does not provide feedback
@@ -1,12 +1,10 @@
 uint64 timestamp                        # time since system start (microseconds)

 bool torque_setpoint_achieved           # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_torque             # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
                                        # Computed as: unallocated_torque = torque_setpoint - allocated_torque

 bool thrust_setpoint_achieved           # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_thrust             # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_thrust           # Unallocated thrust. Equal to 0 if the setpoint was achieved.
                                        # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust

@@ -13,7 +13,7 @@ float32[4] q_d			# Desired quaternion for quaternion control
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body		# Normalized thrust command in body NED frame [-1,1]

-bool reset_rate_integrals	# Reset roll/pitch/yaw integrals (navigation logic change)
+bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)

 bool fw_control_yaw		# control heading with rudder (used for auto takeoff on runway)

@@ -12,6 +12,7 @@ bool has_low_throttle

 bool vertical_movement
 bool horizontal_movement
+bool rotational_movement

 bool close_to_ground_or_skipped_check

@@ -8,3 +8,5 @@ float32 yaw		# [rad/s] yaw rate setpoint
 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body	# Normalized thrust command in body NED frame [-1,1]
+
+bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change)
@@ -33,7 +33,7 @@

 set(SRCS)

-if(NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
+if(NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
 	list(APPEND SRCS
 		px4_log.cpp
 		px4_log_history.cpp
@@ -136,5 +136,9 @@ __END_DECLS
 #define M_LOG2_E_F		0.69314718f
 #define M_INVLN2_F		1.44269504f	// 1 / log(2)

+/* The M_PI, as stated above, is not C standard. If you need it and
+ * it isn't in your math.h file then you can use this instead. */
+#define M_PI_PRECISE	3.141592653589793238462643383279502884
+
 #define M_DEG_TO_RAD 		0.017453292519943295
 #define M_RAD_TO_DEG 		57.295779513082323
@@ -37,12 +37,13 @@ typedef enum  {
 	MTD_PARAMETERS  = 1,
 	MTD_WAYPOINTS   = 2,
 	MTD_CALDATA     = 3,
-	MTD_MFT         = 4,
-	MTD_ID          = 5,
-	MTD_NET         = 6,
+	MTD_MFT_VER     = 4,
+	MTD_MFT_REV     = 5,
+	MTD_ID          = 6,
+	MTD_NET         = 7
 } px4_mtd_types_t;
-#define PX4_MFT_MTD_TYPES  {MTD_PARAMETERS, MTD_WAYPOINTS, MTD_CALDATA, MTD_MFT, MTD_ID, MTD_NET}
-#define PX4_MFT_MTD_STR_TYPES  {"MTD_PARAMETERS", "MTD_WAYPOINTS", "MTD_CALDATA", "MTD_MFT", "MTD_ID", "MTD_NET"}
+#define PX4_MFT_MTD_TYPES  {MTD_PARAMETERS, MTD_WAYPOINTS, MTD_CALDATA, MTD_MFT_VER, MTD_MFT_REV, MTD_ID, MTD_NET}
+#define PX4_MFT_MTD_STR_TYPES  {"MTD_PARAMETERS", "MTD_WAYPOINTS", "MTD_CALDATA", "MTD_MFT_VER", "MTD_MFT_REV", "MTD_ID", "MTD_NET"}

 typedef struct  {
 	const px4_mtd_types_t   type;
@@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version()
 #if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
 	int hw_version_revision = board_get_hw_version();
 #else
-	int hw_version_revision = (board_get_hw_version() << 16) | board_get_hw_revision();
+	int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision());
 #endif


@@ -32,8 +32,10 @@
 ****************************************************************************/
 #pragma once

-#define HW_VERSION_EEPROM 		0x7		//!< Get hw_info from EEPROM
-#define HW_EEPROM_VERSION_MIN	0x10	//!< Minimum supported version
+#include <errno.h>
+
+#define HW_ID_EEPROM 			0x7		//!< Get hw_info from EEPROM
+#define HW_EEPROM_ID_MIN		0x10	//!< Minimum supported id (version/revision)

 #pragma pack(push, 1)

@@ -43,13 +45,13 @@ typedef struct {

 typedef struct {
 	mtd_mft_t version;
-	uint16_t hw_extended_ver;
+	uint16_t hw_extended_id; //<! HW version for MTD_MFT_VER, HW revision for MTD_MFT_REV
 	uint16_t crc;
 } mtd_mft_v0_t;

 typedef struct {
-	mtd_mft_t  version;
-	uint16_t hw_extended_ver;
+	mtd_mft_t version;
+	uint16_t hw_extended_id;
 	//{device tree overlay}
 	uint16_t crc;
 } mtd_mft_v1_t;
@@ -45,8 +45,8 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			1
-#define MAX_TIMER_IO_CHANNELS		1
+#define MAX_IO_TIMERS			2
+#define MAX_TIMER_IO_CHANNELS		2

 #define MAX_LED_TIMERS			1
 #define MAX_TIMER_LED_CHANNELS		3
@@ -445,22 +445,36 @@ __EXPORT int board_get_hw_revision()

 int board_determine_hw_info()
 {
-	// MFT supported?
-	const char *path;
-	int rvmft = px4_mtd_query("MTD_MFT", NULL, &path);

 	// Read ADC jumpering hw_info
 	int rv = determine_hw_info(&hw_revision, &hw_version);

 	if (rv == OK) {

-		if (rvmft == OK && path != NULL && hw_version == HW_VERSION_EEPROM) {
+		// MFT supported?
+		const char *path;
+		int rvmft = px4_mtd_query("MTD_MFT_VER", NULL, &path);
+
+		if (rvmft == OK && path != NULL && hw_version == HW_ID_EEPROM) {

 			mtd_mft_v0_t mtd_mft = {MTD_MFT_v0};
 			rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);

 			if (rv == OK) {
-				hw_version = mtd_mft.hw_extended_ver;
+				hw_version = mtd_mft.hw_extended_id;
+			}
+		}
+
+		path = NULL;
+		rvmft = px4_mtd_query("MTD_MFT_REV", NULL, &path);
+
+		if (rvmft == OK && path != NULL && hw_revision == HW_ID_EEPROM) {
+
+			mtd_mft_v0_t mtd_mft = {MTD_MFT_v0};
+			rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
+
+			if (rv == OK) {
+				hw_revision = mtd_mft.hw_extended_id;
 			}
 		}
 	}
@@ -495,14 +509,14 @@ int board_set_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft_unk)

 	// Later this will be a demux on type
 	if (mtd_mft_unk->id != MTD_MFT_v0) {
-		printf("Verson is: %d, Only mft version %d is supported\n", mtd_mft_unk->id, MTD_MFT_v0);
+		printf("Version is: %d, Only mft version %d is supported\n", mtd_mft_unk->id, MTD_MFT_v0);
 		return -EINVAL;
 	}

 	mtd_mft_v0_t *mtd_mft = (mtd_mft_v0_t *)mtd_mft_unk;

-	if (mtd_mft->hw_extended_ver < HW_EEPROM_VERSION_MIN) {
-		printf("hardware version for EEPROM must be greater than %x\n", HW_EEPROM_VERSION_MIN);
+	if (mtd_mft->hw_extended_id < HW_EEPROM_ID_MIN) {
+		printf("hardware version for EEPROM must be greater than %x\n", HW_EEPROM_ID_MIN);
 		return -EINVAL;
 	}

@@ -31,11 +31,13 @@
 #
 ############################################################################

-set(module_libraries modules__muorb__slpi)
+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
 	)
@@ -126,6 +126,7 @@ function(px4_os_add_flags)

 		-Wno-unknown-warning-option
 		-Wno-cast-align
+		--include=${PX4_SOURCE_DIR}/platforms/qurt/include/qurt_reqs.h
 	)

 	# Clear -rdynamic flag which fails for hexagon
@@ -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.
+ *
+ ****************************************************************************/
+
+// Placeholder
+
@@ -35,6 +35,7 @@
 #include <stdarg.h>
 #include <stdio.h>
 #include <stdint.h>
+#include <px4_platform_common/defines.h>

 __BEGIN_DECLS

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+// This file is meant to tackle the dependencies found in PX4
+// that have not been implemented in the Hexagon SDK yet.
+
+#pragma once
+
+#include <pthread.h>
+#include <visibility.h>
+
+__BEGIN_DECLS
+
+typedef unsigned long useconds_t;
+int usleep(useconds_t usec);
+int pthread_setname_np(pthread_t __target_thread, const char *__name);
+
+__END_DECLS
@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+// This file is meant to tackle the dependencies on IOCTL found in PX4.
+// As QURT does not have IOCTL natively, this file exists to define those
+// functions/params found throughout the code base.
+
+#pragma once
+
+#define	IOCPARM_MASK	0x1fff		/* parameter length, at most 13 bits */
+#define	IOC_VOID	0x20000000	/* no parameters */
+
+#define	_IOC(inout,group,num,len)	((unsigned long) \
+		((inout) | (((len) & IOCPARM_MASK) << 16) | ((group) << 8) | (num)))
+#define	_IO(g,n)	_IOC(IOC_VOID,	(g), (n), 0)
@@ -32,3 +32,14 @@
 ############################################################################

 # Placeholder
+set(QURT_LAYER_SRCS
+	drv_hrt.cpp
+	tasks.cpp
+	px4_qurt_impl.cpp
+	)
+
+add_library(px4_layer
+        ${QURT_LAYER_SRCS}
+)
+
+target_link_libraries(px4_layer PRIVATE work_queue)
@@ -0,0 +1,326 @@
+/****************************************************************************
+ *
+ * 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 <px4_platform_common/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/workqueue.h>
+#include <drivers/drv_hrt.h>
+
+#include <semaphore.h>
+#include <time.h>
+#include <string.h>
+#include <errno.h>
+
+#include "hrt_work.h"
+
+static constexpr unsigned HRT_INTERVAL_MIN = 50;
+static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
+
+static struct sq_queue_s	callout_queue;
+
+static uint64_t			latency_baseline;
+
+static uint64_t			latency_actual;
+
+const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+static px4_sem_t 	_hrt_lock;
+static struct work_s	_hrt_work;
+
+static int32_t dsp_offset = 0;
+
+static void hrt_latency_update();
+
+static void hrt_call_reschedule();
+static void hrt_call_invoke();
+
+hrt_abstime hrt_absolute_time_offset()
+{
+	return 0;
+}
+
+static void hrt_lock()
+{
+	px4_sem_wait(&_hrt_lock);
+}
+
+static void hrt_unlock()
+{
+	px4_sem_post(&_hrt_lock);
+}
+
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+{
+	return 0;
+}
+
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	int rv = clock_gettime(clk_id, tp);
+	hrt_abstime temp_abstime = ts_to_abstime(tp);
+
+	if (dsp_offset < 0) {
+		hrt_abstime temp_offset = -dsp_offset;
+
+		if (temp_offset >= temp_abstime) { temp_abstime = 0; }
+
+		else { temp_abstime -= temp_offset; }
+
+	} else {
+		temp_abstime += (hrt_abstime) dsp_offset;
+	}
+
+	tp->tv_sec = temp_abstime / 1000000;
+	tp->tv_nsec = (temp_abstime % 1000000) * 1000;
+	return rv;
+}
+
+hrt_abstime hrt_absolute_time()
+{
+	struct timespec ts;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
+}
+
+int hrt_set_absolute_time_offset(int32_t time_diff_us)
+{
+	dsp_offset = time_diff_us;
+	return 0;
+}
+
+hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
+}
+
+void hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	*t = hrt_absolute_time();
+}
+
+bool	hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+void	hrt_cancel(struct hrt_call *entry)
+{
+	hrt_lock();
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+	entry->period = 0;
+	hrt_unlock();
+}
+
+static void hrt_latency_update()
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	latency_counters[index]++;
+}
+
+void	hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
+
+void	hrt_init()
+{
+	sq_init(&callout_queue);
+
+	int sem_ret = px4_sem_init(&_hrt_lock, 0, 1);
+
+	if (sem_ret) {
+		PX4_ERR("SEM INIT FAIL: %s", strerror(errno));
+	}
+
+	memset(&_hrt_work, 0, sizeof(_hrt_work));
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == nullptr) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == nullptr) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != nullptr);
+	}
+}
+
+static void
+hrt_tim_isr(void *p)
+{
+	latency_actual = hrt_absolute_time();
+	hrt_latency_update();
+	hrt_call_invoke();
+	hrt_lock();
+	hrt_call_reschedule();
+	hrt_unlock();
+}
+
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	hrt_abstime	delay = HRT_INTERVAL_MAX;
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	if (next != nullptr) {
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			delay = HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			delay = next->deadline - now;
+		}
+	}
+
+	latency_baseline = now + delay;
+	hrt_work_cancel(&_hrt_work);
+	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
+	hrt_lock();
+
+	if (entry->deadline != 0) {
+		sq_rem(&entry->link, &callout_queue);
+	}
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+	hrt_unlock();
+}
+
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+static void
+hrt_call_invoke()
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	hrt_lock();
+
+	while (true) {
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == nullptr) {
+			break;
+		}
+
+		if (call->deadline > now) {
+			break;
+		}
+
+		sq_rem(&call->link, &callout_queue);
+		deadline = call->deadline;
+		call->deadline = 0;
+
+		if (call->callout) {
+			hrt_unlock();
+			call->callout(call->arg);
+			hrt_lock();
+		}
+
+		if (call->period != 0) {
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
+
+			hrt_call_enter(call);
+		}
+	}
+
+	hrt_unlock();
+}
@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ * 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 <px4_platform_common/defines.h>
+
+__BEGIN_DECLS
+
+long PX4_TICKS_PER_SEC = 1000L;
+
+__END_DECLS
@@ -0,0 +1,379 @@
+/****************************************************************************
+ *
+ * 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 <px4_platform_common/log.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/workqueue.h>
+
+#include <drivers/drv_hrt.h>
+#include <pthread.h>
+#include "hrt_work.h"
+
+#define PX4_TASK_STACK_SIZE 8192
+#define PX4_TASK_MAX_NAME_LENGTH 32
+#define PX4_TASK_MAX_ARGC 32
+#define PX4_TASK_MAX_ARGV_LENGTH 32
+#define PX4_MAX_TASKS 24
+
+typedef struct task_entry {
+	pthread_t tid;
+	char name[PX4_TASK_MAX_NAME_LENGTH + 4];
+	char stack[PX4_TASK_STACK_SIZE];
+	pthread_attr_t attr;
+	px4_main_t main_entry;
+	int argc;
+	char argv_storage[PX4_TASK_MAX_ARGC][PX4_TASK_MAX_ARGV_LENGTH];
+	char *argv[PX4_TASK_MAX_ARGC];
+	bool isused;
+
+	task_entry() : isused(false) {}
+} task_entry_t;
+
+static task_entry_t taskmap[PX4_MAX_TASKS];
+
+static bool task_mutex_initialized = false;
+static pthread_mutex_t task_mutex;
+
+static void *entry_adapter(void *ptr)
+{
+	task_entry_t *data;
+	data = (task_entry_t *) ptr;
+
+	if (data->main_entry) { data->main_entry(data->argc, data->argv); }
+
+	else { PX4_ERR("No valid task entry points"); }
+
+	pthread_exit(nullptr);
+	return nullptr;
+
+}
+
+static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_main_t main_entry, char *const argv[])
+{
+	int retcode = 0;
+	int i = 0;
+	int task_index = 0;
+	char *p = (char *)argv;
+
+	PX4_INFO("Creating pthread %s\n", name);
+
+	if (task_mutex_initialized == false) {
+		task_mutex_initialized = true;
+		pthread_mutex_init(&task_mutex, nullptr);
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (task_index = 0; task_index < PX4_MAX_TASKS; task_index++) {
+		if (taskmap[task_index].isused == false) { break; }
+	}
+
+	if (task_index == PX4_MAX_TASKS) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Hit maximum number of threads");
+		return -1;
+	}
+
+	taskmap[task_index].argc = 0;
+
+	while (p) {
+		taskmap[task_index].argc++;
+		p = argv[taskmap[task_index].argc];
+	}
+
+	if (taskmap[task_index].argc >= PX4_TASK_MAX_ARGC) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Too many arguments for thread %d", taskmap[task_index].argc);
+		return -1;
+	}
+
+	for (i = 0; i < PX4_TASK_MAX_ARGC; i++) {
+		if (i < taskmap[task_index].argc) {
+			int argument_length = strlen(argv[i]);
+
+			if (argument_length >= PX4_TASK_MAX_ARGV_LENGTH) {
+				pthread_mutex_unlock(&task_mutex);
+				PX4_ERR("Argument %d is too long %d", i, argument_length);
+				return -1;
+
+			} else {
+				//px4_clock_gettimemap[task_index].argv_storage[i], argv[i]);
+				taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i];
+			}
+
+		} else {
+			taskmap[task_index].argv[i] = nullptr;
+			break;
+		}
+	}
+
+	taskmap[task_index].main_entry = main_entry;
+
+	if ((priority > 255) || (priority < 0)) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Invalid priority %d", priority);
+		return -1;
+	}
+
+	priority = 255 - priority;
+
+	if (priority < 5) { priority = 5; }
+
+	if (priority > 250) { priority = 250; }
+
+	if (strlen(name) >= PX4_TASK_MAX_NAME_LENGTH) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Task name is too long %s", name);
+		return -1;
+	}
+
+	strcpy(taskmap[task_index].name, "PX4_");
+	strcpy(&taskmap[task_index].name[4], name);
+
+	struct sched_param param;
+	param.sched_priority = priority;
+
+	pthread_attr_init(&taskmap[task_index].attr);
+	//pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name);
+	//pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack);
+	pthread_attr_setstacksize(&taskmap[task_index].attr, PX4_TASK_STACK_SIZE);
+	pthread_attr_setschedparam(&taskmap[task_index].attr, &param);
+
+	retcode = pthread_create(&taskmap[task_index].tid, &taskmap[task_index].attr, entry_adapter,
+				 (void *) &taskmap[task_index]);
+
+	if (retcode != PX4_OK) {
+		pthread_mutex_unlock(&task_mutex);
+		PX4_ERR("Couldn't create pthread %s", name);
+		return -1;
+
+	} else {
+		PX4_INFO("Successfully created px4 task %s with tid %u",
+			 taskmap[task_index].name,
+			 (unsigned int) taskmap[task_index].tid);
+	}
+
+	taskmap[task_index].isused = true;
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return i;
+}
+
+px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
+			      char *const argv[])
+{
+	if (entry == nullptr) {
+		PX4_ERR("Entry function pointer is null");
+		return -1;
+	}
+
+	return px4_task_spawn_internal(name, priority, entry, argv);
+}
+
+int px4_task_delete(px4_task_t id)
+{
+	int rv = 0;
+
+	PX4_ERR("Ignoring px4_task_delete for task %d", id);
+
+	pthread_t pid;
+	PX4_WARN("Called px4_task_delete");
+
+	if (id < PX4_MAX_TASKS && taskmap[id].isused) {
+		pid = taskmap[id].tid;
+
+	} else {
+		return -EINVAL;
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	if (pthread_self() == pid) {
+		pthread_join(pid, nullptr);
+		taskmap[id].isused = false;
+		pthread_mutex_unlock(&task_mutex);
+		pthread_exit(nullptr);
+
+	} else {
+		rv = pthread_cancel(pid);
+	}
+
+	taskmap[id].isused = false;
+
+	return rv;
+}
+
+void px4_task_exit(int ret)
+{
+	PX4_ERR("Ignoring px4_task_exit with return value %d", ret);
+
+	int i;
+	pthread_t pid = pthread_self();
+
+	for (i = 0; i < PX4_MAX_TASKS; ++i) {
+		if (taskmap[i].tid == pid) {
+			pthread_mutex_lock(&task_mutex);
+			taskmap[i].isused = false;
+			break;
+		}
+	}
+
+	if (i >= PX4_MAX_TASKS)  {
+		PX4_ERR("px4_task_exit: self task not found!");
+
+	} else {
+		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name);
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
+int px4_task_kill(px4_task_t id, int sig)
+{
+	int rv = 0;
+	pthread_t pid;
+	PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name);
+
+	if (id < PX4_MAX_TASKS && taskmap[id].tid != 0) {
+		pthread_mutex_lock(&task_mutex);
+		pid = taskmap[id].tid;
+		pthread_mutex_unlock(&task_mutex);
+
+	} else {
+		return -EINVAL;
+	}
+
+	rv = pthread_kill(pid, sig);
+
+	return rv;
+}
+
+void px4_show_tasks()
+{
+	int idx = 0;
+	int count = 0;
+
+	PX4_INFO("Active Tasks:");
+
+	for (; idx < PX4_MAX_TASKS; idx++) {
+		if (taskmap[idx].isused) {
+			PX4_INFO("   %-10s %u", taskmap[idx].name,
+				 (unsigned int) taskmap[idx].tid);
+			count++;
+		}
+	}
+
+	if (count == 0) {
+		PX4_INFO("No running tasks");
+	}
+}
+
+px4_task_t px4_getpid()
+{
+	pthread_t pid = pthread_self();
+	px4_task_t ret = -1;
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			ret = i;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+	return ret;
+
+}
+
+
+const char *px4_get_taskname()
+{
+	pthread_t pid = pthread_self();
+	const char *prog_name = "UnknownApp";
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			prog_name = taskmap[i].name;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return prog_name;
+
+}
+
+static void timer_cb(void *data)
+{
+	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
+
+	sem_post(sem);
+}
+
+int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
+{
+	work_s _hpwork = {};
+
+	struct timespec ts_now;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
+
+	hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
+
+	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
+	sem_wait(sem);
+	hrt_work_cancel(&_hpwork);
+	return 0;
+}
+
+int px4_prctl(int option, const char *arg2, pthread_t pid)
+{
+	int rv = -1;
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].tid == pid) {
+			rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2);
+			return rv;
+		}
+	}
+
+	return rv;
+}
@@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstdlib>

 namespace InvenSense_ICM20649
 {
@@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstdlib>

 namespace InvenSense_ICM42670P
 {
@@ -80,8 +80,8 @@
 #define CFG_REG_C_BDU                   (1 << 4) /* avoids reading of incorrect data due to async reads */

 /* interface factories */
-extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);

 #define LIS2MDLL_ADDRESS        0x1e

@@ -56,7 +56,7 @@
 class LIS2MDL_I2C : public device::I2C
 {
 public:
-	LIS2MDL_I2C(int bus, int bus_frequency);
+	LIS2MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_I2C() = default;

 	virtual int     read(unsigned address, void *data, unsigned count);
@@ -68,16 +68,16 @@ protected:
 };

 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency);
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency)
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_I2C(bus, bus_frequency);
+	return new LIS2MDL_I2C(config);
 }

-LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency)
+LIS2MDL_I2C::LIS2MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }

@@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS2MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;

 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS2MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS2MDL_I2C_interface(config);

 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS2MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS2MDL_SPI_interface(config);
 	}

 	if (interface == nullptr) {
@@ -94,6 +94,7 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS2MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS2MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;

@@ -112,8 +113,6 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 		return -1;
 	}

-	cli.i2c_address = LIS2MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL);

 	if (!strcmp(verb, "start")) {
@@ -61,7 +61,7 @@
 class LIS2MDL_SPI : public device::SPI
 {
 public:
-	LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS2MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_SPI() = default;

 	virtual int     init();
@@ -70,16 +70,16 @@ public:
 };

 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS2MDL_SPI(config);
 }

-LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency)
+LIS2MDL_SPI::LIS2MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }

@@ -84,8 +84,8 @@
 #define CNTL_REG5_DEFAULT 0x00

 /* interface factories */
-extern device::Device *LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS3MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);

 enum OPERATING_MODE {
 	CONTINUOUS = 0,
@@ -56,7 +56,7 @@
 class LIS3MDL_I2C : public device::I2C
 {
 public:
-	LIS3MDL_I2C(int bus, int bus_frequency);
+	LIS3MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_I2C() = default;

 	virtual int     read(unsigned address, void *data, unsigned count);
@@ -68,16 +68,16 @@ protected:
 };

 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency);
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency)
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_I2C(bus, bus_frequency);
+	return new LIS3MDL_I2C(config);
 }

-LIS3MDL_I2C::LIS3MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, LIS3MDLL_ADDRESS, bus_frequency)
+LIS3MDL_I2C::LIS3MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }

@@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS3MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;

 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS3MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS3MDL_I2C_interface(config);

 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS3MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS3MDL_SPI_interface(config);
 	}

 	if (interface == nullptr) {
@@ -90,6 +90,7 @@ void LIS3MDL::print_usage()
 	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1e);
 	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
 	PRINT_MODULE_USAGE_COMMAND("reset");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
@@ -100,6 +101,7 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS3MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS3MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;

@@ -118,8 +120,6 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 		return -1;
 	}

-	cli.i2c_address = LIS3MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS3MDL);

 	if (!strcmp(verb, "start")) {
@@ -61,7 +61,7 @@
 class LIS3MDL_SPI : public device::SPI
 {
 public:
-	LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS3MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_SPI() = default;

 	virtual int     init();
@@ -70,16 +70,16 @@ public:
 };

 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS3MDL_SPI(config);
 }

-LIS3MDL_SPI::LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, devid, spi_mode, bus_frequency)
+LIS3MDL_SPI::LIS3MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }

@@ -121,7 +121,7 @@ MspOsd::MspOsd(const char *device) :
 	strcpy(_device, device);

 	// _is_initialized = true;
-	PX4_INFO("MSP OSD prepared to run on %s", _device);
+	PX4_INFO("MSP OSD running on %s", _device);
 }

 MspOsd::~MspOsd()
@@ -504,10 +504,10 @@ int MspOsd::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-Msp OSD!
+MSP telemetry streamer

 ### Implementation
-Does the things for the DJI Air Unit OSD
+Converts uORB messages to MSP telemetry packets

 ### Examples
 CLI usage example:
@@ -87,6 +87,12 @@ int UavcanServers::init()
 		return ret;
 	}

+	/*
+	Check for firmware in the root directory, move it to appropriate location on
+	the SD card, as defined by the APDesc.
+	*/
+	migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ROOT_PATH);
+
 	/* Start fw file server back */
 	ret = _fw_server.start(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);

@@ -131,12 +137,6 @@ int UavcanServers::init()
 		return ret;
 	}

-	/*
-	Check for firmware in the root directory, move it to appropriate location on
-	the SD card, as defined by the APDesc.
-	*/
-	migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ROOT_PATH);
-
 	/*  Start the Node   */
 	return 0;
 }
@@ -152,22 +152,12 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p
 	  /fs/microsd/ufw
 	     nnnnn.bin - where n is the board_id
 	*/
+	static constexpr size_t maxlen = UAVCAN_MAX_PATH_LENGTH;

-	const size_t maxlen = UAVCAN_MAX_PATH_LENGTH;
-	const size_t sd_root_path_len = strlen(sd_root_path);
-	struct stat sb;
-	int rv;
-	char dstpath[maxlen + 1];
-	char srcpath[maxlen + 1];
-
-	DIR *const sd_root_dir = opendir(sd_root_path);
-
-	if (!sd_root_dir) {
-		return;
-	}
+	struct stat sb {};

 	if (stat(sd_path, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
-		rv = mkdir(sd_path, S_IRWXU | S_IRWXG | S_IRWXO);
+		int rv = mkdir(sd_path, S_IRWXU | S_IRWXG | S_IRWXO);

 		if (rv != 0) {
 			PX4_ERR("dev: couldn't create '%s'", sd_path);
@@ -175,29 +165,35 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p
 		}
 	}

+	DIR *const sd_root_dir = opendir(sd_root_path);
+
+	if (!sd_root_dir) {
+		return;
+	}
+
 	// Iterate over all bin files in root directory
-	struct dirent *dev_dirent = NULL;
+	struct dirent *dev_dirent = nullptr;

 	while ((dev_dirent = readdir(sd_root_dir)) != nullptr) {

-		uavcan_posix::FirmwareVersionChecker::AppDescriptor descriptor;
-
 		// Looking for all uavcan.bin files.
-
 		if (DIRENT_ISFILE(dev_dirent->d_type) && strstr(dev_dirent->d_name, ".bin") != nullptr) {

 			// Make sure the path fits
-
-			size_t filename_len = strlen(dev_dirent->d_name);
-			size_t srcpath_len = sd_root_path_len + 1 + filename_len;
+			const size_t filename_len = strlen(dev_dirent->d_name);
+			const size_t sd_root_path_len = strlen(sd_root_path);
+			const size_t srcpath_len = sd_root_path_len + 1 + filename_len;

 			if (srcpath_len > maxlen) {
 				PX4_WARN("file: srcpath '%s%s' too long", sd_root_path, dev_dirent->d_name);
 				continue;
 			}

+			char srcpath[maxlen + 1] {};
 			snprintf(srcpath, sizeof(srcpath), "%s%s", sd_root_path, dev_dirent->d_name);

+			uavcan_posix::FirmwareVersionChecker::AppDescriptor descriptor{};
+
 			if (uavcan_posix::FirmwareVersionChecker::getFileInfo(srcpath, descriptor, 1024) != 0) {
 				continue;
 			}
@@ -206,6 +202,7 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p
 				continue;
 			}

+			char dstpath[maxlen + 1] {};
 			snprintf(dstpath, sizeof(dstpath), "%s/%d.bin", sd_path, descriptor.board_id);

 			if (copyFw(dstpath, srcpath) >= 0) {
@@ -67,10 +67,10 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get();
 	const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get();

-	const bool avoidance_data_timeout =
-		hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US &&
-		hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(bezier_msg.control_points[bezier_msg.bezier_order -
-							1].delta * 1e6f);
+	const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
+	const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(
+						bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f);
+	const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout;

 	const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
 	const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0;
@@ -86,8 +86,12 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	}

 	if (avoidance_invalid) {
-		PX4_WARN("Obstacle Avoidance system failed, loitering");
-		_publishVehicleCmdDoLoiter();
+		if (_avoidance_activated) {
+			// Invalid point received: deactivate
+			PX4_WARN("Obstacle Avoidance system failed, loitering");
+			_publishVehicleCmdDoLoiter();
+			_avoidance_activated = false;
+		}

 		if (!_failsafe_position.isAllFinite()) {
 			// save vehicle position when entering failsafe
@@ -98,13 +102,18 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 		vel_sp.setNaN();
 		yaw_sp = NAN;
 		yaw_speed_sp = NAN;
+
+		// Do nothing further - wait until activation
 		return;

-	} else {
+	} else if (!_avoidance_activated) {
+		// First setpoint has been received: activate
+		PX4_INFO("Obstacle Avoidance system activated");
 		_failsafe_position.setNaN();
+		_avoidance_activated = true;
 	}

-	if (avoidance_point_valid) {
+	if (avoidance_point_valid && !wp_msg_timeout) {
 		const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0];
 		pos_sp = Vector3f(point0.position);
 		vel_sp = Vector3f(point0.velocity);
@@ -115,8 +124,7 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 			yaw_speed_sp = point0.yaw_speed;
 		}

-	} else if (avoidance_bezier_valid) {
-
+	} else if (avoidance_bezier_valid && !bezier_msg_timeout) {
 		float yaw = NAN, yaw_speed = NAN;
 		_generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed);

@@ -130,6 +130,8 @@ protected:
 	matrix::Vector3f _position = {}; /**< current vehicle position */
 	matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */

+	bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */
+
 	systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
 	systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */

@@ -93,11 +93,11 @@ public:
 	{
 		theta() = std::asin(-dcm(2, 0));

-		if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
+		if ((std::fabs(theta() - Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(dcm(1, 2), dcm(0, 2));

-		} else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
+		} else if ((std::fabs(theta() + Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(-dcm(1, 2), -dcm(0, 2));

@@ -95,7 +95,7 @@ Integer wrap(Integer x, Integer low, Integer high)
 template<typename Type>
 Type wrap_pi(Type x)
 {
-	return wrap(x, Type(-M_PI), Type(M_PI));
+	return wrap(x, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }

 /**
@@ -104,7 +104,7 @@ Type wrap_pi(Type x)
 template<typename Type>
 Type wrap_2pi(Type x)
 {
-	return wrap(x, Type(0), Type((2 * M_PI)));
+	return wrap(x, Type(0), Type((2 * M_PI_PRECISE)));
 }

 /**
@@ -132,7 +132,7 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high
 template<typename Type>
 Type unwrap_pi(const Type last_angle, const Type new_angle)
 {
-	return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI));
+	return unwrap(last_angle, new_angle, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }

 /**
@@ -1,6 +1,7 @@
 #pragma once

 #include <assert.h>
+#include <px4_platform_common/defines.h>

 #include "helper_functions.hpp"

@@ -200,8 +200,8 @@ TEST(MatrixAttitudeTest, Attitude)
 	}

 	// constants
-	double deg2rad = M_PI / 180.0;
-	double rad2deg = 180.0 / M_PI;
+	double deg2rad = M_PI_PRECISE / 180.0;
+	double rad2deg = 180.0 / M_PI_PRECISE;

 	// euler dcm round trip check
 	for (double roll = -90; roll <= 90; roll += 90) {
@@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude)
 	EXPECT_EQ(q, q_true);

 	// from axis angle, with length of vector the rotation
-	float n = float(std::sqrt(4 * M_PI * M_PI / 3));
+	float n = float(std::sqrt(4 * M_PI_F * M_PI_F / 3));
 	q = AxisAnglef(n, n, n);
 	EXPECT_EQ(q, Quatf(-1, 0, 0, 0));
 	q = AxisAnglef(0, 0, 0);
@@ -81,20 +81,20 @@ TEST(MatrixHelperTest, Helper)

 	// wrap pi
 	EXPECT_FLOAT_EQ(wrap_pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI_PRECISE)));
 	EXPECT_FLOAT_EQ(wrap_pi(3.), 3.);
-	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI_PRECISE));
 	EXPECT_FALSE(std::isfinite(wrap_pi(NAN)));

 	// wrap 2pi
 	EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI_PRECISE));
 	EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.));
-	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE)));
 	EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));

 	// Equality checks
@@ -5,7 +5,7 @@ using namespace matrix;

 TEST(MatrixUnwrapTest, UnwrapFloats)
 {
-	const float M_TWO_PI_F = float(M_PI * 2);
+	const float M_TWO_PI_F = float(M_PI_F * 2);

 	float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
@@ -34,7 +34,7 @@ TEST(MatrixUnwrapTest, UnwrapFloats)

 TEST(MatrixUnwrapTest, UnwrapDoubles)
 {
-	const double M_TWO_PI = M_PI * 2;
+	const double M_TWO_PI = M_PI_PRECISE * 2;

 	double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
@@ -250,5 +250,14 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}

+	// 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y
+	{
+		if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) {
+			strcpy(node->name, "VT_FW_DIFTHR_S_Y");
+			PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y");
+			return true;
+		}
+	}
+
 	return false;
 }
@@ -419,35 +419,25 @@ perf_print_counter(perf_counter_t handle)
 		return;
 	}

-	perf_print_counter_fd(1, handle);
-}
-
-void
-perf_print_counter_fd(int fd, perf_counter_t handle)
-{
-	if (handle == nullptr) {
-		return;
-	}
-
 	switch (handle->type) {
 	case PC_COUNT:
-		dprintf(fd, "%s: %" PRIu64 " events\n",
-			handle->name,
-			((struct perf_ctr_count *)handle)->event_count);
+		PX4_INFO_RAW("%s: %" PRIu64 " events\n",
+			     handle->name,
+			     ((struct perf_ctr_count *)handle)->event_count);
 		break;

 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 			float rms = sqrtf(pce->M2 / (pce->event_count - 1));
-			dprintf(fd, "%s: %" PRIu64 " events, %" PRIu64 "us elapsed, %.2fus avg, min %" PRIu32 "us max %" PRIu32
-				"us %5.3fus rms\n",
-				handle->name,
-				pce->event_count,
-				pce->time_total,
-				(pce->event_count == 0) ? 0 : (double)pce->time_total / (double)pce->event_count,
-				pce->time_least,
-				pce->time_most,
-				(double)(1e6f * rms));
+			PX4_INFO_RAW("%s: %" PRIu64 " events, %" PRIu64 "us elapsed, %.2fus avg, min %" PRIu32 "us max %" PRIu32
+				     "us %5.3fus rms\n",
+				     handle->name,
+				     pce->event_count,
+				     pce->time_total,
+				     (pce->event_count == 0) ? 0 : (double)pce->time_total / (double)pce->event_count,
+				     pce->time_least,
+				     pce->time_most,
+				     (double)(1e6f * rms));
 			break;
 		}

@@ -455,13 +445,13 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 			float rms = sqrtf(pci->M2 / (pci->event_count - 1));

-			dprintf(fd, "%s: %" PRIu64 " events, %.2fus avg, min %" PRIu32 "us max %" PRIu32 "us %5.3fus rms\n",
-				handle->name,
-				pci->event_count,
-				(pci->event_count == 0) ? 0 : (double)(pci->time_last - pci->time_first) / (double)pci->event_count,
-				pci->time_least,
-				pci->time_most,
-				(double)(1e6f * rms));
+			PX4_INFO_RAW("%s: %" PRIu64 " events, %.2fus avg, min %" PRIu32 "us max %" PRIu32 "us %5.3fus rms\n",
+				     handle->name,
+				     pci->event_count,
+				     (pci->event_count == 0) ? 0 : (double)(pci->time_last - pci->time_first) / (double)pci->event_count,
+				     pci->time_least,
+				     pci->time_most,
+				     (double)(1e6f * rms));
 			break;
 		}

@@ -593,13 +583,13 @@ perf_iterate_all(perf_callback cb, void *user)
 }

 void
-perf_print_all(int fd)
+perf_print_all(void)
 {
 	pthread_mutex_lock(&perf_counters_mutex);
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);

 	while (handle != nullptr) {
-		perf_print_counter_fd(fd, handle);
+		perf_print_counter(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}

@@ -607,19 +597,19 @@ perf_print_all(int fd)
 }

 void
-perf_print_latency(int fd)
+perf_print_latency(void)
 {
 	latency_info_t latency;
-	dprintf(fd, "bucket [us] : events\n");
+	PX4_INFO_RAW("bucket [us] : events\n");

 	for (int i = 0; i < get_latency_bucket_count(); i++) {
 		latency = get_latency(i, i);
-		dprintf(fd, "       %4i : %li\n", latency.bucket, (long int)latency.counter);
+		PX4_INFO_RAW("       %4i : %li\n", latency.bucket, (long int)latency.counter);
 	}

 	// print the overflow bucket value
 	latency = get_latency(get_latency_bucket_count() - 1, get_latency_bucket_count());
-	dprintf(fd, " >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
+	PX4_INFO_RAW(" >%4" PRIu16 " : %" PRIu32 "\n", latency.bucket, latency.counter);
 }

 void
@@ -174,14 +174,6 @@ __EXPORT extern void		perf_reset(perf_counter_t handle);
 */
 __EXPORT extern void		perf_print_counter(perf_counter_t handle);

-/**
- * Print one performance counter to a fd.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
- * @param handle		The counter to print.
- */
-__EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
-
 /**
 * Print one performance counter to a buffer.
 *
@@ -194,10 +186,8 @@ __EXPORT extern int		perf_print_counter_buffer(char *buffer, int length, perf_co

 /**
 * Print all of the performance counters.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
 */
-__EXPORT extern void		perf_print_all(int fd);
+__EXPORT extern void		perf_print_all(void);


 typedef void (*perf_callback)(perf_counter_t handle, void *user);
@@ -216,10 +206,8 @@ __EXPORT extern void	perf_iterate_all(perf_callback cb, void *user);

 /**
 * Print hrt latency counters.
- *
- * @param fd			File descriptor to print to - e.g. 0 for stdout
 */
-__EXPORT extern void		perf_print_latency(int fd);
+__EXPORT extern void		perf_print_latency(void);

 /**
 * Reset all of the performance counters.
@@ -218,10 +218,10 @@ AngularVelocityController::Run()
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			Vector3f integral = _control.getIntegral();
-			rate_ctrl_status.timestamp = hrt_absolute_time();
 			rate_ctrl_status.rollspeed_integ = integral(0);
 			rate_ctrl_status.pitchspeed_integ = integral(1);
 			rate_ctrl_status.yawspeed_integ = integral(2);
+			rate_ctrl_status.timestamp = hrt_absolute_time();
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);

 			// publish controller output
@@ -238,14 +238,12 @@ void
 AngularVelocityController::publish_angular_acceleration_setpoint()
 {
 	Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
-
 	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
-	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
 	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
 	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
 	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
-
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
 }

@@ -253,14 +251,12 @@ void
 AngularVelocityController::publish_torque_setpoint()
 {
 	Vector3f torque_sp = _control.getTorqueSetpoint();
-
 	vehicle_torque_setpoint_s v_torque_sp = {};
-	v_torque_sp.timestamp = hrt_absolute_time();
 	v_torque_sp.timestamp_sample = _timestamp_sample;
 	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
 	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
 	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
+	v_torque_sp.timestamp = hrt_absolute_time();
 	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
 }

@@ -268,12 +264,11 @@ void
 AngularVelocityController::publish_thrust_setpoint()
 {
 	vehicle_thrust_setpoint_s v_thrust_sp = {};
-	v_thrust_sp.timestamp = hrt_absolute_time();
 	v_thrust_sp.timestamp_sample = _timestamp_sample;
 	v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
 	v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
 	v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
-
+	v_thrust_sp.timestamp = hrt_absolute_time();
 	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
 }

@@ -33,6 +33,8 @@

 #include "CameraFeedback.hpp"

+using namespace time_literals;
+
 CameraFeedback::CameraFeedback() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -112,11 +114,39 @@ CameraFeedback::Run()
 		}

 		// Fill attitude data
-		// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
-		capture.q[0] = att.q[0];
-		capture.q[1] = att.q[1];
-		capture.q[2] = att.q[2];
-		capture.q[3] = att.q[3];
+		gimbal_device_attitude_status_s gimbal{};
+
+		if (_gimbal_sub.copy(&gimbal) && (hrt_elapsed_time(&gimbal.timestamp) < 1_s)) {
+			if (gimbal.device_flags & gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK) {
+				// Gimbal yaw angle is absolute angle relative to North
+				capture.q[0] = gimbal.q[0];
+				capture.q[1] = gimbal.q[1];
+				capture.q[2] = gimbal.q[2];
+				capture.q[3] = gimbal.q[3];
+
+			} else {
+				// Gimbal quaternion frame is in the Earth frame rotated so that the x-axis is pointing
+				// forward (yaw relative to vehicle). Get heading from the vehicle attitude and combine it
+				// with the gimbal orientation.
+				const matrix::Eulerf euler_vehicle(matrix::Quatf(att.q));
+				const matrix::Quatf q_heading(matrix::Eulerf(0.0f, 0.0f, euler_vehicle(2)));
+				matrix::Quatf q_gimbal(gimbal.q);
+				q_gimbal = q_heading * q_gimbal;
+
+				capture.q[0] = q_gimbal(0);
+				capture.q[1] = q_gimbal(1);
+				capture.q[2] = q_gimbal(2);
+				capture.q[3] = q_gimbal(3);
+			}
+
+		} else {
+			// No gimbal orientation, use vehicle attitude
+			capture.q[0] = att.q[0];
+			capture.q[1] = att.q[1];
+			capture.q[2] = att.q[2];
+			capture.q[3] = att.q[3];
+		}
+
 		capture.result = 1;

 		_capture_pub.publish(capture);
@@ -55,6 +55,7 @@
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>

 class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
 {
@@ -81,6 +82,7 @@ private:

 	uORB::Subscription	_gpos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription	_att_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription	_gimbal_sub{ORB_ID(gimbal_device_attitude_status)};

 	uORB::Publication<camera_capture_s>	_capture_pub{ORB_ID(camera_capture)};

@@ -1575,7 +1575,7 @@ void Commander::updateParameters()
 			       && _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 	const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status)
 			      && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-	const bool is_ground = is_ground_rover(_vehicle_status);
+	const bool is_ground = is_ground_vehicle(_vehicle_status);

 	/* disable manual override for all systems that rely on electronic stabilization */
 	if (is_rotary) {
@@ -1862,17 +1862,22 @@ void Commander::checkForMissionUpdate()
 			}
 		}

-		// Transition mode to loiter or auto-mission after takeoff is completed.
 		if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
-		    && (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
-			_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 		    && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
 		    && mission_result.finished) {

-			if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
-				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
+			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
+			    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
+				// Transition mode to loiter or auto-mission after takeoff is completed.
+				if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
+					_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);

-			} else {
+				} else {
+					_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+				}
+
+			} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+				// Transition to loiter when the mission is cleared and/or finished, and we are still in mission mode.
 				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
 			}
 		}
@@ -35,8 +35,7 @@

 HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status)
 	: ModuleParams(parent),
-	  _context(status),
-	  _reporter(_failsafe_flags)
+	  _context(status)
 {
 	// Initialize mode requirements to invalid
 	_failsafe_flags.angular_velocity_invalid = true;
@@ -103,12 +103,12 @@ public:
 protected:
 	void updateParams() override;
 private:
-	Context _context;
-	Report _reporter;
-	orb_advert_t _mavlink_log_pub{nullptr};
-
 	failsafe_flags_s _failsafe_flags{};

+	Context _context;
+	Report _reporter{_failsafe_flags};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
 	uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
 	uORB::Publication<failsafe_flags_s> _failsafe_flags_pub{ORB_ID(failsafe_flags)};

@@ -35,11 +35,6 @@

 using namespace time_literals;

-OffboardChecks::OffboardChecks()
-{
-	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
-}
-
 void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 {
 	reporter.failsafeFlags().offboard_control_signal_lost = true;
@@ -47,9 +42,10 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 	offboard_control_mode_s offboard_control_mode;

 	if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
-		bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
-					  || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
-					  || offboard_control_mode.actuator;
+		bool data_is_recent = hrt_absolute_time() < offboard_control_mode.timestamp + _param_com_of_loss_t.get() * 1_s;
+		bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity
+					   || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
+					   || offboard_control_mode.actuator) && data_is_recent;

 		if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
 			offboard_available = false;
@@ -62,9 +58,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 			offboard_available = false;
 		}

-		_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
-
 		// This is a mode requirement, no need to report
-		reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
+		reporter.failsafeFlags().offboard_control_signal_lost = !offboard_available;
 	}
 }
@@ -37,19 +37,17 @@

 #include <uORB/Subscription.hpp>
 #include <uORB/topics/offboard_control_mode.h>
-#include <lib/hysteresis/hysteresis.h>

 class OffboardChecks : public HealthAndArmingCheckBase
 {
 public:
-	OffboardChecks();
+	OffboardChecks() = default;
 	~OffboardChecks() = default;

 	void checkAndReport(const Context &context, Report &reporter) override;

 private:
 	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	systemlib::Hysteresis _offboard_available{false};

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t
@@ -119,7 +119,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
 					    events::ID("check_rc_dl_no_dllink"),
 					    log_level, "No connection to the ground control station");

-		if (reporter.mavlink_log_pub()) {
+		if (gcs_connection_required && reporter.mavlink_log_pub()) {
 			mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t");
 		}

@@ -32,6 +32,8 @@
 ****************************************************************************/

 #include "sdcardCheck.hpp"
+#include <dirent.h>
+#include <string.h>

 #ifdef __PX4_DARWIN
 #include <sys/param.h>
@@ -42,38 +44,85 @@

 void SdCardChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_com_arm_sdcard.get() <= 0) {
-		return;
-	}
+	if (_param_com_arm_sdcard.get() > 0) {

-	struct statfs statfs_buf;
+		struct statfs statfs_buf;

-	if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
-		// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
-		_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
-	}
-
-	if (!_sdcard_detected) {
-		NavModes affected_modes{NavModes::None};
-
-		if (_param_com_arm_sdcard.get() == 2) {
-			// disallow arming without sd card
-			affected_modes = NavModes::All;
+		if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
+			// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
+			_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
 		}

+		if (!_sdcard_detected) {
+			NavModes affected_modes{NavModes::None};
+
+			if (_param_com_arm_sdcard.get() == 2) {
+				// disallow arming without sd card
+				affected_modes = NavModes::All;
+			}
+
+			/* EVENT
+			 * @description
+			 * Insert an SD Card to the autopilot and reboot the system.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(affected_modes, health_component_t::system,
+						    events::ID("check_missing_fmu_sdcard"),
+						    events::Log::Error, "Missing FMU SD Card");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			}
+		}
+	}
+
+#ifdef __PX4_NUTTX
+	// Check for hardfault files
+
+	if (!_hardfault_checked_once && _param_com_arm_hardfault_check.get()) {
+		_hardfault_checked_once = true;
+
+		DIR *dp = opendir(PX4_STORAGEDIR);
+
+		if (dp != nullptr) {
+
+			struct dirent *result;
+
+			while ((result = readdir(dp)) && !_hardfault_file_present) {
+
+				// Check for pattern fault_*.log
+				if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) {
+					_hardfault_file_present = true;
+				}
+			}
+
+			closedir(dp);
+		}
+	}
+
+	if (_hardfault_file_present && _param_com_arm_hardfault_check.get()) {
 		/* EVENT
 		 * @description
-		 * Insert an SD Card to the autopilot and reboot the system.
+		 * The SD card contains crash dump files, service the vehicle before continuing to fly.
 		 *
 		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+		 * Check how to debug hardfaults on <a>https://docs.px4.io/main/en/debug/gdb_debugging.html#hard-fault-debugging</a>.
+		 * When completed, remove the files 'fault_*.log' on the SD card.
+		 *
+		 * This check can be configured via <param>COM_ARM_HFLT_CHK</param> parameter.
 		 * </profile>
 		 */
-		reporter.armingCheckFailure(affected_modes, health_component_t::system, events::ID("check_missing_fmu_sdcard"),
-					    events::Log::Error, "Missing FMU SD Card");
+		reporter.healthFailure(NavModes::All, health_component_t::system,
+				       events::ID("check_hardfault_present"),
+				       events::Log::Error, "Crash dumps present on SD card");

 		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service");
 		}
 	}
+
+#endif /* __PX4_NUTTX */
 }
@@ -46,7 +46,13 @@ public:
 private:
 	bool _sdcard_detected{false};

+#ifdef __PX4_NUTTX
+	bool _hardfault_checked_once {false};
+	bool _hardfault_file_present{false};
+#endif
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard
+					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard,
+					(ParamBool<px4::params::COM_ARM_HFLT_CHK>) _param_com_arm_hardfault_check
 				       )
 };
@@ -118,19 +118,9 @@ bool is_fixed_wing(const vehicle_status_s &current_status)
 	return current_status.system_type == VEHICLE_TYPE_FIXED_WING;
 }

-bool is_ground_rover(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
-}
-
-bool is_boat(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_BOAT;
-}
-
 bool is_ground_vehicle(const vehicle_status_s &current_status)
 {
-	return is_ground_rover(current_status) || is_boat(current_status);
+	return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
 }

 // End time for currently blinking LED message, 0 if no blink message
@@ -55,8 +55,6 @@ bool is_rotary_wing(const vehicle_status_s &current_status);
 bool is_vtol(const vehicle_status_s &current_status);
 bool is_vtol_tailsitter(const vehicle_status_s &current_status);
 bool is_fixed_wing(const vehicle_status_s &current_status);
-bool is_ground_rover(const vehicle_status_s &current_status);
-bool is_boat(const vehicle_status_s &current_status);
 bool is_ground_vehicle(const vehicle_status_s &current_status);

 int buzzer_init(void);
@@ -1010,6 +1010,17 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
 */
 PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);

+/**
+ * Enable FMU SD card hardfault detection check
+ *
+ * This check detects if there are hardfault files present on the
+ * SD card. If so, and the parameter is enabled, arming is prevented.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
+
 /**
 * Enforced delay between arming and further navigation
 *
@@ -467,6 +467,11 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_
 	// offboard signal
 	if (status_flags.offboard_control_signal_lost && (status_flags.mode_req_offboard_signal & (1u << user_intended_mode))) {
 		action = fromOffboardLossActParam(_param_com_obl_rc_act.get(), user_intended_mode);
+
+		// for this specific case, user_intended_mode is not modified, we shouldn't check additional fallbacks
+		if (action == Action::Disarm) {
+			return action;
+		}
 	}

 	// posctrl
@@ -199,10 +199,10 @@ public:
 	virtual uint32_t getStoppedMotors() const { return 0; }

 	/**
-	 * Fill in the allocated and unallocated torque and thrust.
-	 * Should return false if not filled in and the effectivenes matrix should be used instead
+	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
+	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
 	 */
-	virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; }
+	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}

 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
@@ -217,48 +217,36 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit
 	}
 }

-bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const
+void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
 {
-	status.torque_setpoint_achieved = true;
-	status.thrust_setpoint_achieved = true;

 	// Note: the values '-1', '1' and '0' are just to indicate a negative,
 	// positive or no saturation to the rate controller. The actual magnitude is not used.
 	if (_saturation_flags.roll_pos) {
 		status.unallocated_torque[0] = 1.f;
-		status.torque_setpoint_achieved = false;

 	} else if (_saturation_flags.roll_neg) {
 		status.unallocated_torque[0] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}

 	if (_saturation_flags.pitch_pos) {
 		status.unallocated_torque[1] = 1.f;
-		status.torque_setpoint_achieved = false;

 	} else if (_saturation_flags.pitch_neg) {
 		status.unallocated_torque[1] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}

 	if (_saturation_flags.yaw_pos) {
 		status.unallocated_torque[2] = 1.f;
-		status.torque_setpoint_achieved = false;

 	} else if (_saturation_flags.yaw_neg) {
 		status.unallocated_torque[2] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}

 	if (_saturation_flags.thrust_pos) {
 		status.unallocated_thrust[2] = 1.f;
-		status.thrust_setpoint_achieved = false;

 	} else if (_saturation_flags.thrust_neg) {
 		status.unallocated_thrust[2] = -1.f;
-		status.thrust_setpoint_achieved = false;
 	}
-
-	return true;
 }
@@ -79,7 +79,7 @@ public:
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;

-	bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override;
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();
@@ -233,10 +233,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
 	return num_actuators;
 }

-uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control)
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
+		float collective_tilt_control)
 {
-	if (!PX4_ISFINITE(tilt_control)) {
-		tilt_control = -1.f;
+	if (!PX4_ISFINITE(collective_tilt_control)) {
+		collective_tilt_control = -1.f;
 	}

 	uint32_t nontilted_motors = 0;
@@ -250,8 +251,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv
 		}

 		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
-		float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f);
-		float tilt_direction = math::radians((float)tilt.tilt_direction);
+		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
+		const float tilt_direction = math::radians((float)tilt.tilt_direction);
 		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
 	}

@@ -54,7 +54,7 @@ bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
 {
-	if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+	if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}

@@ -66,9 +66,9 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// Update matrix with tilts in vertical position when update is triggered by a manual
 	// configuration (parameter) change. This is to make sure the normalization
 	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
-	const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f :
-					   _last_tilt_control;
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
+	const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ?
+			-1.f : _last_collective_tilt_control;
+	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
 			    << configuration.num_actuators[(int)ActuatorType::MOTORS];

 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
@@ -86,7 +86,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config

 	// If it was an update coming from a config change, then make sure to update matrix in
 	// the next iteration again with the correct tilt (but without updating the normalization scale).
-	_combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+	_collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);

 	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }
@@ -113,24 +113,24 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		actuator_controls_s actuator_controls_1;

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

-			// set control_tilt to exactly -1 or 1 if close to these end points
-			control_tilt = control_tilt < -0.99f ? -1.f : control_tilt;
-			control_tilt = control_tilt > 0.99f ? 1.f : control_tilt;
+			// set control_collective_tilt to exactly -1 or 1 if close to these end points
+			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;

-			// initialize _last_tilt_control
-			if (!PX4_ISFINITE(_last_tilt_control)) {
-				_last_tilt_control = control_tilt;
+			// initialize _last_collective_tilt_control
+			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+				_last_collective_tilt_control = control_collective_tilt;

-			} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
-				_combined_tilt_updated = true;
-				_last_tilt_control = control_tilt;
+			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+				_collective_tilt_updated = true;
+				_last_collective_tilt_control = control_collective_tilt;
 			}

 			for (int i = 0; i < _tilts.count(); ++i) {
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
-					actuator_sp(i + _first_tilt_idx) += control_tilt;
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
 			}
 		}
@@ -147,6 +147,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			}
 		}
 	}
+
+	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
+	// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
+	// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
+	if (matrix_index == 0 && _tilts.hasYawControl()) {
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
+
+		if (control_sp(2) < -1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
+
+		} else if (control_sp(2) > 1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
+		}
+	}
 }

 void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
@@ -170,3 +185,23 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 		break;
 	}
 }
+
+void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+	// only handle matrix 0 (motors and tilts)
+	if (matrix_index == 1) {
+		return;
+	}
+
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
+	}
+}
@@ -81,8 +81,11 @@ public:
 	const char *name() const override { return "VTOL Tiltrotor"; }

 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
+
 protected:
-	bool _combined_tilt_updated{true};
+	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;
@@ -93,8 +96,15 @@ protected:
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 	int _first_tilt_idx{0}; ///< applies to matrix 0

-	float _last_tilt_control{NAN};
+	float _last_collective_tilt_control{NAN};

 	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+
+	struct YawTiltSaturationFlags {
+		bool tilt_yaw_pos;
+		bool tilt_yaw_neg;
+	};
+
+	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
 };
@@ -589,33 +589,29 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)

 	// TODO: disabled motors (?)

-	if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) {
-		// Allocated control
-		const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
-		control_allocator_status.allocated_torque[0] = allocated_control(0);
-		control_allocator_status.allocated_torque[1] = allocated_control(1);
-		control_allocator_status.allocated_torque[2] = allocated_control(2);
-		control_allocator_status.allocated_thrust[0] = allocated_control(3);
-		control_allocator_status.allocated_thrust[1] = allocated_control(4);
-		control_allocator_status.allocated_thrust[2] = allocated_control(5);
+	// Allocated control
+	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();

-		// Unallocated control
-		const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
-				allocated_control;
-		control_allocator_status.unallocated_torque[0] = unallocated_control(0);
-		control_allocator_status.unallocated_torque[1] = unallocated_control(1);
-		control_allocator_status.unallocated_torque[2] = unallocated_control(2);
-		control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
-		control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
-		control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
+	// Unallocated control
+	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
+			allocated_control;
+	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
+	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
+	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
+	control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
+	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
+	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);

-		// Allocation success flags
-		control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
-				unallocated_control(2)).norm_squared() < 1e-6f);
-		control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
-				unallocated_control(5)).norm_squared() < 1e-6f);
-	}
+	// override control_allocator_status in customized saturation logic for certain effectiveness types
+	_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);

+	// Allocation success flags
+	control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
+			control_allocator_status.unallocated_torque[1],
+			control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
+	control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
+			control_allocator_status.unallocated_thrust[1],
+			control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);

 	// Actuator saturation
 	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();
@@ -92,6 +92,7 @@ px4_add_module(
 		EKF/EKFGSF_yaw.cpp
 		EKF/estimator_interface.cpp
 		EKF/ev_height_control.cpp
+		EKF/ev_vel_control.cpp
 		EKF/fake_height_control.cpp
 		EKF/fake_pos_control.cpp
 		EKF/gnss_height_control.cpp
@@ -43,6 +43,7 @@ add_library(ecl_EKF
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
 	ev_height_control.cpp
+	ev_vel_control.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp
@@ -79,8 +79,9 @@ using math::Utilities::updateYawInRotMat;
 #define 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_FRD = 0,
-	BODY_FRAME_FRD  = 1
+	LOCAL_FRAME_NED = 0,
+	LOCAL_FRAME_FRD = 1,
+	BODY_FRAME_FRD  = 2
 };

 enum GeoDeclinationMask : uint8_t {
@@ -126,6 +127,13 @@ enum RngCtrl : uint8_t {
 	ENABLED     = 2
 };

+enum class EvCtrl : uint8_t {
+	HPOS = (1<<0),
+	VPOS = (1<<1),
+	VEL  = (1<<2),
+	YAW  = (1<<3)
+};
+
 enum SensorFusionMask : uint16_t {
 	// Bit locations for fusion_mode
 	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
@@ -136,7 +144,7 @@ enum SensorFusionMask : uint16_t {
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
 	ROTATE_EXT_VIS   = (1<<6),      ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
 	DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
-	USE_EXT_VIS_VEL  = (1<<8),      ///< set to true to use external vision velocity data
+	DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
 };

 struct gpsMessage {
@@ -227,10 +235,11 @@ struct extVisionSample {
 	Vector3f    vel{};         ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
 	Quatf       quat{};        ///< quaternion defining rotation from body to earth frame
 	Vector3f    posVar{};      ///< XYZ position variances (m**2)
-	Vector3f    velVar{};      ///< XYZ velocity variances ((m/sec)**2)
-	float       angVar{};      ///< angular heading variance (rad**2)
+	Vector3f    velocity_var{};    ///< XYZ velocity variances ((m/sec)**2)
+	Vector3f    orientation_var{}; ///< orientation variance (rad**2)
 	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
+	int8_t     quality{};     ///< quality indicator between 0 and 100
 };

 struct dragSample {
@@ -265,6 +274,7 @@ struct parameters {
 	int32_t baro_ctrl{1};
 	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
 	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
+	int32_t ev_ctrl{0};
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator

@@ -336,6 +346,7 @@ struct parameters {
 	float arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion

 	// synthetic sideslip fusion
+	int32_t beta_fusion_enabled{0};
 	float beta_innov_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
 	float beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
@@ -356,6 +367,9 @@ struct parameters {
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check

 	// vision position fusion
+	float ev_vel_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
+	float ev_att_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
+	int32_t ev_quality_minimum{0};          ///< vision minimum acceptable quality integer
 	float ev_vel_innov_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
--- a/Show More
+++ b/Show More