Makefile: add target for unit tests

2026-05-27 03:40:04 +08:00 · 2022-10-26 19:40:31 +02:00
235 changed files with 3481 additions and 5165 deletions
@@ -920,6 +920,7 @@ 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 Discord (you can find an invite link on this project README).
+For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Slack (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,29 +1,17 @@
-<!--
+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.

-Thank you for your contribution!
+## 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...

-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 your solution
+A clear and concise description of what you have implemented.

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

-### Solved Problem
-When ... I found that ...
+## 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.

-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
+## Additional context
+Add any other related context or media.
@@ -410,7 +410,7 @@ if(BUILD_TESTING)
 	include(gtest)

 	add_custom_target(test_results
-			COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test -R ${TESTFILTER} USES_TERMINAL
+			COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test -R \"${TESTFILTER}\" USES_TERMINAL
 			DEPENDS
 				px4
 				examples__dyn_hello
@@ -94,22 +94,21 @@ pipeline {

        stage('failsafe docs') {
          agent {
-            docker { image 'px4io/px4-dev-nuttx-focal:2021-08-18' }
+            docker { image 'px4io/px4-dev-base-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,7 +190,3 @@ endmenu
 menu "examples"
 source "src/examples/Kconfig"
 endmenu
-
-menu "platforms"
-source "platforms/common/Kconfig"
-endmenu
@@ -386,6 +386,8 @@ tests_coverage:
 	@mkdir -p coverage
 	@lcov --directory build/px4_sitl_test --base-directory build/px4_sitl_test --gcov-tool gcov --capture -o coverage/lcov.info

+test_unit:
+	@$(MAKE) tests TESTFILTER="unit\|functional"

 rostest: px4_sitl_default
 	@$(MAKE) --no-print-directory px4_sitl_default sitl_gazebo
@@ -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_S_Y 0.3
+param set-default VT_FW_DIFTHR_SC 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

@@ -10,7 +10,6 @@
 # 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_S_Y 0.5
+param set-default VT_FW_DIFTHR_SC 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_S_Y 0.3
+param set-default VT_FW_DIFTHR_SC 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

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

@@ -264,6 +264,23 @@ 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
 	#
@@ -510,23 +527,6 @@ 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.GetType()
+            result += '### %s\n\n' % group.GetName()

            # 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.GetAirframes())
-            for airframe in group.GetAirframes():
-                if not self.IsExcluded(airframe, board):
-                    for output_name in airframe.GetOutputCodes():
-                        value = airframe.GetOutputValue(output_name)
+            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)
                        key_value_pair = (output_name, value)
                        if key_value_pair not in all_outputs:
                            all_outputs[key_value_pair] = 0
@@ -104,17 +104,18 @@ div.frame_variant td, div.frame_variant th {
            result += ' </thead>\n'
            result += '<tbody>\n'

-            for airframe in group.GetAirframes():
-                if not self.IsExcluded(airframe, board):
-                    name = airframe.GetName()
-                    airframe_id = airframe.GetId()
+            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()
                    airframe_id_entry = '<p><code>SYS_AUTOSTART</code> = %s</p>' % (airframe_id)
-                    maintainer = airframe.GetMaintainer()
+                    maintainer = param.GetMaintainer()
                    maintainer_entry = ''
                    if maintainer != '':
                        maintainer_entry = 'Maintainer: %s' % (html.escape(maintainer))
-                    url = airframe.GetFieldValue('url')
-                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetType(),name)
+                    url = param.GetFieldValue('url')
+                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetName(),name)
                    name_anchor=name_anchor.replace(' ','_').lower()
                    name_anchor=name_anchor.replace('"','_').lower()
                    name_anchor='id="%s"' % name_anchor
@@ -123,8 +124,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 airframe.GetOutputCodes():
-                        value = airframe.GetOutputValue(output_name)
+                    for output_name in param.GetOutputCodes():
+                        value = param.GetOutputValue(output_name)
                        valstrs = value.split(";")
                        key_value_pair = (output_name, value)
                        if all_outputs[key_value_pair] < num_configs:
@@ -151,9 +152,9 @@ div.frame_variant td, div.frame_variant th {

        self.output = result

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

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

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

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

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

                if not path:
                    continue

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

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

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

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


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

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

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

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

-    def GetAirframes(self):
+    def GetParams(self):
        """
-        Returns the parsed list of airframes objects. Note that returned
-        object is not a copy. Modifications affect state of the parser.
+        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.
        """
-        return sorted(self.airframes, key=lambda x: x.GetId())

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

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

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

    def GetSupportedExtensions(self):
        """
@@ -352,10 +347,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
@@ -432,7 +427,7 @@ class SourceParser(object):
            post_path = None

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

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

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

        # Store outputs
        for arch in archs:
-            airframe.SetArch(arch, archs[arch])
+            param.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.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)
+        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)

        return True

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

        return True

-    def GetAirframeGroups(self):
+    def GetParamGroups(self):
        """
-        Returns the parsed list of Airframe groups. Every Airframe is an Airframe
+        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.
        """
-        groups = self.airframe_groups.values()
-        groups = sorted(groups, key=lambda x: x.GetType())
+        groups = self.param_groups.values()
+        groups = sorted(groups, key=lambda x: x.GetName())
        groups = sorted(groups, key=lambda x: x.GetClass())
-        groups = sorted(groups, key=lambda x: self.priority.get(x.GetType(), 0), reverse=True)
+        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 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.GetType() in duplicate_test:
-                duplicate_set.add(group.GetType())
+            if group.GetName() in duplicate_test:
+                duplicate_set.add(group.GetName())
            else:
-                duplicate_test.add(group.GetType() )
+                duplicate_test.add(group.GetName() )
        for group in groups:
-            if group.GetType() in duplicate_set:
-                group.name=group.GetType()+' (%s)' % group.GetClass()
+            if group.GetName() in duplicate_set:
+                group.name=group.GetName()+' (%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.GetType()
+            xml_group.attrib["name"] = group.GetName()
            xml_group.attrib["image"] = group.GetImageName()
-            for airframe in group.GetAirframes():
+            for param in group.GetParams():

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

                if not excluded:
-                    #print("generating: {0} {1}".format(airframe.GetName(), excluded))
+                    #print("generating: {0} {1}".format(param.GetName(), excluded))
                    xml_param = ET.SubElement(xml_group, "airframe")
-                    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_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_field = ET.SubElement(xml_param, code)
                        xml_field.text = value
-                    for code in airframe.GetOutputCodes():
-                        value = airframe.GetOutputValue(code)
+                    for code in param.GetOutputCodes():
+                        value = param.GetOutputValue(code)
                        valstrs = value.split(";")
                        xml_field = ET.SubElement(xml_param, "output")
                        xml_field.attrib["name"] = code
@@ -35,11 +35,12 @@
 #
 # PX4 airframe config processor (main executable file)
 #
-# This tool scans the PX4 ROMFS directory for declarations of airframes
+# 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)
 #
-# 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
@@ -103,31 +104,31 @@ def main():
    # We can't validate yet
    # if not parser.Validate():
    #     sys.exit(1)
-    airframe_groups = parser.GetAirframeGroups()
+    param_groups = parser.GetParamGroups()

    # Output to XML file
    if args.xml:
        if args.verbose: print("Creating XML file " + args.xml)
-        out = xmlout.XMLOutput(airframe_groups, args.board)
+        out = xmlout.XMLOutput(param_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(airframe_groups, args.board, args.image_path)
+        out = markdownout.MarkdownTablesOutput(param_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(airframe_groups, args.board)
+        out = rcout.RCOutput(param_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(airframe_groups, args.board, post_start=True)
+        out_post = rcout.RCOutput(param_groups, args.board, post_start=True)
        out_post.Save(post_start_script)

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

    # upload the firmware
-    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()
-
+    def upload(self, fw, force=False, boot_delay=None):
        # Make sure we are doing the right thing
        start = _time()
        if self.board_type != fw.property('board_id'):
@@ -792,7 +764,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", nargs='+', help="Firmware file(s)")
+    parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
    args = parser.parse_args()

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

-    print("Waiting for bootloader...")
+    # 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()
+
    # tell any GCS that might be connected to the autopilot to give up
    # control of the serial port

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

                try:
                    # ok, we have a bootloader, try flashing it
-                    up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay)
+                    up.upload(fw, 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_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.nblocks = 248
 		},
 		{
@@ -86,17 +86,12 @@ static const px4_mtd_entry_t base_eeprom = {

 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 3,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 240
-		},
-		{
-			.type = MTD_MFT_REV,
-			.path = "/fs/mtd_mft_rev",
-			.nblocks = 8
+			.nblocks = 248
 		},
 		{
 			.type = MTD_ID,
@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- *   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>
@@ -1,35 +0,0 @@
-/****************************************************************************
- *
- * 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/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
+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
 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/scripts/install-voxl.sh
+px4$ boards/modalai/voxl2/install-voxl.sh
 ```

 ## Running PX4 on VOXL 2
@@ -66,17 +66,10 @@ 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 +1,12 @@
 #!/bin/bash

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

 source /home/build-env.sh

-make modalai_voxl2
+make modalai_voxl2_default

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

-echo "*** End of apps processor build ***"
+
+echo "*** End of posix build ***"
@@ -0,0 +1,11 @@
+#!/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 ***"
@@ -1,7 +0,0 @@
-
-# libfc_sensor.so is provided in the Docker build environment
-target_link_libraries(px4 PRIVATE
-		/home/libfc_sensor.so
-		px4_layer
-		${module_libraries}
-)
@@ -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,8 +30,25 @@
 # 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.
+#
+############################################################################

-add_library(drivers_board
-	board_config.h
-	init.c
-    )
+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)
@@ -0,0 +1,43 @@
+############################################################################
+#
+# 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,6 +2,3 @@ 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,19 +1,19 @@
 #!/bin/bash

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

-# Push apps processor image to voxl2
+# Push posix 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/target/voxl-px4 /usr/bin
+adb push boards/modalai/voxl2/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/target/voxl-px4.config /etc/modalai
+adb push boards/modalai/voxl2/voxl-px4.config /etc/modalai

 adb shell sync
@@ -1,5 +1,3 @@
 CONFIG_PLATFORM_QURT=y
 CONFIG_BOARD_TOOLCHAIN="qurt"
 CONFIG_MODULES_MUORB_SLPI=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_ORB_COMMUNICATOR=y
@@ -1,11 +0,0 @@
-#!/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 ***"
@@ -42,8 +42,10 @@
 #define BOARD_HAS_NO_RESET
 #define BOARD_HAS_NO_BOOTLOADER

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

 #include <system_config.h>
 #include <px4_platform_common/board_common.h>
@@ -99,14 +99,14 @@ __BEGIN_DECLS

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

-#define DIRECT_PWM_OUTPUT_CHANNELS  2
+#define DIRECT_PWM_OUTPUT_CHANNELS  1


 #define BOARD_HAS_LED_PWM              1
@@ -105,13 +105,11 @@
 #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 =
@@ -156,26 +154,17 @@ 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, FTM1 and FTM2 */
+	/* Enabled System Clock Gating Control for FTM0, 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_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.nblocks = 248
 		},
 		{
@@ -86,17 +86,12 @@ static const px4_mtd_entry_t base_eeprom = {

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

 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 3,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 240
-		},
-		{
-			.type = MTD_MFT_REV,
-			.path = "/fs/mtd_mft_rev",
-			.nblocks = 8
+			.nblocks = 248
 		},
 		{
 			.type = MTD_ID,
@@ -74,7 +74,6 @@ 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_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.nblocks = 248
 		},
 		{
@@ -86,17 +86,12 @@ static const px4_mtd_entry_t base_eeprom = {

 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 3,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 240
-		},
-		{
-			.type = MTD_MFT_REV,
-			.path = "/fs/mtd_mft_rev",
-			.nblocks = 8
+			.nblocks = 248
 		},
 		{
 			.type = MTD_ID,
@@ -66,8 +66,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.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 "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
+    if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "qurt" 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,6 +228,10 @@ 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,9 +170,6 @@ 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,6 +186,7 @@ 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 relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude
+float32[4] q					# Attitude of the camera, zero rotation is facing towards front of vehicle
 int8 result					# 1 for success, 0 for failure, -1 if camera does not provide feedback
@@ -1,10 +1,12 @@
 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

@@ -0,0 +1,5 @@
+
+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,9 +1,7 @@

-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_derivative # angular acceleration about the FRD body frame XYZ-axis in rad/s^2
+float32[3] xyz		# Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s

 # 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_integral	# Reset roll/pitch/yaw integrals (navigation logic change)
+bool reset_rate_integrals	# Reset roll/pitch/yaw integrals (navigation logic change)

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

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

 bool vertical_movement
 bool horizontal_movement
-bool rotational_movement

 bool close_to_ground_or_skipped_check

@@ -8,5 +8,3 @@ 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_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
+if(NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
 	list(APPEND SRCS
 		px4_log.cpp
 		px4_log_history.cpp
@@ -1 +0,0 @@
-rsource "*/Kconfig"
@@ -136,9 +136,5 @@ __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,13 +37,12 @@ typedef enum  {
 	MTD_PARAMETERS  = 1,
 	MTD_WAYPOINTS   = 2,
 	MTD_CALDATA     = 3,
-	MTD_MFT_VER     = 4,
-	MTD_MFT_REV     = 5,
-	MTD_ID          = 6,
-	MTD_NET         = 7
+	MTD_MFT         = 4,
+	MTD_ID          = 5,
+	MTD_NET         = 6,
 } px4_mtd_types_t;
-#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"}
+#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"}

 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 = HW_VER_REV(board_get_hw_version(), board_get_hw_revision());
+	int hw_version_revision = (board_get_hw_version() << 16) | board_get_hw_revision();
 #endif


@@ -1,5 +0,0 @@
-menuconfig ORB_COMMUNICATOR
-	bool "orb communicator"
-	default n
-	---help---
-		Enable support for the uorb communicator for distributed platforms
@@ -36,6 +36,10 @@
 #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 CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 #include "uORBCommunicator.hpp"
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* 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 CONFIG_ORB_COMMUNICATOR
+#ifdef 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 /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */

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

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

 	return -1;
 }
-#endif /* CONFIG_ORB_COMMUNICATOR */
+
+/*
+//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 */

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

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

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

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

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

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

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

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

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

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

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

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

 	static int        unadvertise(orb_advert_t handle);

-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef 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 /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */

 	/**
 	  * Add the subscriber to the node's list of subscriber.  If there is
@@ -48,10 +48,6 @@
 #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()
@@ -262,7 +258,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		}
 	}

-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR

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

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

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

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

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

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

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

 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;

-	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);
+	if (isAdvertisement) {
 		_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;
@@ -762,7 +723,7 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
 	return _remote_subscriber_topics.find(messageName);
 }

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

 #ifdef ORB_USE_PUBLISHER_RULES

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

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

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

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

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

 private: // class methods

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

-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef 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 /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */

 	DeviceMaster *_device_master{nullptr};

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

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

 #ifdef ORB_USE_PUBLISHER_RULES

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

 #if defined(CONFIG_SYSTEM_CDCACM)
 __BEGIN_DECLS
-#include <board_config.h>
 #include <arch/board/board.h>
 #include <syslog.h>
 #include <nuttx/wqueue.h>
@@ -43,7 +42,6 @@ __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,10 +32,8 @@
 ****************************************************************************/
 #pragma once

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

 #pragma pack(push, 1)

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

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

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

 #define MAX_LED_TIMERS			1
 #define MAX_TIMER_LED_CHANNELS		3
@@ -445,36 +445,22 @@ __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) {

-		// 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) {
+		if (rvmft == OK && path != NULL && hw_version == HW_VERSION_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_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;
+				hw_version = mtd_mft.hw_extended_ver;
 			}
 		}
 	}
@@ -509,14 +495,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("Version is: %d, Only mft version %d is supported\n", 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);
 		return -EINVAL;
 	}

 	mtd_mft_v0_t *mtd_mft = (mtd_mft_v0_t *)mtd_mft_unk;

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

@@ -97,14 +97,18 @@ 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,18 +31,11 @@
 #
 ############################################################################

-get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES)
+set(module_libraries modules__muorb__slpi)

-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
+QURT_LIB(LIB_NAME px4
+	SOURCES
+		${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
+	LINK_LIBS
+		${module_libraries}
 	)
@@ -31,27 +31,6 @@
 #
 ############################################################################

-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
@@ -147,7 +126,6 @@ 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,3 +176,46 @@ 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()
@@ -1,35 +0,0 @@
-/****************************************************************************
- *
- * 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,7 +35,6 @@
 #include <stdarg.h>
 #include <stdio.h>
 #include <stdint.h>
-#include <px4_platform_common/defines.h>

 __BEGIN_DECLS

@@ -1,48 +0,0 @@
-/****************************************************************************
- *
- * 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
@@ -1,45 +0,0 @@
-/****************************************************************************
- *
- * 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,14 +32,3 @@
 ############################################################################

 # 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)
@@ -1,326 +0,0 @@
-/****************************************************************************
- *
- * 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();
-}
@@ -1,39 +0,0 @@
-/****************************************************************************
- *
- * 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
@@ -1,379 +0,0 @@
-/****************************************************************************
- *
- * 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,6 +180,16 @@ 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 = (typeof(ts->tv_sec))(abstime / 1000000);
-	abstime -= (hrt_abstime)(ts->tv_sec) * 1000000;
+	ts->tv_sec = (time_t)abstime / 1000000;
+	abstime -= (hrt_abstime)(ts->tv_sec * 1000000);
 	ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000);
 }

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

 	bool updated = false;

-	// Limit maximum number of GPS injections to 8 since usually
+	// Limit maximum number of GPS injections to 6 since usually
 	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
-	// Looking at 8 packets thus guarantees, that at least a full injection
+	// Looking at 6 packets thus guarantees, that at least a full injection
 	// data set is evaluated.
-	// 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;
+	const size_t max_num_injections = 6;
 	size_t num_injections = 0;

 	do {
@@ -41,7 +41,6 @@
 #pragma once

 #include <cstdint>
-#include <cstdlib>

 namespace InvenSense_ICM20649
 {
@@ -41,7 +41,6 @@
 #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(const I2CSPIDriverConfig &config);
-extern device::Device *LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);
+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);

 #define LIS2MDLL_ADDRESS        0x1e

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

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

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

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

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

@@ -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);
+		interface = LIS2MDL_I2C_interface(config.bus, config.bus_frequency);

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

 	if (interface == nullptr) {
@@ -94,7 +94,6 @@ 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;

@@ -113,6 +112,8 @@ 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(const I2CSPIDriverConfig &config);
+	LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
 	virtual ~LIS2MDL_SPI() = default;

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

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

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

-LIS2MDL_SPI::LIS2MDL_SPI(const I2CSPIDriverConfig &config) :
-	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)
 {
 }

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

 /* interface factories */
-extern device::Device *LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);
-extern device::Device *LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);
+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);

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

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

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

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

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

@@ -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);
+		interface = LIS3MDL_I2C_interface(config.bus, config.bus_frequency);

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

 	if (interface == nullptr) {
@@ -90,7 +90,6 @@ 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();
@@ -101,7 +100,6 @@ 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;

@@ -120,6 +118,8 @@ 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(const I2CSPIDriverConfig &config);
+	LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
 	virtual ~LIS3MDL_SPI() = default;

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

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

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

-LIS3MDL_SPI::LIS3MDL_SPI(const I2CSPIDriverConfig &config) :
-	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)
 {
 }

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

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

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

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

 ### Examples
 CLI usage example:
@@ -37,7 +37,6 @@

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

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

-#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_mbd = 0;
-	param_get(param_find("CANNODE_SUB_MBD"), &cannode_sub_mbd);
+	int32_t cannode_sub_mdb = 0;
+	param_get(param_find("CANNODE_SUB_MDB"), &cannode_sub_mdb);

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

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