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

Increase allowed rtk injections to 8 for moving base. Update GPS submodule.
Fix CANNODE_SUB_MBD typo
2026-08-09 03:10:35 +08:00 · 2022-11-18 11:53:56 -05:00 · 2022-11-18 11:04:17 -05:00 · 2022-11-18 11:04:17 -05:00 · 2022-11-18 07:10:27 +01:00 · 2022-11-18 07:10:27 +01:00
233 changed files with 5163 additions and 3477 deletions
@@ -920,7 +920,6 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true'
@@ -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,17 +1,29 @@
-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.
+<!--

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

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

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

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

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

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

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

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

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

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

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

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

 source /home/build-env.sh

-make modalai_voxl2_default
+make modalai_voxl2

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

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

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

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

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

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

 adb shell sync
@@ -42,10 +42,8 @@
 #define BOARD_HAS_NO_RESET
 #define BOARD_HAS_NO_BOOTLOADER

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

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

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

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

 	if(ARCHITECTURE)
@@ -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})
@@ -186,7 +186,6 @@ set(msg_files
 	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
-	VehicleAngularAcceleration.msg
 	VehicleAngularAccelerationSetpoint.msg
 	VehicleAngularVelocity.msg
 	VehicleAttitude.msg
@@ -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

@@ -1,5 +0,0 @@
-
-uint64 timestamp        # time since system start (microseconds)
-uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
-
-float32[3] xyz          # angular acceleration about the FRD body frame XYZ-axis in rad/s^2
@@ -1,7 +1,9 @@

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

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

 # TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth
@@ -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
@@ -0,0 +1 @@
+rsource "*/Kconfig"
@@ -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


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

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

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

 #include "SubscriptionCallback.hpp"

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 	static int        unadvertise(orb_advert_t handle);

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

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

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

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

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

-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR

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

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

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

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

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

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

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

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

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

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

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

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

 #ifdef ORB_USE_PUBLISHER_RULES

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

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

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

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

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

 private: // class methods

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

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

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

 	DeviceMaster *_device_master{nullptr};

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

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

 #ifdef ORB_USE_PUBLISHER_RULES

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

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

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

 extern int sercon_main(int c, char **argv);
 extern int serdis_main(int c, char **argv);
@@ -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;
 	}

@@ -97,18 +97,14 @@ if(EXISTS "${PX4_BOARD_DIR}/cmake/upload.cmake")
 	include(${PX4_BOARD_DIR}/cmake/upload.cmake)
 endif()

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

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

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

@@ -31,11 +31,18 @@
 #
 ############################################################################

-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
-		${module_libraries}
+include_directories(
+	${CMAKE_CURRENT_BINARY_DIR}
+	)
+
+add_library(px4 SHARED
+	${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
+	)
+
+target_link_libraries(px4
+	modules__muorb__slpi
+	${module_libraries}
+	px4_layer
 	)
@@ -31,6 +31,27 @@
 #
 ############################################################################

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

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

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

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

 	bool updated = false;

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

 	do {
@@ -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:
@@ -37,6 +37,7 @@

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

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

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

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

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

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

@@ -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 */

--- a/Show More
+++ b/Show More