boardctl hacks

move I2C bus clock defaults to board px4_i2c_buses
Tools/px_uploader.py: use monotonic clock if available (#20352 )
2026-06-01 13:30:04 +08:00 · 2022-10-09 11:31:33 -04:00 · 2022-10-08 13:25:15 -04:00 · 2022-10-06 09:49:05 -04:00 · 2022-10-06 08:44:36 -04:00 · 2022-10-05 20:26:25 -04:00
124 changed files with 3949 additions and 721 deletions
@@ -9,15 +9,15 @@
 [submodule "Tools/simulation/jmavsim/jMAVSim"]
 	path = Tools/simulation/jmavsim/jMAVSim
 	url = https://github.com/PX4/jMAVSim.git
-	branch = master
+	branch = main
 [submodule "Tools/simulation/gazebo/sitl_gazebo"]
 	path = Tools/simulation/gazebo/sitl_gazebo
 	url = https://github.com/PX4/PX4-SITL_gazebo.git
-	branch = master
+	branch = main
 [submodule "src/drivers/gps/devices"]
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/PX4-GPSDrivers.git
-	branch = master
+	branch = main
 [submodule "src/modules/micrortps_bridge/micro-CDR"]
 	path = src/modules/micrortps_bridge/micro-CDR
 	url = https://github.com/PX4/Micro-CDR.git
@@ -53,6 +53,7 @@
 [submodule "src/lib/events/libevents"]
 	path = src/lib/events/libevents
 	url = https://github.com/mavlink/libevents.git
+	branch = main
 [submodule "src/lib/crypto/libtomcrypt"]
 	path = src/lib/crypto/libtomcrypt
 	url = https://github.com/PX4/libtomcrypt.git
@@ -41,6 +41,20 @@ elif [ "$PX4_SIMULATOR" = "gz" ]; then
 	fi

 	gz_world=$( ign topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+	
+	gz_version_major=$( ign gazebo --versions | sed 's/\..*//g' )
+	gz_version_minor=$( ign gazebo --versions | sed 's/'"${gz_version_major}"\.'//; s/\..*//g' )
+	gz_version_point=$( ign gazebo --versions | sed 's/'"${gz_version_major}"\.'//; s/'"${gz_version_minor}"\.'//')
+
+	if [ "$gz_version_major" -gt "6" ] || { [ "$gz_version_major" -eq "6" ] && [ "$gz_version_minor" -gt "12" ]; } || { [ "$gz_version_major" -eq "6" ] && [ "$gz_version_minor" -eq "12" ] && [ "$gz_version_point" -gt "0" ]; }; then
+		echo "INFO  [init] using latest version of MultiCopterMotor plugin."
+	else
+		echo "WARN  [init] using older version of MultiCopterMotor plugin, please update to latest gazebo > 6.12.0."
+		if [ "$PX4_SIM_MODEL" = "x500" ]; then
+			PX4_SIM_MODEL="x500-Legacy"
+			echo "WARN  [init] setting PX4_SIM_MODEL -> $PX4_SIM_MODEL from x500 till gazebo > 6.12.0"
+		fi
+	fi

 	if [ -z $gz_world ]; then

@@ -38,7 +38,6 @@ param set-default MIS_YAW_TMT 10
 param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_THR_MIN 0.1
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_XY_P 0.8
 param set-default MPC_XY_VEL_D_ACC 0.1
 param set-default MPC_XY_VEL_I_ACC 4
@@ -65,7 +65,6 @@ param set-default COM_SPOOLUP_TIME 1.5
 param set-default MPC_THR_HOVER 0.45
 param set-default MPC_TILTMAX_AIR 25
 param set-default MPC_TKO_RAMP_T 1.8
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_VEL_MANUAL 3
 param set-default MPC_XY_CRUISE 3
 param set-default MPC_XY_VEL_MAX 3.5
@@ -63,7 +63,6 @@ param set-default MPC_ACC_UP_MAX 4
 param set-default MPC_MAN_Y_MAX 120
 param set-default MPC_TILTMAX_AIR 45
 param set-default MPC_THR_HOVER 0.3
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_VEL_MANUAL 5
 param set-default MPC_XY_CRUISE 5
 param set-default MPC_XY_VEL_MAX 5
@@ -64,7 +64,6 @@ param set-default MPC_MANTHR_MIN 0
 param set-default MPC_MAN_Y_MAX 120
 param set-default MPC_TILTMAX_AIR 45
 param set-default MPC_THR_HOVER 0.3
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_VEL_MANUAL 5
 param set-default MPC_XY_CRUISE 5
 param set-default MPC_XY_VEL_MAX 5
@@ -53,7 +53,6 @@ param set-default MPC_XY_VEL_P_ACC 2.6
 param set-default MPC_XY_VEL_I_ACC 1.2
 param set-default MPC_XY_VEL_D_ACC 0.2
 param set-default MPC_TKO_RAMP_T 1
-param set-default MPC_TKO_SPEED 1.1
 param set-default MPC_VEL_MANUAL 3

 param set-default BAT1_SOURCE 0
@@ -62,7 +62,6 @@ param set-default MPC_XY_VEL_P_ACC 2.6
 param set-default MPC_XY_VEL_I_ACC 1.2
 param set-default MPC_XY_VEL_D_ACC 0.2
 param set-default MPC_TKO_RAMP_T 1
-param set-default MPC_TKO_SPEED 1.1
 param set-default MPC_VEL_MANUAL 3

 param set-default BAT1_SOURCE 0
@@ -324,6 +324,27 @@ else
 	rc_update start
 	manual_control start

+	# Start camera trigger, capture and PPS before pwm_out as they might access
+	# pwm pins
+	if param greater -s TRIG_MODE 0
+	then
+		camera_trigger start
+		camera_feedback start
+	fi
+	# PPS capture driver
+	if param greater -s PPS_CAP_ENABLE 0
+	then
+		pps_capture start
+	fi
+	# Camera capture driver
+	if param greater -s CAM_CAP_FBACK 0
+	then
+		if camera_capture start
+		then
+			camera_capture on
+		fi
+	fi
+
 	#
 	# Sensors System (start before Commander so Preflight checks are properly run).
 	# Commander needs to be this early for in-air-restarts.
@@ -390,12 +411,6 @@ else
 		mag_bias_estimator start
 	fi

-	if param greater -s TRIG_MODE 0
-	then
-		camera_trigger start
-		camera_feedback start
-	fi
-
 	#
 	# Optional board mavlink streams: rc.board_mavlink
 	#
@@ -416,21 +431,6 @@ else
 	# Must be started after the serial config is read
 	rc_input start $RC_INPUT_ARGS

-	# PPS capture driver (before pwm_out)
-	if param greater -s PPS_CAP_ENABLE 0
-	then
-		pps_capture start
-	fi
-
-	# Camera capture driver (before pwm_out)
-	if param greater -s CAM_CAP_FBACK 0
-	then
-		if camera_capture start
-		then
-			camera_capture on
-		fi
-	fi
-
 	#
 	# Play the startup tune (if not disabled or there is an error)
 	#
@@ -489,7 +489,7 @@ actuators = {

 with open(output_file, 'w') as outfile:
    indent = 2 if verbose else None
-    json.dump(actuators, outfile, indent=indent)
+    json.dump(actuators, outfile, indent=indent, sort_keys=True)

 if compress:
    save_compressed(output_file)
@@ -77,11 +77,23 @@ except ImportError as e:
    print("")
    sys.exit(1)

+
+# Define time to use time.time() by default
+def _time():
+    return time.time()
+
 # Detect python version
 if sys.version_info[0] < 3:
    runningPython3 = False
 else:
    runningPython3 = True
+    if sys.version_info[1] >=3:
+        # redefine to use monotonic time when available
+        def _time():
+            try:
+                return time.monotonic()
+            except Exception:
+                return time.time()

 class FirmwareNotSuitableException(Exception):
    def __init__(self, message):
@@ -230,7 +242,7 @@ class uploader(object):

    def open(self):
        # upload timeout
-        timeout = time.time() + 0.2
+        timeout = _time() + 0.2

        # attempt to open the port while it exists and until timeout occurs
        while self.port is not None:
@@ -240,7 +252,7 @@ class uploader(object):
            except AttributeError:
                portopen = self.port.isOpen()

-            if not portopen and time.time() < timeout:
+            if not portopen and _time() < timeout:
                try:
                    self.port.open()
                except OSError:
@@ -415,16 +427,16 @@ class uploader(object):
                    uploader.EOC)

        # erase is very slow, give it 30s
-        deadline = time.time() + 30.0
-        while time.time() < deadline:
+        deadline = _time() + 30.0
+        while _time() < deadline:

            usualEraseDuration = 15.0
-            estimatedTimeRemaining = deadline-time.time()
+            estimatedTimeRemaining = deadline-_time()
            if estimatedTimeRemaining >= usualEraseDuration:
                self.__drawProgressBar(label, 30.0-estimatedTimeRemaining, usualEraseDuration)
            else:
                self.__drawProgressBar(label, 10.0, 10.0)
-                sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()))
+                sys.stdout.write(" (timeout: %d seconds) " % int(deadline-_time()))
                sys.stdout.flush()

            if self.__trySync():
@@ -572,7 +584,7 @@ class uploader(object):
    # upload the firmware
    def upload(self, fw, force=False, boot_delay=None):
        # Make sure we are doing the right thing
-        start = time.time()
+        start = _time()
        if self.board_type != fw.property('board_id'):
            msg = "Firmware not suitable for this board (Firmware board_type=%u board_id=%u)" % (
                self.board_type, fw.property('board_id'))
@@ -668,7 +680,7 @@ class uploader(object):
        print("\nRebooting.", end='')
        self.__reboot()
        self.port.close()
-        print(" Elapsed Time %3.3f\n" % (time.time() - start))
+        print(" Elapsed Time %3.3f\n" % (_time() - start))

    def __next_baud_flightstack(self):
        if self.baudrate_flightstack_idx + 1 >= len(self.baudrate_flightstack):
@@ -2,7 +2,7 @@

 set -e

-## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
+## Bash script to setup PX4 development environment on Ubuntu LTS (22.04, 20.04, 18.04).
 ## Can also be used in docker.
 ##
 ## Installs:
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>x500-Legacy</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Benjamin Perseghetti</name>
+    <email>bperseghetti@rudislabs.com</email>
+  </author>
+  <description>Model of the NXP HoverGames Drone development kit (KIT-HGDRONEK66). The PX4 software compatible kit provides mechanical, RC remote and other components needed to evaluate the RDDRONE-FMUK66 reference design. The FMU includes 100Base-T1 Automotive Ethernet, dual CAN transceivers, as well as SE050 secure element, and works with add on boards NavQPlus, MR-T1ETH8, MR-T1ADAPT, and CAN-nodes such as UCANS32K1SIC.  Kit may be used with, and contains the components needed for the HoverGames.com coding challenges.</description>
+</model>
@@ -0,0 +1,76 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500-Legacy'>
+    <include merge='true'>
+      <uri>model://x500-NoPlugin</uri>
+    </include>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_0_joint</jointName>
+      <linkName>rotor_0</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_1_joint</jointName>
+      <linkName>rotor_1</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>1</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_2_joint</jointName>
+      <linkName>rotor_2</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>2</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_3_joint</jointName>
+      <linkName>rotor_3</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>3</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+  </model>
+</sdf>
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>x500-NoPlugin</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Benjamin Perseghetti</name>
+    <email>bperseghetti@rudislabs.com</email>
+  </author>
+  <description>Model of the NXP HoverGames Drone development kit (KIT-HGDRONEK66). The PX4 software compatible kit provides mechanical, RC remote and other components needed to evaluate the RDDRONE-FMUK66 reference design. The FMU includes 100Base-T1 Automotive Ethernet, dual CAN transceivers, as well as SE050 secure element, and works with add on boards NavQPlus, MR-T1ETH8, MR-T1ADAPT, and CAN-nodes such as UCANS32K1SIC.  Kit may be used with, and contains the components needed for the HoverGames.com coding challenges.</description>
+</model>
@@ -0,0 +1,516 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500-NoPlugin'>
+    <pose>0 0 .24 0 0 0</pose>
+    <self_collide>false</self_collide>
+    <static>false</static>
+    <link name="base_link">
+      <inertial>
+        <mass>2.0</mass>
+        <inertia>
+          <ixx>0.02166666666666667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.02166666666666667</iyy>
+          <iyz>0</iyz>
+          <izz>0.04000000000000001</izz>
+        </inertia>
+      </inertial>
+      <gravity>true</gravity>
+      <velocity_decay/>
+      <visual name="base_link_visual">
+        <pose>0 0 .025 0 0 3.141592654</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/NXP-HGD-CF.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_0">
+        <pose>0.174 0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_1">
+        <pose>-0.174 0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_2">
+        <pose>0.174 -0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_3">
+        <pose>-0.174 -0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="NXP_FMUK66_FRONT">
+        <pose>0.047 .001 .043 1 0 1.57</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>.013 .007</size>
+          </plane>
+        </geometry>
+        <material>
+          <diffuse>1.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+          <pbr>
+            <metal>
+              <albedo_map>model://x500-NoPlugin/materials/textures/nxp.png</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+      <visual name="NXP_FMUK66_TOP">
+        <pose>-0.023 0 .0515 0 0 -1.57</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>.013 .007</size>
+          </plane>
+        </geometry>
+        <material>
+          <diffuse>1.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+          <pbr>
+            <metal>
+              <albedo_map>model://x500-NoPlugin/materials/textures/nxp.png</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+      <visual name="RDDRONE_FMUK66_TOP">
+        <pose>-.03 0 .0515 0 0 -1.57</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>.032 .0034</size>
+          </plane>
+        </geometry>
+        <material>
+          <diffuse>1.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+          <pbr>
+            <metal>
+              <albedo_map>model://x500-NoPlugin/materials/textures/rd.png</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+      <collision name="base_link_collision_0">
+        <pose>0 0 .007 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.35355339059327373 0.35355339059327373 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_1">
+        <pose>0 -0.098 -.123 -0.35 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.015 0.015 0.21</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_2">
+        <pose>0 0.098 -.123 0.35 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.015 0.015 0.21</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_3">
+        <pose>0 -0.132 -.2195 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.25 0.015 0.015</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_4">
+        <pose>0 0.132 -.2195 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.25 0.015 0.015</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <sensor name="imu_sensor" type="imu">
+        <always_on>1</always_on>
+        <update_rate>250</update_rate>
+      </sensor>
+    </link>
+    <link name="rotor_0">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>0.174 -0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_0_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_ccw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_0_visual_motor_bell">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_0_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_0_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_0</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    <link name="rotor_1">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>-0.174 0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_1_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_ccw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_1_visual_motor_top">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_1_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_1_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_1</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    <link name="rotor_2">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>0.174 0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_2_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_cw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_2_visual_motor_top">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_2_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_2_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_2</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    <link name="rotor_3">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>-0.174 -0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_3_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_cw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_3_visual_motor_top">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_3_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_3_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_3</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+  </model>
+</sdf>
@@ -2,7 +2,71 @@
 <sdf version='1.9'>
  <model name='x500'>
    <include merge='true'>
-      <uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</uri>
+      <uri>model://x500-NoPlugin</uri>
    </include>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_0_joint</jointName>
+      <linkName>rotor_0</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_1_joint</jointName>
+      <linkName>rotor_1</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>1</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_2_joint</jointName>
+      <linkName>rotor_2</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>2</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_3_joint</jointName>
+      <linkName>rotor_3</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>3</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
  </model>
 </sdf>
@@ -85,17 +85,8 @@
 #define GPIO_NRF_TXEN			(GPIO_INPUT|GPIO_PULLUP|GPIO_EXTI|GPIO_PORTA|GPIO_PIN4)


-/*
- * I2C busses
- */
-#define PX4_I2C_BUS_ONBOARD_HZ      400000
-#define PX4_I2C_BUS_EXPANSION_HZ      400000
-
 #define PX4_I2C_BUS_MTD	1

-#define BOARD_NUMBER_I2C_BUSES  3
-#define BOARD_I2C_BUS_CLOCK_INIT {PX4_I2C_BUS_ONBOARD_HZ, 100000, PX4_I2C_BUS_EXPANSION_HZ}
-

 /* Devices on the onboard bus.
 *
@@ -86,17 +86,8 @@

 #define GPIO_I2C4_DRDY1_BMP388        	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5)

-/*
- * I2C busses
- */
-#define PX4_I2C_BUS_ONBOARD_HZ      400000
-#define PX4_I2C_BUS_EXPANSION_HZ      400000
-
 #define PX4_I2C_BUS_MTD	1

-#define BOARD_NUMBER_I2C_BUSES  3
-#define BOARD_I2C_BUS_CLOCK_INIT {PX4_I2C_BUS_ONBOARD_HZ, 400000, PX4_I2C_BUS_EXPANSION_HZ}
-

 /* Devices on the onboard bus.
 *
@@ -34,6 +34,6 @@
 #include <px4_arch/i2c_hw_description.h>

 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
-	initI2CBusExternal(1),
-	initI2CBusInternal(3),
+	initI2CBusExternal(1, 400'000),
+	initI2CBusInternal(3, 400'000),
 };
@@ -127,7 +127,6 @@ CONFIG_MTD_RAMTRON=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
-CONFIG_NETUTILS_TELNETD=y
 CONFIG_NET_ARP_IPIN=y
 CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
@@ -157,7 +156,6 @@ CONFIG_NSH_DISABLE_UNAME=y
 CONFIG_NSH_DISABLE_WGET=y
 CONFIG_NSH_DISABLE_XD=y
 CONFIG_NSH_FILEIOSIZE=512
-CONFIG_NSH_IOBUFFER_SIZE=256
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
 CONFIG_NSH_NESTDEPTH=8
@@ -165,8 +163,6 @@ CONFIG_NSH_READLINE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
-CONFIG_NSH_TELNETD_CLIENTSTACKSIZE=1280
-CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
@@ -204,10 +200,6 @@ CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_PING=y
 CONFIG_TASK_NAME_SIZE=24
-CONFIG_TELNET_IOTHREAD_STACKSIZE=512
-CONFIG_TELNET_MAXLCLIENTS=1
-CONFIG_TELNET_RXBUFFER_SIZE=128
-CONFIG_TELNET_TXBUFFER_SIZE=128
 CONFIG_UART0_IFLOWCONTROL=y
 CONFIG_UART0_OFLOWCONTROL=y
 CONFIG_UART1_RXBUFSIZE=600
@@ -95,7 +95,9 @@ CONFIG_KINETIS_SDHC=y
 CONFIG_KINETIS_SERIALBRK_BSDCOMPAT=y
 CONFIG_KINETIS_SPI0=y
 CONFIG_KINETIS_SPI1=y
+CONFIG_KINETIS_SPI1_DMA=y
 CONFIG_KINETIS_SPI2=y
+CONFIG_KINETIS_SPI_DMA=y
 CONFIG_KINETIS_UART0=y
 CONFIG_KINETIS_UART0_RXDMA=y
 CONFIG_KINETIS_UART1=y
@@ -125,7 +127,6 @@ CONFIG_MTD_RAMTRON=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
-CONFIG_NETUTILS_TELNETD=y
 CONFIG_NET_ARP_IPIN=y
 CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
@@ -155,7 +156,6 @@ CONFIG_NSH_DISABLE_UNAME=y
 CONFIG_NSH_DISABLE_WGET=y
 CONFIG_NSH_DISABLE_XD=y
 CONFIG_NSH_FILEIOSIZE=512
-CONFIG_NSH_IOBUFFER_SIZE=256
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
 CONFIG_NSH_NESTDEPTH=8
@@ -163,8 +163,6 @@ CONFIG_NSH_READLINE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
-CONFIG_NSH_TELNETD_CLIENTSTACKSIZE=1280
-CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
@@ -202,10 +200,6 @@ CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_PING=y
 CONFIG_TASK_NAME_SIZE=24
-CONFIG_TELNET_IOTHREAD_STACKSIZE=512
-CONFIG_TELNET_MAXLCLIENTS=1
-CONFIG_TELNET_RXBUFFER_SIZE=128
-CONFIG_TELNET_TXBUFFER_SIZE=128
 CONFIG_UART1_RXBUFSIZE=600
 CONFIG_UART1_TXBUFSIZE=1100
 CONFIG_UART4_BAUD=57600
@@ -100,7 +100,7 @@ else
 	# V3 build hwtypecmp supports V2|V2M|V30
 	if ! ver hwtypecmp V2M
 	then
-		mpu9250 -s -b 1 -R 14 start
+		mpu9250 -s -b 1 -R 14 -q start
 	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
 	fi

@@ -43,7 +43,7 @@

 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
-	initI2CBusInternal(2),
+	initI2CBusInternal(2, 100'000),
 };


@@ -100,7 +100,7 @@ else
 	# V3 build hwtypecmp supports V2|V2M|V30
 	if ! ver hwtypecmp V2M
 	then
-		mpu9250 -s -b 1 -R 14 start
+		mpu9250 -s -b 1 -R 14 -q start
 	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
 	fi

@@ -43,7 +43,7 @@

 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
-	initI2CBusInternal(2),
+	initI2CBusInternal(2, 100'000),
 };


@@ -34,6 +34,6 @@
 #include <px4_arch/i2c_hw_description.h>

 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
-	initI2CBusInternal(1),
+	initI2CBusInternal(1, 100'000),
 	initI2CBusExternal(2),
 };
@@ -87,6 +87,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_BOARDCTL=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -45,6 +45,7 @@ bool heartbeat_type_gimbal                  # MAV_TYPE_GIMBAL
 bool heartbeat_type_adsb                    # MAV_TYPE_ADSB
 bool heartbeat_type_camera                  # MAV_TYPE_CAMERA
 bool heartbeat_type_parachute               # MAV_TYPE_PARACHUTE
+bool heartbeat_type_open_drone_id           # MAV_TYPE_ODID

 # Heartbeats per component
 bool heartbeat_component_telemetry_radio    # MAV_COMP_ID_TELEMETRY_RADIO
@@ -58,4 +59,5 @@ bool heartbeat_component_uart_bridge        # MAV_COMP_ID_UART_BRIDGE

 # Misc component health
 bool avoidance_system_healthy
+bool open_drone_id_system_healthy
 bool parachute_system_healthy
@@ -112,10 +112,11 @@ bool auto_mission_available
 bool power_input_valid                            # set if input power is valid
 bool usb_connected                                # set to true (never cleared) once telemetry received from usb link

+bool open_drone_id_system_present
+bool open_drone_id_system_healthy
+
 bool parachute_system_present
 bool parachute_system_healthy

 bool avoidance_system_required                    # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
 bool avoidance_system_valid                       # Status of the obstacle avoidance system
-
-
@@ -41,7 +41,7 @@
 bool px4_i2c_bus_external(int bus)
 {
 	for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
-		if (px4_i2c_buses[i].bus == bus) {
+		if ((px4_i2c_buses[i].bus != -1) && (px4_i2c_buses[i].bus == bus)) {
 			return px4_i2c_buses[i].is_external;
 		}
 	}
@@ -60,6 +60,17 @@ bool px4_i2c_device_external(const uint32_t device_id)
 }
 #endif // BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL

+int px4_i2c_bus_max_speed(int bus)
+{
+	for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
+		if ((px4_i2c_buses[i].bus != -1) && (px4_i2c_buses[i].bus == bus)) {
+			return px4_i2c_buses[i].max_speed_hz;
+		}
+	}
+
+	return 0;
+}
+
 bool I2CBusIterator::next()
 {
 	while (++_index < I2C_BUS_MAX_BUS_ITEMS && px4_i2c_buses[_index].bus != -1) {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- * Copyright (C) 2020, 2021 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2020-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -49,6 +49,185 @@ static List<I2CSPIInstance *> i2c_spi_module_instances; ///< list of currently r
 static pthread_mutex_t i2c_spi_module_instances_mutex = PTHREAD_MUTEX_INITIALIZER;


+void px4_print_all_instances()
+{
+	pthread_mutex_lock(&i2c_spi_module_instances_mutex);
+
+#if defined(CONFIG_I2C)
+	// I2C
+	{
+		PX4_INFO_RAW("\nI2C                    Type       Address\n");
+		int num_i2c_buses = 0;
+
+		for (auto &i2c_bus : px4_i2c_buses) {
+			if (i2c_bus.bus != -1) {
+				num_i2c_buses++;
+
+				PX4_INFO_RAW("|__ I2C:%d    \n", i2c_bus.bus); // TODO: internal/external
+				// TODO: print differently if last
+
+				for (I2CSPIInstance *instance : i2c_spi_module_instances) {
+					if (instance && (instance->bus() == i2c_bus.bus)
+					    && (instance->bus_option() == I2CSPIBusOption::I2CInternal || instance->bus_option() == I2CSPIBusOption::I2CExternal)
+					   ) {
+						//PX4_INFO_RAW("|__%2d) ", i);
+						//PX4_INFO_RAW("|__ I2C %d %s\n", px4_i2c_buses[i].bus, px4_i2c_buses[i].is_external ? "external" : "internal");
+						// Type: 0x%02X, I2C:%d 0x%02X
+						PX4_INFO_RAW("|   |__ %s      0x%02X           0x%02X\n", instance->module_name(), instance->devid_driver_index(),
+							     instance->get_i2c_address());
+
+						// TODO: if running?
+						// TOOD: if exiting?
+						// TODO: general status otherwise?
+					}
+				}
+			}
+		}
+
+		// int num_i2c_instances = 0;
+		// for (const I2CSPIInstance *instance : i2c_spi_module_instances) {
+		// 	if (instance) {
+		// 		if (instance->bus_option() == I2CSPIBusOption::I2CInternal || instance->bus_option() == I2CSPIBusOption::I2CExternal) {
+		// 			num_i2c_instances++;
+		// 		}
+		// 	}
+		// }
+	}
+#endif // CONFIG_I2C
+
+#if defined(CONFIG_SPI)
+	// SPI
+	{
+		PX4_INFO_RAW("\nSPI                    Type       \n");
+
+		for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+			const auto &spi_bus = px4_spi_buses[i];
+
+			if (spi_bus.bus != -1) {
+				//num_i2c_buses++;
+
+				PX4_INFO_RAW("|__ SPI:%d (%s) %s %s \n",
+					     spi_bus.bus,
+					     px4_spi_buses[i].is_external ? "external" : "internal",
+					     (px4_spi_buses[i].power_enable_gpio != 0) ? " Power Control " : "",
+					     px4_spi_buses[i].requires_locking ? " requires locking " : ""
+					    );
+
+
+
+
+
+				// TODO: print differently if last
+
+				// power_enable_gpio
+				// is_external
+				// requires_locking
+
+				// PX4_SPIDEVID_TYPE(bus_data.devices[j].devid
+				// PX4_SPIDEVID_TYPE(devid) == PX4_SPIDEVID_TYPE(bus_data.devices[j].devid)
+				// PX4_SPI_DEV_ID(devid) == bus_data.devices[j].devtype_driver
+
+				for (const auto &dev : spi_bus.devices) {
+					if (dev.cs_gpio || dev.devid || dev.devtype_driver) {
+						// cs_gpio
+						// drdy_gpio
+						// devid
+						// devtype_driver
+						PX4_INFO_RAW("|   |__ %" PRIu32 ", devtype_driver: 0x%02X    \n", dev.devid, dev.devtype_driver);
+
+						// match bus type, bus, devtype_driver
+
+						// _bus_device_index = -1;
+
+
+						// devtype_driver  devid_driver_index()
+						//PX4_INFO_RAW("|   |__ %s      0x%02X           \n", instance->module_name(), instance->devid_driver_index());
+					}
+
+					// match bus
+					// match devtype_driver  devid_driver_index()
+				}
+
+				for (I2CSPIInstance *instance : i2c_spi_module_instances) {
+					if (instance && (instance->bus() == spi_bus.bus)
+					    && (instance->bus_option() == I2CSPIBusOption::SPIInternal || instance->bus_option() == I2CSPIBusOption::SPIExternal)
+					   ) {
+						//PX4_INFO_RAW("|__%2d) ", i);
+						//PX4_INFO_RAW("|__ I2C %d %s\n", px4_i2c_buses[i].bus, px4_i2c_buses[i].is_external ? "external" : "internal");
+						// Type: 0x%02X, I2C:%d 0x%02X
+						PX4_INFO_RAW("|   |__ %s      0x%02X           \n", instance->module_name(), instance->devid_driver_index());
+
+						//instance->
+
+						// TODO: if running?
+						// TOOD: if exiting?
+						// TODO: general status otherwise?
+					}
+				}
+			}
+		}
+	}
+#endif // CONFIG_SPI
+
+
+#if defined(CONFIG_I2C) && 0
+
+	for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
+		if ((px4_i2c_buses[i].bus != -1)) {
+			PX4_INFO_RAW("I2C %d %s\n", px4_i2c_buses[i].bus, px4_i2c_buses[i].is_external ? "external" : "internal");
+		}
+	}
+
+#endif // CONFIG_I2C
+
+#if defined(CONFIG_SPI) && 0
+
+	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+		if ((px4_spi_buses[i].bus != -1)) {
+			PX4_INFO_RAW("SPI %d %s\n", px4_spi_buses[i].bus, px4_spi_buses[i].is_external ? "external" : "internal");
+
+			for (const auto &dev : px4_spi_buses[i].devices) {
+				if (dev.devid || dev.devtype_driver) {
+					PX4_INFO_RAW("-->devid: %" PRIu32 ", devtype_driver: %" PRIu16 "\n", dev.devid, dev.devtype_driver);
+				}
+			}
+
+			// SPI_BUS_MAX_DEVICES
+			// devid
+			// devtype_driver
+			// for (px4_spi_buses[i].devices) {
+
+			// }
+		}
+	}
+
+#endif // CONFIG_SPI
+
+
+	// TODO: decode device id
+	// // Device ID
+	// uint32_t device_id = *(uint32_t *)(data_ptr + previous_data_offset);
+	// char device_id_buffer[80];
+	// device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), device_id);
+	// PX4_INFO_RAW(" (%s)", device_id_buffer);
+
+
+	// for (const I2CSPIInstance *instance : i2c_spi_module_instances) {
+	// 	if (instance) {
+
+
+	// 		// TODO: I2CSPIBusOption
+	// 		//  I2C address
+
+	// 		PX4_INFO_RAW("name: %s, bus: %d, devid_driver_index: %d, bus_device_index: %d\n", instance->module_name(),
+	// 			     instance->bus(), instance->devid_driver_index(), instance->bus_device_index());
+	// 	}
+	// }
+
+	pthread_mutex_unlock(&i2c_spi_module_instances_mutex);
+}
+
+
 I2CSPIDriverConfig::I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				       const px4::wq_config_t &wq_config_)
 	: module_name(iterator.moduleName()),
@@ -237,7 +416,6 @@ int BusCLIArguments::getOpt(int argc, char *argv[], const char *options)

 			if (bus_option == I2CSPIBusOption::I2CExternal || bus_option == I2CSPIBusOption::I2CInternal) {
 				bus_frequency = default_i2c_frequency;
-
 			}

 #endif // CONFIG_I2C
@@ -332,7 +510,7 @@ bool BusInstanceIterator::next()

 	} else if (busType() == BOARD_SPI_BUS) {
 		if (_spi_bus_iterator.next()) {
-			bus = _spi_bus_iterator.bus().bus;
+			bus = _spi_bus_iterator.bus();
 		}

 #endif // CONFIG_SPI
@@ -340,7 +518,7 @@ bool BusInstanceIterator::next()

 	} else if (busType() == BOARD_I2C_BUS) {
 		if (_i2c_bus_iterator.next()) {
-			bus = _i2c_bus_iterator.bus().bus;
+			bus = _i2c_bus_iterator.bus();
 		}

 #endif // CONFIG_I2C
@@ -448,14 +626,14 @@ int BusInstanceIterator::bus() const
 #if defined(CONFIG_SPI)

 	if (busType() == BOARD_SPI_BUS) {
-		return _spi_bus_iterator.bus().bus;
+		return _spi_bus_iterator.bus();
 	}

 #endif // CONFIG_SPI
 #if defined(CONFIG_I2C)

 	if (busType() == BOARD_I2C_BUS) {
-		return _i2c_bus_iterator.bus().bus;
+		return _i2c_bus_iterator.bus();
 	}

 #endif // CONFIG_I2C
@@ -813,6 +991,31 @@ void I2CSPIDriverBase::print_status()
 #endif // CONFIG_SPI
 }

+void I2CSPIDriverBase::print_run_status()
+{
+	// specialized ScheduledWorkItem::print_run_status() with additional I2C or SPI info
+	char name_description[34] {};
+#if defined(CONFIG_SPI)
+
+	if (_bus_option == I2CSPIBusOption::SPIInternal || _bus_option == I2CSPIBusOption::SPIExternal) {
+		snprintf(name_description, sizeof(name_description), "%s (Type: 0x%02X)", _item_name, _devid_driver_index);
+	}
+
+#endif // CONFIG_SPI
+
+#if defined(CONFIG_I2C)
+
+	if (_bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal) {
+		snprintf(name_description, sizeof(name_description), "%s (Type: 0x%02X, I2C:%d 0x%02X)", _item_name,
+			 _devid_driver_index, _bus, _i2c_address);
+	}
+
+#endif // CONFIG_I2C
+
+	PX4_INFO_RAW("%-34s %8.1f Hz %12.0f us (%" PRId64 " us)\n", name_description, (double)average_rate(),
+		     (double)average_interval(), 0ULL);
+}
+
 void I2CSPIDriverBase::request_stop_and_wait()
 {
 	_task_should_exit.store(true);
@@ -51,27 +51,6 @@
 * Definitions
 ************************************************************************************/

-/* I2C PX4 clock configuration
- *
- * A board may override BOARD_I2C_BUS_CLOCK_INIT simply by defining the #defines.
- */
-
-#if defined(BOARD_I2C_BUS_CLOCK_INIT)
-#  define PX4_I2C_BUS_CLOCK_INIT BOARD_I2C_BUS_CLOCK_INIT
-#else
-#  if (PX4_NUMBER_I2C_BUSES) == 1
-#    define PX4_I2C_BUS_CLOCK_INIT {100000}
-#  elif (PX4_NUMBER_I2C_BUSES) == 2
-#    define PX4_I2C_BUS_CLOCK_INIT {100000, 100000}
-#  elif (PX4_NUMBER_I2C_BUSES) == 3
-#    define PX4_I2C_BUS_CLOCK_INIT {100000, 100000, 100000}
-#  elif (PX4_NUMBER_I2C_BUSES) == 4
-#    define PX4_I2C_BUS_CLOCK_INIT {100000, 100000, 100000, 100000}
-#  else
-#    error PX4_NUMBER_I2C_BUSES not supported
-#  endif
-#endif
-
 #ifdef BOARD_SPI_BUS_MAX_BUS_ITEMS
 #define SPI_BUS_MAX_BUS_ITEMS BOARD_SPI_BUS_MAX_BUS_ITEMS
 #else
@@ -40,8 +40,9 @@
 #define I2C_BUS_MAX_BUS_ITEMS PX4_NUMBER_I2C_BUSES

 struct px4_i2c_bus_t {
-	int bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
-	bool is_external; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
+	uint32_t max_speed_hz{100'000}; ///< max speed in Hz
+	int8_t bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
+	bool is_external{true}; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
 };

 __EXPORT extern const px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS]; ///< board-specific I2C bus configuration
@@ -58,6 +59,11 @@ __EXPORT bool px4_i2c_bus_external(int bus);
 */
 __EXPORT bool px4_i2c_device_external(const uint32_t device_id);

+/**
+ * Return board defined max speed per bus in Hz. 0 if unknown or invalid bus.
+ */
+__EXPORT int px4_i2c_bus_max_speed(int bus);
+
 /**
 * @class I2CBusIterator
 * Iterate over configured I2C buses by the board
@@ -80,7 +86,7 @@ public:

 	bool next();

-	const px4_i2c_bus_t &bus() const { return px4_i2c_buses[_index]; }
+	int bus() const { return px4_i2c_buses[_index].bus; }

 	int externalBusIndex() const { return _external_bus_counter; }

@@ -56,6 +56,9 @@
 # include <drivers/device/spi.h>
 #endif // CONFIG_SPI

+
+__EXPORT void px4_print_all_instances();
+
 enum class I2CSPIBusOption : uint8_t {
 	All = 0, ///< select all runnning instances
 #if defined(CONFIG_I2C)
@@ -114,6 +117,15 @@ public:
 #if defined(CONFIG_I2C)
 	virtual int8_t  get_i2c_address() {return _i2c_address;}
 #endif // CONFIG_I2C
+
+	const char *module_name() const { return _module_name; }
+	const int &bus() const { return _bus; }
+	const I2CSPIBusOption &bus_option() const { return _bus_option; }
+	const uint16_t &devid_driver_index() const { return _devid_driver_index; }
+	const int8_t &bus_device_index() const { return _bus_device_index; }
+
+	// I2C address?
+
 private:
 	I2CSPIInstance(const I2CSPIDriverConfig &config)
 		: _module_name(config.module_name), _bus_option(config.bus_option), _bus(config.bus),
@@ -295,6 +307,7 @@ protected:
 	virtual ~I2CSPIDriverBase() = default;

 	virtual void print_status();
+	virtual void print_run_status();

 	virtual void custom_method(const BusCLIArguments &cli) {}

@@ -356,6 +369,7 @@ private:
 	{
 		template <typename C>
 		static constexpr I2CSPIDriverBase::instantiate_method get(decltype(&C::instantiate)) { return &C::instantiate; }
+
 		template <typename C>
 		static constexpr I2CSPIDriverBase::instantiate_method get(...) { return &C::instantiate_default; }
 	public:
@@ -153,14 +153,15 @@ public:

 	bool next();

-	const px4_spi_bus_t &bus() const { return px4_spi_buses[_index]; }
+	//const px4_spi_bus_t &bus() const { return px4_spi_buses[_index]; }
+	int bus() const { return px4_spi_buses[_index].bus; }
 	spi_drdy_gpio_t DRDYGPIO() const { return px4_spi_buses[_index].devices[_bus_device_index].drdy_gpio; }

 	uint32_t devid() const { return px4_spi_buses[_index].devices[_bus_device_index].devid; }

 	int externalBusIndex() const { return _external_bus_counter; }

-	bool external() const { return px4_spi_bus_external(bus()); }
+	bool external() const { return px4_spi_bus_external(px4_spi_buses[_index]); }

 	int busDeviceIndex() const { return _bus_device_index; }

@@ -75,7 +75,7 @@ void ScheduledWorkItem::ScheduleClear()
 void ScheduledWorkItem::print_run_status()
 {
 	if (_call.period > 0) {
-		PX4_INFO_RAW("%-29s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(),
+		PX4_INFO_RAW("%-34s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(),
 			     (double)average_interval(), _call.period);

 	} else {
@@ -134,7 +134,7 @@ float WorkItem::average_interval() const

 void WorkItem::print_run_status()
 {
-	PX4_INFO_RAW("%-29s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval());
+	PX4_INFO_RAW("%-34s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval());

 	// reset statistics
 	_run_count = 0;
@@ -455,7 +455,7 @@ WorkQueueManagerStatus()
 	if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) {

 		const size_t num_wqs = _wq_manager_wqs_list->size();
-		PX4_INFO_RAW("\nWork Queue: %-2zu threads                          RATE        INTERVAL\n", num_wqs);
+		PX4_INFO_RAW("\nWork Queue: %-2zu threads                               RATE        INTERVAL\n", num_wqs);

 		LockGuard lg{_wq_manager_wqs_list->mutex()};
 		size_t i = 0;
@@ -139,7 +139,7 @@ int px4_platform_init()
 	I2CBusIterator i2c_bus_iterator {I2CBusIterator::FilterType::All};

 	while (i2c_bus_iterator.next()) {
-		i2c_master_s *i2c_dev = px4_i2cbus_initialize(i2c_bus_iterator.bus().bus);
+		i2c_master_s *i2c_dev = px4_i2cbus_initialize(i2c_bus_iterator.bus());

 #if defined(CONFIG_I2C_RESET)
 		I2C_RESET(i2c_dev);
@@ -38,17 +38,19 @@

 #if defined(CONFIG_I2C)

-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }

-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
@@ -38,17 +38,19 @@

 #if defined(CONFIG_I2C)

-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }

-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
@@ -37,17 +37,19 @@

 #if defined(CONFIG_I2C)

-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }

-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
@@ -37,17 +37,19 @@

 #if defined(CONFIG_I2C)

-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }

-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
@@ -36,19 +36,23 @@
 #include <px4_arch/hw_description.h>
 #include <px4_platform_common/i2c.h>

+#if defined(CONFIG_I2C)

-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }

-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
 }
+#endif // CONFIG_I2C
@@ -38,17 +38,19 @@

 #if defined(CONFIG_I2C)

-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }

-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
@@ -37,17 +37,19 @@

 #if defined(CONFIG_I2C)

-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }

-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
@@ -35,7 +35,7 @@ set(HEXAGON_SDK_INCLUDES
 	${HEXAGON_SDK_ROOT}/incs
 	${HEXAGON_SDK_ROOT}/incs/stddef
 	${HEXAGON_SDK_ROOT}/rtos/qurt/computev66/include/qurt
-	# ${HEXAGON_SDK_ROOT}/rtos/qurt/computev66/include/posix
+	${HEXAGON_SDK_ROOT}/rtos/qurt/computev66/include/posix
 	${HEXAGON_SDK_ROOT}/tools/HEXAGON_Tools/8.4.05/Tools/target/hexagon/include
 	)

@@ -100,7 +100,6 @@ set(ARCHCPUFLAGS
 add_definitions(
 	-D __QURT
 	-D _PROVIDE_POSIX_TIME_DECLS
-	-D _TIMER_T
 	-D _HAS_C9X
 	-D restrict=__restrict__
 	-D noreturn_function=
@@ -966,8 +966,8 @@ void ModalaiEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
 					     float)_parameters.turtle_motor_percent / 100.f);

 		// Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
-		float dead_band_rpm = ((float)_parameters.turtle_motor_deadband / 100.0f) * _rpm_fullscale;
-		motor_output = (motor_output < _rpm_turtle_min + dead_band_rpm) ? 0.0f : (motor_output -  dead_band_rpm);
+		motor_output = (motor_output < _rpm_turtle_min + _parameters.turtle_motor_deadband) ? 0.0f :
+			       (motor_output - _parameters.turtle_motor_deadband);

 		// using the output map here for clarity as PX4 motors are 1-4
 		switch (_output_map[i].number) {
@@ -253,6 +253,18 @@ MS5611::collect()
 		return ret;
 	}

+	// According to the sensor docs:
+	// If the conversion is not executed before the ADC read command, or the
+	// ADC read command is repeated, it will give 0 as the output result.
+	//
+	// We have seen 0 during the init phase on I2C, therefore, we add this
+	// protection in.
+	if (raw == 0) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
 	/* handle a measurement */
 	if (_measure_phase == 0) {

@@ -54,6 +54,7 @@ extern "C" __EXPORT int gy_us42_main(int argc, char *argv[])
 	BusCLIArguments cli{true, false};
 	cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	cli.default_i2c_frequency = 100000;
+	cli.i2c_address = GY_US42_BASEADDR;

 	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
 		switch (ch) {
@@ -38,7 +38,7 @@

 void MPU9250_I2C::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("mpu9520_i2c", "driver");
+	PRINT_MODULE_USAGE_NAME("mpu9250_i2c", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -38,7 +38,7 @@

 void MPU9250::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("mpu9520", "driver");
+	PRINT_MODULE_USAGE_NAME("mpu9250", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
@@ -90,6 +90,7 @@ add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz
 		--version-file ${PX4_BINARY_DIR}/src/lib/version/build_git_version.h
 	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_crc.py
 		${component_general_json}
+		${PX4_BINARY_DIR}/events/all_events.json.xz
 		--output ${component_information_header}
 	DEPENDS
 		generate_component_general.py
@@ -74,7 +74,7 @@ for metadata_type_tuple in args.type:
 component_general['metadataTypes'] = metadata_types

 with open(filename, 'w') as outfile:
-    json.dump(component_general, outfile)
+    json.dump(component_general, outfile, sort_keys=True)

 if compress:
    save_compressed(filename)
@@ -2,6 +2,7 @@

 import argparse
 import os
+import hashlib

 parser = argparse.ArgumentParser(description="""Generate the COMPONENT_INFORMATION checksums (CRC32) header file""")
 parser.add_argument('--output', metavar='checksums.h', help='output file')
@@ -30,6 +31,16 @@ def crc_update(buf, crc_table, crc):

 crc_table = create_table()

+def sha256sum(filename):
+    h  = hashlib.sha256()
+    b  = bytearray(128*1024)
+    mv = memoryview(b)
+    with open(filename, 'rb', buffering=0) as f:
+        for n in iter(lambda : f.readinto(mv), 0):
+            h.update(mv[:n])
+    return h.hexdigest()
+
+
 with open(filename, 'w') as outfile:
    outfile.write("#pragma once\n")
    outfile.write("#include <stdint.h>\n")
@@ -38,10 +49,12 @@ with open(filename, 'w') as outfile:
        file_crc = 0
        for line in open(filename, "rb"):
            file_crc = crc_update(line, crc_table, file_crc)
+        file_sha256 = sha256sum(filename)

        basename = os.path.basename(filename)
        identifier = basename.split('.')[0]
        outfile.write("static constexpr uint32_t {:}_crc = {:};\n".format(identifier, file_crc))
+        outfile.write("static constexpr const char *{:}_sha256 = \"{:}\";\n".format(identifier, file_sha256))


    outfile.write("}\n")
@@ -169,7 +169,7 @@ public:

 	union DeviceId {
 		struct DeviceStructure devid_s;
-		uint32_t devid;
+		uint32_t devid{0};
 	};

 	uint32_t get_device_id() const { return _device_id.devid; }
@@ -268,8 +268,8 @@ protected:
 	Device(uint8_t devtype, const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address) : _name(name)
 	{
 		set_device_type(devtype);
-		_device_id.devid_s.bus_type = bus_type;
-		_device_id.devid_s.bus = bus;
+		set_device_bus_type(bus_type);
+		set_device_bus(bus);
 		set_device_address(address);
 	}

@@ -49,12 +49,6 @@

 namespace device
 {
-/*
- *  N.B. By defaulting the value of _bus_clocks to non Zero
- *  All calls to init() will NOT set the buss frequency
- */
-
-unsigned int I2C::_bus_clocks[PX4_NUMBER_I2C_BUSES] = PX4_I2C_BUS_CLOCK_INIT;

 I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) :
 	CDev(name, nullptr),
@@ -80,98 +74,50 @@ I2C::~I2C()
 	}
 }

-int
-I2C::set_bus_clock(unsigned bus, unsigned clock_hz)
-{
-	int index = bus - 1;
-
-	if (index < 0 || index >= static_cast<int>(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) {
-		return -EINVAL;
-	}
-
-	if (_bus_clocks[index] > 0) {
-		// DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz);
-	}
-
-	_bus_clocks[index] = clock_hz;
-
-	return OK;
-}
-
 int
 I2C::init()
 {
-	int ret = PX4_ERROR;
-	unsigned bus_index;
-
 	// attach to the i2c bus
 	_dev = px4_i2cbus_initialize(get_device_bus());

 	if (_dev == nullptr) {
 		DEVICE_DEBUG("failed to init I2C");
-		ret = -ENOENT;
-		goto out;
+		return -ENOENT;
 	}

-	// the above call fails for a non-existing bus index,
-	// so the index math here is safe.
-	bus_index = get_device_bus() - 1;
+	const uint32_t max_speed_hz = px4_i2c_bus_max_speed(get_device_bus());

-	// abort if the max frequency we allow (the frequency we ask)
-	// is smaller than the bus frequency
-	if (_bus_clocks[bus_index] > _frequency) {
-		(void)px4_i2cbus_uninitialize(_dev);
-		_dev = nullptr;
-		DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %" PRIu32 " KHz)",
-			   get_device_bus(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
-		ret = -EINVAL;
-		goto out;
-	}
-
-	// set frequency for this instance once to the bus speed
-	// the bus speed is the maximum supported by all devices on the bus,
-	// as we have to prioritize performance over compatibility.
-	// If a new device requires a lower clock speed, this has to be
-	// manually set via "fmu i2c <bus> <clock>" before starting any
-	// drivers.
-	// This is necessary as automatically lowering the bus speed
-	// for maximum compatibility could induce timing issues on
-	// critical sensors the adopter might be unaware of.
-
-	// set the bus frequency on the first access if it has
-	// not been set yet
-	if (_bus_clocks[bus_index] == 0) {
-		_bus_clocks[bus_index] = _frequency;
+	if (_frequency > max_speed_hz) {
+		DEVICE_DEBUG("frequency %" PRIu32 " Hz exceeds bus %d maximum, limited to max %" PRIu32 " Hz", _frequency,
+			     get_device_bus(), max_speed_hz);
+		_frequency = max_speed_hz;
 	}

 	// call the probe function to check whether the device is present
-	ret = probe();
+	int probe_ret = probe();

-	if (ret != OK) {
+	if (probe_ret != OK) {
 		DEVICE_DEBUG("probe failed");
-		goto out;
+		px4_i2cbus_uninitialize(_dev);
+		_dev = nullptr;
+		return probe_ret;
 	}

 	// do base class init, which will create device node, etc
-	ret = CDev::init();
+	int cdev_init_ret = CDev::init();

-	if (ret != OK) {
+	if (cdev_init_ret != OK) {
 		DEVICE_DEBUG("cdev init failed");
-		goto out;
+		px4_i2cbus_uninitialize(_dev);
+		_dev = nullptr;
+		return cdev_init_ret;
 	}

 	// tell the world where we are
-	DEVICE_DEBUG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %" PRIu32 " KHz)",
-		     get_device_bus(), get_device_address(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
+	DEVICE_DEBUG("on I2C bus %d at 0x%02x (bus: %" PRIu32 " KHz)",
+		     get_device_bus(), get_device_address(), _frequency / 1000);

-out:
-
-	if ((ret != OK) && (_dev != nullptr)) {
-		px4_i2cbus_uninitialize(_dev);
-		_dev = nullptr;
-	}
-
-	return ret;
+	return PX4_OK;
 }

 int
@@ -192,7 +138,7 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const
 		unsigned msgs = 0;

 		if (send_len > 0) {
-			msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
+			msgv[msgs].frequency = _frequency;
 			msgv[msgs].addr = get_device_address();
 			msgv[msgs].flags = 0;
 			msgv[msgs].buffer = const_cast<uint8_t *>(send);
@@ -201,7 +147,7 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const
 		}

 		if (recv_len > 0) {
-			msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
+			msgv[msgs].frequency = _frequency;
 			msgv[msgs].addr = get_device_address();
 			msgv[msgs].flags = I2C_M_READ;
 			msgv[msgs].buffer = recv;
@@ -67,8 +67,6 @@ public:

 	virtual int	init() override;

-	static int	set_bus_clock(unsigned bus, unsigned clock_hz);
-
 protected:
 	/**
 	 * The number of times a read or write operation will be retried on
@@ -111,9 +109,7 @@ protected:
 	bool	external() const override { return px4_i2c_device_external(_device_id.devid); }

 private:
-	static unsigned	int	_bus_clocks[PX4_NUMBER_I2C_BUSES];
-
-	const uint32_t		_frequency;
+	uint32_t		_frequency{100'000};
 	i2c_master_s		*_dev{nullptr};

 };
@@ -114,7 +114,7 @@ class JsonOutput():


        #Json string output.
-        self.output = json.dumps(all_json,indent=2)
+        self.output = json.dumps(all_json, indent=2, sort_keys=True)


    def Save(self, filename):
@@ -4,7 +4,6 @@ Param source code generation script.
 """
 from __future__ import print_function
 import xml.etree.ElementTree as ET
-import codecs
 import argparse
 import sys

@@ -50,9 +50,7 @@ import argparse
 from px4params import srcscanner, srcparser, injectxmlparams, xmlout, markdownout, jsonout

 import lzma #to create .xz file
-import re
 import json
-import codecs

 def save_compressed(filename):
    #create lzma compressed version
@@ -685,7 +685,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 {
 	if (!forced) {
 		const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
-				     || is_ground_rover(_vehicle_status));
+				     || is_ground_vehicle(_vehicle_status));
 		const bool mc_manual_thrust_mode = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 						   && _vehicle_control_mode.flag_control_manual_enabled
 						   && !_vehicle_control_mode.flag_control_climb_rate_enabled;
@@ -3022,6 +3022,23 @@ void Commander::data_link_check()
 				_vehicle_status.parachute_system_healthy = healthy;
 			}

+			if (telemetry.heartbeat_type_open_drone_id) {
+				if (_open_drone_id_system_lost) {
+					_open_drone_id_system_lost = false;
+
+					if (_datalink_last_heartbeat_open_drone_id_system != 0) {
+						mavlink_log_info(&_mavlink_log_pub, "OpenDroneID system regained\t");
+						events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "OpenDroneID system regained");
+					}
+				}
+
+				bool healthy = telemetry.open_drone_id_system_healthy;
+
+				_datalink_last_heartbeat_open_drone_id_system = telemetry.timestamp;
+				_vehicle_status.open_drone_id_system_present = true;
+				_vehicle_status.open_drone_id_system_healthy = healthy;
+			}
+
 			if (telemetry.heartbeat_component_obstacle_avoidance) {
 				if (_avoidance_system_lost) {
 					_avoidance_system_lost = false;
@@ -3072,6 +3089,17 @@ void Commander::data_link_check()
 		_status_changed = true;
 	}

+	// OpenDroneID system
+	if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s)
+	    && !_open_drone_id_system_lost) {
+		mavlink_log_critical(&_mavlink_log_pub, "OpenDroneID system lost");
+		events::send(events::ID("commander_open_drone_id_lost"), events::Log::Critical, "OpenDroneID system lost");
+		_vehicle_status.open_drone_id_system_present = false;
+		_vehicle_status.open_drone_id_system_healthy = false;
+		_open_drone_id_system_lost = true;
+		_status_changed = true;
+	}
+
 	// AVOIDANCE SYSTEM state check (only if it is enabled)
 	if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) {
 		// if heartbeats stop
@@ -296,9 +296,11 @@ private:
 	hrt_abstime	_datalink_last_heartbeat_avoidance_system{0};
 	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
 	hrt_abstime	_datalink_last_heartbeat_parachute_system{0};
+	hrt_abstime	_datalink_last_heartbeat_open_drone_id_system{0};
 	bool		_onboard_controller_lost{false};
 	bool		_avoidance_system_lost{false};
 	bool		_parachute_system_lost{true};
+	bool		_open_drone_id_system_lost{true};

 	hrt_abstime	_high_latency_datalink_heartbeat{0};
 	hrt_abstime	_high_latency_datalink_lost{0};
@@ -123,6 +123,16 @@ bool is_ground_rover(const vehicle_status_s &current_status)
 	return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
 }

+bool is_boat(const vehicle_status_s &current_status)
+{
+	return current_status.system_type == VEHICLE_TYPE_BOAT;
+}
+
+bool is_ground_vehicle(const vehicle_status_s &current_status)
+{
+	return is_ground_rover(current_status) || is_boat(current_status);
+}
+
 // End time for currently blinking LED message, 0 if no blink message
 static hrt_abstime blink_msg_end = 0;
 static int fd_leds{-1};
@@ -56,6 +56,8 @@ bool is_vtol(const vehicle_status_s &current_status);
 bool is_vtol_tailsitter(const vehicle_status_s &current_status);
 bool is_fixed_wing(const vehicle_status_s &current_status);
 bool is_ground_rover(const vehicle_status_s &current_status);
+bool is_boat(const vehicle_status_s &current_status);
+bool is_ground_vehicle(const vehicle_status_s &current_status);

 int buzzer_init(void);
 void buzzer_deinit(void);
@@ -143,7 +143,7 @@ public:
 	 * @return Control vector
 	 */
 	matrix::Vector<float, NUM_AXES> getAllocatedControl() const
-	{ return (_effectiveness * _actuator_sp).emult(_control_allocation_scale); }
+	{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }

 	/**
 	 * Get the control effectiveness matrix
@@ -33,6 +33,36 @@

 add_subdirectory(Utility)

+# Symforce code generation
+execute_process(
+    COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
+    RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
+    OUTPUT_QUIET
+)
+if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)
+
+	set(EKF_DERIVATION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation)
+
+	set(EKF_GENERATED_SRC_FILES
+		${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h
+		${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h
+	)
+
+	add_custom_command(
+		OUTPUT ${EKF_GENERATED_SRC_FILES}
+		COMMAND ${PYTHON_EXECUTABLE} derivation.py
+		DEPENDS
+			${EKF_DERIVATION_DIR}/derivation.py
+			${EKF_DERIVATION_DIR}/derivation_utils.py
+
+		WORKING_DIRECTORY ${EKF_DERIVATION_DIR}
+		COMMENT "Symforce code generation"
+		USES_TERMINAL
+	)
+
+	add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES})
+endif()
+
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -43,6 +43,10 @@
 */

 #include "ekf.h"
+
+#include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h"
+#include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h"
+
 #include <mathlib/mathlib.h>

 void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const
@@ -50,38 +54,17 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
 	// reset flags
 	resetEstimatorAidStatusFlags(airspeed);

-	const float vn = _state.vel(0); // Velocity in north direction
-	const float ve = _state.vel(1); // Velocity in east direction
-	const float vd = _state.vel(2); // Velocity in downwards direction
-	const float vwn = _state.wind_vel(0); // Wind speed in north direction
-	const float vwe = _state.wind_vel(1); // Wind speed in east direction
-
 	// Variance for true airspeed measurement - (m/sec)^2
-	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
-			       math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
+	const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
+			   math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));

-	// Intermediate variables
-	const float IV0 = ve - vwe;
-	const float IV1 = vn - vwn;
-	const float IV2 = (IV0)*(IV0) + (IV1)*(IV1) + (vd)*(vd);
-
-	const float predicted_airspeed = sqrtf(IV2);
-
-	if (fabsf(predicted_airspeed) < FLT_EPSILON) {
-		return;
-	}
-
-	const float IV3 = 1.0F/(IV2);
-	const float IV4 = IV0*P(5,23);
-	const float IV5 = IV0*IV3;
-	const float IV6 = IV1*P(4,22);
-	const float IV7 = IV1*IV3;
-
-	const float innov_var = IV3*vd*(IV0*P(5,6) - IV0*P(6,23) + IV1*P(4,6) - IV1*P(6,22) + P(6,6)*vd) - IV5*(-IV0*P(23,23) - IV1*P(22,23) + IV1*P(4,23) + IV4 + P(6,23)*vd) + IV5*(IV0*P(5,5) + IV1*P(4,5) - IV1*P(5,22) - IV4 + P(5,6)*vd) - IV7*(-IV0*P(22,23) + IV0*P(5,22) - IV1*P(22,22) + IV6 + P(6,22)*vd) + IV7*(-IV0*P(4,23) + IV0*P(4,5) + IV1*P(4,4) - IV6 + P(4,6)*vd) + R_TAS;
+	float innov = 0.f;
+	float innov_var = 0.f;
+	sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);

 	airspeed.observation = airspeed_sample.true_airspeed;
-	airspeed.observation_variance = R_TAS;
-	airspeed.innovation = predicted_airspeed - airspeed.observation;
+	airspeed.observation_variance = R;
+	airspeed.innovation = innov;
 	airspeed.innovation_variance = innov_var;

 	airspeed.fusion_enabled = _control_status.flags.fuse_aspd;
@@ -98,34 +81,9 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed)
 		return;
 	}

-	const float vn = _state.vel(0); // Velocity in north direction
-	const float ve = _state.vel(1); // Velocity in east direction
-	const float vd = _state.vel(2); // Velocity in downwards direction
-	const float vwn = _state.wind_vel(0); // Wind speed in north direction
-	const float vwe = _state.wind_vel(1); // Wind speed in east direction
-
 	// determine if we need the airspeed fusion to correct states other than wind
 	const bool update_wind_only = !_control_status.flags.wind_dead_reckoning;

-	// Intermediate variables
-	const float HK0 = vn - vwn;
-	const float HK1 = ve - vwe;
-	const float HK2 = sqrtf((HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd));
-
-	const float predicted_airspeed = HK2;
-
-	if (predicted_airspeed < 1.0f) {
-		// calculation can be badly conditioned for very low airspeed values so don't fuse this time
-		return;
-	}
-
-	const float HK3 = 1.0F/(HK2);
-	const float HK4 = HK0*HK3;
-	const float HK5 = HK1*HK3;
-	const float HK6 = HK3*vd;
-	const float HK7 = -HK0*HK3;
-	const float HK8 = -HK1*HK3;
-
 	const float innov_var = airspeed.innovation_variance;

 	if (innov_var < airspeed.observation_variance || innov_var < FLT_EPSILON) {
@@ -150,31 +108,22 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed)
 		return;
 	}

-	const float HK9 = 1.0F/(innov_var);
-
 	_fault_status.flags.bad_airspeed = false;

-	// Observation Jacobians
-	SparseVector24f<4,5,6,22,23> Hfusion;
-	Hfusion.at<4>() = HK4;
-	Hfusion.at<5>() = HK5;
-	Hfusion.at<6>() = HK6;
-	Hfusion.at<22>() = HK7;
-	Hfusion.at<23>() = HK8;
+	Vector24f H; // Observation jacobian
+	Vector24f K; // Kalman gain vector

-	Vector24f Kfusion; // Kalman gain vector
+	sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);

-	if (!update_wind_only) {
-		// we have no other source of aiding, so use airspeed measurements to correct states
+	SparseVector24f<4,5,6,22,23> H_sparse(H);
+
+	if (update_wind_only) {
 		for (unsigned row = 0; row <= 21; row++) {
-			Kfusion(row) = HK9*(HK4*P(row,4) + HK5*P(row,5) + HK6*P(row,6) + HK7*P(row,22) + HK8*P(row,23));
+			K(row) = 0.f;
 		}
 	}

-	Kfusion(22) = HK9*(HK4*P(4,22) + HK5*P(5,22) + HK6*P(6,22) + HK7*P(22,22) + HK8*P(22,23));
-	Kfusion(23) = HK9*(HK4*P(4,23) + HK5*P(5,23) + HK6*P(6,23) + HK7*P(22,23) + HK8*P(23,23));
-
-	const bool is_fused = measurementUpdate(Kfusion, Hfusion, airspeed.innovation);
+	const bool is_fused = measurementUpdate(K, H_sparse, airspeed.innovation);

 	airspeed.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;
@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+    Copyright (c) 2022 PX4 Development Team
+    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: derivation.py
+Description:
+"""
+
+import symforce.symbolic as sf
+from derivation_utils import *
+
+class State:
+    qw = 0
+    qx = 1
+    qy = 2
+    qz = 3
+    vx = 4
+    vy = 5
+    vz = 6
+    px = 7
+    py = 8
+    pz = 9
+    d_ang_bx = 10
+    d_ang_by = 11
+    d_ang_bz = 12
+    d_vel_bx = 13
+    d_vel_by = 14
+    d_vel_bz = 15
+    ix = 16
+    iy = 17
+    iz = 18
+    ibx = 19
+    iby = 20
+    ibz = 21
+    wx = 22
+    wy = 23
+    n_states = 24
+
+class VState(sf.Matrix):
+    SHAPE = (State.n_states, 1)
+
+class MState(sf.Matrix):
+    SHAPE = (State.n_states, State.n_states)
+
+def compute_airspeed_innov_and_innov_var(
+        state: VState,
+        P: MState,
+        airspeed: sf.Scalar,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.Scalar, sf.Scalar):
+
+    vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
+    airspeed_pred = vel_rel.norm(epsilon=epsilon)
+
+    innov = airspeed_pred - airspeed
+
+    H = sf.V1(airspeed_pred).jacobian(state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov, innov_var)
+
+def compute_airspeed_h_and_k(
+        state: VState,
+        P: MState,
+        innov_var: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (VState, VState):
+
+    vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
+    airspeed_pred = vel_rel.norm(epsilon=epsilon)
+    H = sf.V1(airspeed_pred).jacobian(state)
+
+    K = P * H.T / sm.Max(innov_var, epsilon)
+
+    return (H.T, K)
+
+generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
+generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
@@ -0,0 +1,101 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+    Copyright (c) 2022 PX4 Development Team
+    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: derivation_utils.py
+Description:
+    Common functions used for the derivation of most estimators
+"""
+
+from symforce import symbolic as sm
+from symforce import geo
+from symforce import typing as T
+
+# q: quaternion describing rotation from frame 1 to frame 2
+# returns a rotation matrix derived form q which describes the same
+# rotation
+def quat_to_rot(q):
+    q0 = q[0]
+    q1 = q[1]
+    q2 = q[2]
+    q3 = q[3]
+
+    Rot = geo.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
+                  [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
+                   [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
+
+    return Rot
+
+def sign_no_zero(x) -> T.Scalar:
+    """
+    Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero
+    """
+    return 2 * sm.Min(sm.sign(x), 0) + 1
+
+def add_epsilon_sign(expr, var, eps):
+    # Avoids a singularity at 0 while keeping the derivative correct
+    return expr.subs(var, var + eps * sign_no_zero(var))
+
+def generate_px4_function(function_name, output_names):
+    from symforce.codegen import Codegen, CppConfig
+    import os
+    import fileinput
+
+    codegen = Codegen.function(
+            function_name,
+            output_names=output_names,
+            config=CppConfig())
+    metadata = codegen.generate_function(
+            output_dir="generated",
+            skip_directory_nesting=True)
+
+    print("Files generated in {}:\n".format(metadata.output_dir))
+    for f in metadata.generated_files:
+        print("  |- {}".format(os.path.relpath(f, metadata.output_dir)))
+
+    # Replace cstdlib and Eigen functions by PX4 equivalents
+    with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True, backup='.bak') as file:
+        for line in file:
+            line = line.replace("std::max", "math::max")
+            line = line.replace("std::min", "math::min")
+            line = line.replace("Eigen", "matrix")
+            line = line.replace("matrix/Dense", "matrix/math.hpp")
+            print(line, end='')
+
+def generate_python_function(function_name, output_names):
+    from symforce.codegen import Codegen, PythonConfig
+    codegen = Codegen.function(
+            function_name,
+            output_names=output_names,
+            config=PythonConfig())
+
+    metadata = codegen.generate_function(
+            output_dir="generated",
+            skip_directory_nesting=True)
@@ -0,0 +1 @@
+*.bak
@@ -0,0 +1,116 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_airspeed_h_and_k
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     innov_var: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     H: Matrix24_1
+ *     K: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
+                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
+  // Total ops: 256
+
+  // Input arrays
+
+  // Intermediate terms (7)
+  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp1 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
+                                       epsilon + std::pow(state(6, 0), Scalar(2))),
+                                Scalar(Scalar(-1) / Scalar(2)));
+  const Scalar _tmp3 = _tmp1 * _tmp2;
+  const Scalar _tmp4 = _tmp0 * _tmp2;
+  const Scalar _tmp5 = _tmp2 * state(6, 0);
+  const Scalar _tmp6 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
+
+  // Output terms (2)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
+
+    _H.setZero();
+
+    _H(4, 0) = _tmp3;
+    _H(5, 0) = _tmp4;
+    _H(6, 0) = _tmp5;
+    _H(22, 0) = -_tmp3;
+    _H(23, 0) = -_tmp4;
+  }
+
+  if (K != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _K = (*K);
+
+    _K(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 +
+                        P(0, 6) * _tmp5);
+    _K(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 +
+                        P(1, 6) * _tmp5);
+    _K(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 +
+                        P(2, 6) * _tmp5);
+    _K(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 +
+                        P(3, 6) * _tmp5);
+    _K(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 +
+                        P(4, 6) * _tmp5);
+    _K(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 +
+                        P(5, 6) * _tmp5);
+    _K(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 +
+                        P(6, 6) * _tmp5);
+    _K(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 +
+                        P(7, 6) * _tmp5);
+    _K(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 +
+                        P(8, 6) * _tmp5);
+    _K(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 +
+                        P(9, 6) * _tmp5);
+    _K(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 +
+                         P(10, 5) * _tmp4 + P(10, 6) * _tmp5);
+    _K(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 +
+                         P(11, 5) * _tmp4 + P(11, 6) * _tmp5);
+    _K(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 +
+                         P(12, 5) * _tmp4 + P(12, 6) * _tmp5);
+    _K(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 +
+                         P(13, 5) * _tmp4 + P(13, 6) * _tmp5);
+    _K(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 +
+                         P(14, 5) * _tmp4 + P(14, 6) * _tmp5);
+    _K(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 +
+                         P(15, 5) * _tmp4 + P(15, 6) * _tmp5);
+    _K(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 +
+                         P(16, 5) * _tmp4 + P(16, 6) * _tmp5);
+    _K(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 +
+                         P(17, 5) * _tmp4 + P(17, 6) * _tmp5);
+    _K(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 +
+                         P(18, 5) * _tmp4 + P(18, 6) * _tmp5);
+    _K(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 +
+                         P(19, 5) * _tmp4 + P(19, 6) * _tmp5);
+    _K(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 +
+                         P(20, 5) * _tmp4 + P(20, 6) * _tmp5);
+    _K(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 +
+                         P(21, 5) * _tmp4 + P(21, 6) * _tmp5);
+    _K(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 +
+                         P(22, 5) * _tmp4 + P(22, 6) * _tmp5);
+    _K(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 +
+                         P(23, 5) * _tmp4 + P(23, 6) * _tmp5);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -0,0 +1,74 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_airspeed_innov_and_innov_var
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     airspeed: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov: Scalar
+ *     innov_var: Scalar
+ */
+template <typename Scalar>
+void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
+                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
+                                     const Scalar R, const Scalar epsilon,
+                                     Scalar* const innov = nullptr,
+                                     Scalar* const innov_var = nullptr) {
+  // Total ops: 69
+
+  // Input arrays
+
+  // Intermediate terms (7)
+  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp1 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp2 = std::sqrt(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
+                                        epsilon + std::pow(state(6, 0), Scalar(2))));
+  const Scalar _tmp3 = Scalar(1.0) / (_tmp2);
+  const Scalar _tmp4 = _tmp3 * state(6, 0);
+  const Scalar _tmp5 = _tmp1 * _tmp3;
+  const Scalar _tmp6 = _tmp0 * _tmp3;
+
+  // Output terms (2)
+  if (innov != nullptr) {
+    Scalar& _innov = (*innov);
+
+    _innov = _tmp2 - airspeed;
+  }
+
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = R +
+                 _tmp4 * (-P(22, 6) * _tmp5 - P(23, 6) * _tmp6 + P(4, 6) * _tmp5 + P(5, 6) * _tmp6 +
+                          P(6, 6) * _tmp4) -
+                 _tmp5 * (-P(22, 22) * _tmp5 - P(23, 22) * _tmp6 + P(4, 22) * _tmp5 +
+                          P(5, 22) * _tmp6 + P(6, 22) * _tmp4) +
+                 _tmp5 * (-P(22, 4) * _tmp5 - P(23, 4) * _tmp6 + P(4, 4) * _tmp5 + P(5, 4) * _tmp6 +
+                          P(6, 4) * _tmp4) -
+                 _tmp6 * (-P(22, 23) * _tmp5 - P(23, 23) * _tmp6 + P(4, 23) * _tmp5 +
+                          P(5, 23) * _tmp6 + P(6, 23) * _tmp4) +
+                 _tmp6 * (-P(22, 5) * _tmp5 - P(23, 5) * _tmp6 + P(4, 5) * _tmp5 + P(5, 5) * _tmp6 +
+                          P(6, 5) * _tmp4);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -1,227 +0,0 @@
-#include <math.h>
-#include <stdio.h>
-#include <cstdlib>
-#include "../../../../../matrix/matrix/math.hpp"
-#include "util.h"
-
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
-template<int ... Idxs>
-using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
-
-int main()
-{
-    // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
-
-    float airspeed_innov_var;
-
-    Vector24f Kfusion; // Kalman gain vector
-
-    Vector24f Hfusion_sympy;
-    Vector24f Kfusion_sympy;
-
-    Vector24f Hfusion_matlab;
-    Vector24f Kfusion_matlab;
-
-    const float R_TAS = sq(1.5f);
-
-    const bool update_wind_only = false;
-
-    // get latest velocity in earth frame
-    const float vn = 9.0f;
-    const float ve = 12.0f;
-    const float vd = -1.5f;
-
-    // get latest wind velocity in earth frame
-    const float vwn = -4.0f;
-    const float vwe = 3.0f;
-
-    // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
-    for (int col=0; col<=23; col++) {
-        for (int row=0; row<=col; row++) {
-            if (row == col) {
-                P(row,col) = (float)rand();
-            } else {
-                P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
-            }
-        }
-    }
-
-    // First calculate observationjacobians and Kalman gains using sympy generated equations
-
-    {
-        // Intermediate variables
-        const float HK0 = vn - vwn;
-        const float HK1 = ve - vwe;
-        const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
-        const float v_tas_pred = sqrtf(HK2); // predicted airspeed
-        //const float HK3 = powf(HK2, -1.0F/2.0F);
-        if (v_tas_pred < 1.0f) {
-            // calculation can be badly conditioned for very low airspeed values so don't fuse this time
-            return 0;
-        }
-        const float HK3 = 1.0f / v_tas_pred;
-        const float HK4 = HK0*HK3;
-        const float HK5 = HK1*HK3;
-        const float HK6 = 1.0F/HK2;
-        const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd;
-        const float HK8 = HK1*P(5,23);
-        const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd;
-        const float HK10 = HK1*HK6;
-        const float HK11 = HK0*P(4,22);
-        const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd;
-        const float HK13 = HK0*HK6;
-        const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd;
-        const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd;
-        const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
-
-        // Observation Jacobians
-        SparseVector24f<4,5,6,22,23> Hfusion;
-        Hfusion.at<4>() = HK4;
-        Hfusion.at<5>() = HK5;
-        Hfusion.at<6>() = HK3*vd;
-        Hfusion.at<22>() = -HK4;
-        Hfusion.at<23>() = -HK5;
-
-        if (true) {
-            // we have no other source of aiding, so use airspeed measurements to correct states
-            for (unsigned row = 0; row <= 3; row++) {
-                Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
-            }
-
-            Kfusion(4) = HK12*HK16;
-            Kfusion(5) = HK16*HK9;
-            Kfusion(6) = HK16*HK7;
-
-            for (unsigned row = 7; row <= 21; row++) {
-                Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
-            }
-        }
-
-        Kfusion(22) = HK15*HK16;
-        Kfusion(23) = HK14*HK16;
-
-        // save output
-        Hfusion_sympy(4) = Hfusion.at<4>();
-        Hfusion_sympy(5) = Hfusion.at<5>();
-        Hfusion_sympy(6) = Hfusion.at<6>();
-        Hfusion_sympy(22) = Hfusion.at<22>();
-        Hfusion_sympy(23) = Hfusion.at<23>();
-        Kfusion_sympy = Kfusion;
-    }
-    // repeat calculation using matlab generated equations
-    {
-        const float v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
-
-        // intermediate variable from algebraic optimisation
-        float SH_TAS[3];
-        SH_TAS[0] = 1.0f/v_tas_pred;
-        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
-        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
-
-        // Observation Jacobian
-        Vector24f H_TAS = {};
-        H_TAS(4) = SH_TAS[2];
-        H_TAS(5) = SH_TAS[1];
-        H_TAS(6) = vd*SH_TAS[0];
-        H_TAS(22) = -SH_TAS[2];
-        H_TAS(23) = -SH_TAS[1];
-
-        airspeed_innov_var = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
-
-        float SK_TAS[2];
-        SK_TAS[0] = 1.0f / airspeed_innov_var;
-        SK_TAS[1] = SH_TAS[1];
-
-        // Kalman gain
-        Kfusion(0) = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]);
-        Kfusion(1) = SK_TAS[0]*(P(1,4)*SH_TAS[2] - P(1,22)*SH_TAS[2] + P(1,5)*SK_TAS[1] - P(1,23)*SK_TAS[1] + P(1,6)*vd*SH_TAS[0]);
-        Kfusion(2) = SK_TAS[0]*(P(2,4)*SH_TAS[2] - P(2,22)*SH_TAS[2] + P(2,5)*SK_TAS[1] - P(2,23)*SK_TAS[1] + P(2,6)*vd*SH_TAS[0]);
-        Kfusion(3) = SK_TAS[0]*(P(3,4)*SH_TAS[2] - P(3,22)*SH_TAS[2] + P(3,5)*SK_TAS[1] - P(3,23)*SK_TAS[1] + P(3,6)*vd*SH_TAS[0]);
-        Kfusion(4) = SK_TAS[0]*(P(4,4)*SH_TAS[2] - P(4,22)*SH_TAS[2] + P(4,5)*SK_TAS[1] - P(4,23)*SK_TAS[1] + P(4,6)*vd*SH_TAS[0]);
-        Kfusion(5) = SK_TAS[0]*(P(5,4)*SH_TAS[2] - P(5,22)*SH_TAS[2] + P(5,5)*SK_TAS[1] - P(5,23)*SK_TAS[1] + P(5,6)*vd*SH_TAS[0]);
-        Kfusion(6) = SK_TAS[0]*(P(6,4)*SH_TAS[2] - P(6,22)*SH_TAS[2] + P(6,5)*SK_TAS[1] - P(6,23)*SK_TAS[1] + P(6,6)*vd*SH_TAS[0]);
-        Kfusion(7) = SK_TAS[0]*(P(7,4)*SH_TAS[2] - P(7,22)*SH_TAS[2] + P(7,5)*SK_TAS[1] - P(7,23)*SK_TAS[1] + P(7,6)*vd*SH_TAS[0]);
-        Kfusion(8) = SK_TAS[0]*(P(8,4)*SH_TAS[2] - P(8,22)*SH_TAS[2] + P(8,5)*SK_TAS[1] - P(8,23)*SK_TAS[1] + P(8,6)*vd*SH_TAS[0]);
-        Kfusion(9) = SK_TAS[0]*(P(9,4)*SH_TAS[2] - P(9,22)*SH_TAS[2] + P(9,5)*SK_TAS[1] - P(9,23)*SK_TAS[1] + P(9,6)*vd*SH_TAS[0]);
-        Kfusion(10) = SK_TAS[0]*(P(10,4)*SH_TAS[2] - P(10,22)*SH_TAS[2] + P(10,5)*SK_TAS[1] - P(10,23)*SK_TAS[1] + P(10,6)*vd*SH_TAS[0]);
-        Kfusion(11) = SK_TAS[0]*(P(11,4)*SH_TAS[2] - P(11,22)*SH_TAS[2] + P(11,5)*SK_TAS[1] - P(11,23)*SK_TAS[1] + P(11,6)*vd*SH_TAS[0]);
-        Kfusion(12) = SK_TAS[0]*(P(12,4)*SH_TAS[2] - P(12,22)*SH_TAS[2] + P(12,5)*SK_TAS[1] - P(12,23)*SK_TAS[1] + P(12,6)*vd*SH_TAS[0]);
-        Kfusion(13) = SK_TAS[0]*(P(13,4)*SH_TAS[2] - P(13,22)*SH_TAS[2] + P(13,5)*SK_TAS[1] - P(13,23)*SK_TAS[1] + P(13,6)*vd*SH_TAS[0]);
-        Kfusion(14) = SK_TAS[0]*(P(14,4)*SH_TAS[2] - P(14,22)*SH_TAS[2] + P(14,5)*SK_TAS[1] - P(14,23)*SK_TAS[1] + P(14,6)*vd*SH_TAS[0]);
-        Kfusion(15) = SK_TAS[0]*(P(15,4)*SH_TAS[2] - P(15,22)*SH_TAS[2] + P(15,5)*SK_TAS[1] - P(15,23)*SK_TAS[1] + P(15,6)*vd*SH_TAS[0]);
-        Kfusion(16) = SK_TAS[0]*(P(16,4)*SH_TAS[2] - P(16,22)*SH_TAS[2] + P(16,5)*SK_TAS[1] - P(16,23)*SK_TAS[1] + P(16,6)*vd*SH_TAS[0]);
-        Kfusion(17) = SK_TAS[0]*(P(17,4)*SH_TAS[2] - P(17,22)*SH_TAS[2] + P(17,5)*SK_TAS[1] - P(17,23)*SK_TAS[1] + P(17,6)*vd*SH_TAS[0]);
-        Kfusion(18) = SK_TAS[0]*(P(18,4)*SH_TAS[2] - P(18,22)*SH_TAS[2] + P(18,5)*SK_TAS[1] - P(18,23)*SK_TAS[1] + P(18,6)*vd*SH_TAS[0]);
-        Kfusion(19) = SK_TAS[0]*(P(19,4)*SH_TAS[2] - P(19,22)*SH_TAS[2] + P(19,5)*SK_TAS[1] - P(19,23)*SK_TAS[1] + P(19,6)*vd*SH_TAS[0]);
-        Kfusion(20) = SK_TAS[0]*(P(20,4)*SH_TAS[2] - P(20,22)*SH_TAS[2] + P(20,5)*SK_TAS[1] - P(20,23)*SK_TAS[1] + P(20,6)*vd*SH_TAS[0]);
-        Kfusion(21) = SK_TAS[0]*(P(21,4)*SH_TAS[2] - P(21,22)*SH_TAS[2] + P(21,5)*SK_TAS[1] - P(21,23)*SK_TAS[1] + P(21,6)*vd*SH_TAS[0]);
-        Kfusion(22) = SK_TAS[0]*(P(22,4)*SH_TAS[2] - P(22,22)*SH_TAS[2] + P(22,5)*SK_TAS[1] - P(22,23)*SK_TAS[1] + P(22,6)*vd*SH_TAS[0]);
-        Kfusion(23) = SK_TAS[0]*(P(23,4)*SH_TAS[2] - P(23,22)*SH_TAS[2] + P(23,5)*SK_TAS[1] - P(23,23)*SK_TAS[1] + P(23,6)*vd*SH_TAS[0]);
-
-        // save output;
-        Hfusion_matlab = H_TAS;
-        Kfusion_matlab = Kfusion;
-    }
-
-    // find largest observation variance difference as a fraction of the matlab value
-    float max_diff_fraction = 0.0f;
-    int max_row;
-    float max_old, max_new;
-    for (int row=0; row<24; row++) {
-        float diff_fraction;
-        if (Hfusion_matlab(row) != 0.0f) {
-            diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
-        } else if (Hfusion_sympy(row) != 0.0f) {
-            diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
-        } else {
-            diff_fraction = 0.0f;
-        }
-        if (Hfusion_sympy(row) - Hfusion_matlab(row) != 0.0f) {
-            printf("new,old Hfusion(%i) = %e,%e\n",row,Hfusion_sympy(row),Hfusion_matlab(row));
-        }
-        if (diff_fraction > max_diff_fraction) {
-            max_diff_fraction = diff_fraction;
-            max_row = row;
-            max_old = Hfusion_matlab(row);
-            max_new = Hfusion_sympy(row);
-        }
-    }
-
-    if (max_diff_fraction > 1e-5f) {
-        printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
-    } else {
-        printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
-    }
-
-    // find largest Kalman gain difference as a fraction of the matlab value
-    max_diff_fraction = 0.0f;
-    for (int row=0; row<4; row++) {
-        float diff_fraction;
-        if (Kfusion(row) != 0.0f) {
-            diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
-        } else if (Kfusion_sympy(row) != 0.0f) {
-            diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
-        } else {
-            diff_fraction = 0.0f;
-        }
-        // if (Kfusion_sympy(row) - Kfusion(row) != 0.0f) {
-        //     printf("new,old Kfusion(%i) = %e,%e\n",row,Kfusion_sympy(row),Kfusion(row));
-        // }
-        if (diff_fraction > max_diff_fraction) {
-            max_diff_fraction = diff_fraction;
-            max_row = row;
-            max_old = Kfusion(row);
-            max_new = Kfusion_sympy(row);
-        }
-    }
-
-    if (max_diff_fraction > 1e-5f) {
-        printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
-    } else {
-        printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
-    }
-
-    return 0;
-}
@@ -1,61 +0,0 @@
-// Sub Expressions
-const float HK0 = vn - vwn;
-const float HK1 = ve - vwe;
-const float HK2 = (HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd);
-const float HK3 = 1.0F/sqrtf(HK2);
-const float HK4 = HK0*HK3;
-const float HK5 = HK1*HK3;
-const float HK6 = 1.0F/(HK2);
-const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd;
-const float HK8 = HK1*P(5,23);
-const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd;
-const float HK10 = HK1*HK6;
-const float HK11 = HK0*P(4,22);
-const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd;
-const float HK13 = HK0*HK6;
-const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd;
-const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd;
-const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
-
-
-// Observation Jacobians
-Hfusion.at<4>() = HK4;
-Hfusion.at<5>() = HK5;
-Hfusion.at<6>() = HK3*vd;
-Hfusion.at<22>() = -HK4;
-Hfusion.at<23>() = -HK5;
-
-
-// Kalman gains
-Kfusion(0) = HK16*(-HK0*P(0,22) + HK0*P(0,4) - HK1*P(0,23) + HK1*P(0,5) + P(0,6)*vd);
-Kfusion(1) = HK16*(-HK0*P(1,22) + HK0*P(1,4) - HK1*P(1,23) + HK1*P(1,5) + P(1,6)*vd);
-Kfusion(2) = HK16*(-HK0*P(2,22) + HK0*P(2,4) - HK1*P(2,23) + HK1*P(2,5) + P(2,6)*vd);
-Kfusion(3) = HK16*(-HK0*P(3,22) + HK0*P(3,4) - HK1*P(3,23) + HK1*P(3,5) + P(3,6)*vd);
-Kfusion(4) = HK12*HK16;
-Kfusion(5) = HK16*HK9;
-Kfusion(6) = HK16*HK7;
-Kfusion(7) = HK16*(HK0*P(4,7) - HK0*P(7,22) + HK1*P(5,7) - HK1*P(7,23) + P(6,7)*vd);
-Kfusion(8) = HK16*(HK0*P(4,8) - HK0*P(8,22) + HK1*P(5,8) - HK1*P(8,23) + P(6,8)*vd);
-Kfusion(9) = HK16*(HK0*P(4,9) - HK0*P(9,22) + HK1*P(5,9) - HK1*P(9,23) + P(6,9)*vd);
-Kfusion(10) = HK16*(-HK0*P(10,22) + HK0*P(4,10) - HK1*P(10,23) + HK1*P(5,10) + P(6,10)*vd);
-Kfusion(11) = HK16*(-HK0*P(11,22) + HK0*P(4,11) - HK1*P(11,23) + HK1*P(5,11) + P(6,11)*vd);
-Kfusion(12) = HK16*(-HK0*P(12,22) + HK0*P(4,12) - HK1*P(12,23) + HK1*P(5,12) + P(6,12)*vd);
-Kfusion(13) = HK16*(-HK0*P(13,22) + HK0*P(4,13) - HK1*P(13,23) + HK1*P(5,13) + P(6,13)*vd);
-Kfusion(14) = HK16*(-HK0*P(14,22) + HK0*P(4,14) - HK1*P(14,23) + HK1*P(5,14) + P(6,14)*vd);
-Kfusion(15) = HK16*(-HK0*P(15,22) + HK0*P(4,15) - HK1*P(15,23) + HK1*P(5,15) + P(6,15)*vd);
-Kfusion(16) = HK16*(-HK0*P(16,22) + HK0*P(4,16) - HK1*P(16,23) + HK1*P(5,16) + P(6,16)*vd);
-Kfusion(17) = HK16*(-HK0*P(17,22) + HK0*P(4,17) - HK1*P(17,23) + HK1*P(5,17) + P(6,17)*vd);
-Kfusion(18) = HK16*(-HK0*P(18,22) + HK0*P(4,18) - HK1*P(18,23) + HK1*P(5,18) + P(6,18)*vd);
-Kfusion(19) = HK16*(-HK0*P(19,22) + HK0*P(4,19) - HK1*P(19,23) + HK1*P(5,19) + P(6,19)*vd);
-Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P(6,20)*vd);
-Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd);
-Kfusion(22) = HK15*HK16;
-Kfusion(23) = HK14*HK16;
-
-
-// Predicted observation
-
-
-// Innovation variance
-
-
@@ -1,55 +0,0 @@
-// Sub Expressions
-const float HK0 = vn - vwn;
-const float HK1 = ve - vwe;
-const float HK2 = sqrtf((HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd));
-const float HK3 = 1.0F/(HK2);
-const float HK4 = HK0*HK3;
-const float HK5 = HK1*HK3;
-const float HK6 = HK3*vd;
-const float HK7 = -HK0*HK3;
-const float HK8 = -HK1*HK3;
-const float HK9 = 1.0F/(innov_var);
-
-
-// Observation Jacobians
-Hfusion.at<4>() = HK4;
-Hfusion.at<5>() = HK5;
-Hfusion.at<6>() = HK6;
-Hfusion.at<22>() = HK7;
-Hfusion.at<23>() = HK8;
-
-
-// Kalman gains
-Kfusion(0) = HK9*(HK4*P(0,4) + HK5*P(0,5) + HK6*P(0,6) + HK7*P(0,22) + HK8*P(0,23));
-Kfusion(1) = HK9*(HK4*P(1,4) + HK5*P(1,5) + HK6*P(1,6) + HK7*P(1,22) + HK8*P(1,23));
-Kfusion(2) = HK9*(HK4*P(2,4) + HK5*P(2,5) + HK6*P(2,6) + HK7*P(2,22) + HK8*P(2,23));
-Kfusion(3) = HK9*(HK4*P(3,4) + HK5*P(3,5) + HK6*P(3,6) + HK7*P(3,22) + HK8*P(3,23));
-Kfusion(4) = HK9*(HK4*P(4,4) + HK5*P(4,5) + HK6*P(4,6) + HK7*P(4,22) + HK8*P(4,23));
-Kfusion(5) = HK9*(HK4*P(4,5) + HK5*P(5,5) + HK6*P(5,6) + HK7*P(5,22) + HK8*P(5,23));
-Kfusion(6) = HK9*(HK4*P(4,6) + HK5*P(5,6) + HK6*P(6,6) + HK7*P(6,22) + HK8*P(6,23));
-Kfusion(7) = HK9*(HK4*P(4,7) + HK5*P(5,7) + HK6*P(6,7) + HK7*P(7,22) + HK8*P(7,23));
-Kfusion(8) = HK9*(HK4*P(4,8) + HK5*P(5,8) + HK6*P(6,8) + HK7*P(8,22) + HK8*P(8,23));
-Kfusion(9) = HK9*(HK4*P(4,9) + HK5*P(5,9) + HK6*P(6,9) + HK7*P(9,22) + HK8*P(9,23));
-Kfusion(10) = HK9*(HK4*P(4,10) + HK5*P(5,10) + HK6*P(6,10) + HK7*P(10,22) + HK8*P(10,23));
-Kfusion(11) = HK9*(HK4*P(4,11) + HK5*P(5,11) + HK6*P(6,11) + HK7*P(11,22) + HK8*P(11,23));
-Kfusion(12) = HK9*(HK4*P(4,12) + HK5*P(5,12) + HK6*P(6,12) + HK7*P(12,22) + HK8*P(12,23));
-Kfusion(13) = HK9*(HK4*P(4,13) + HK5*P(5,13) + HK6*P(6,13) + HK7*P(13,22) + HK8*P(13,23));
-Kfusion(14) = HK9*(HK4*P(4,14) + HK5*P(5,14) + HK6*P(6,14) + HK7*P(14,22) + HK8*P(14,23));
-Kfusion(15) = HK9*(HK4*P(4,15) + HK5*P(5,15) + HK6*P(6,15) + HK7*P(15,22) + HK8*P(15,23));
-Kfusion(16) = HK9*(HK4*P(4,16) + HK5*P(5,16) + HK6*P(6,16) + HK7*P(16,22) + HK8*P(16,23));
-Kfusion(17) = HK9*(HK4*P(4,17) + HK5*P(5,17) + HK6*P(6,17) + HK7*P(17,22) + HK8*P(17,23));
-Kfusion(18) = HK9*(HK4*P(4,18) + HK5*P(5,18) + HK6*P(6,18) + HK7*P(18,22) + HK8*P(18,23));
-Kfusion(19) = HK9*(HK4*P(4,19) + HK5*P(5,19) + HK6*P(6,19) + HK7*P(19,22) + HK8*P(19,23));
-Kfusion(20) = HK9*(HK4*P(4,20) + HK5*P(5,20) + HK6*P(6,20) + HK7*P(20,22) + HK8*P(20,23));
-Kfusion(21) = HK9*(HK4*P(4,21) + HK5*P(5,21) + HK6*P(6,21) + HK7*P(21,22) + HK8*P(21,23));
-Kfusion(22) = HK9*(HK4*P(4,22) + HK5*P(5,22) + HK6*P(6,22) + HK7*P(22,22) + HK8*P(22,23));
-Kfusion(23) = HK9*(HK4*P(4,23) + HK5*P(5,23) + HK6*P(6,23) + HK7*P(22,23) + HK8*P(23,23));
-
-
-// Predicted observation
-meas_pred = HK2;
-
-
-// Innovation variance
-
-
@@ -1,25 +0,0 @@
-// Sub Expressions
-const float IV0 = ve - vwe;
-const float IV1 = vn - vwn;
-const float IV2 = (IV0)*(IV0) + (IV1)*(IV1) + (vd)*(vd);
-const float IV3 = 1.0F/(IV2);
-const float IV4 = IV0*P(5,23);
-const float IV5 = IV0*IV3;
-const float IV6 = IV1*P(4,22);
-const float IV7 = IV1*IV3;
-
-
-// Observation Jacobians
-
-
-// Kalman gains
-
-
-// Predicted observation
-meas_pred = sqrtf(IV2);
-
-
-// Innovation variance
-innov_var = IV3*vd*(IV0*P(5,6) - IV0*P(6,23) + IV1*P(4,6) - IV1*P(6,22) + P(6,6)*vd) - IV5*(-IV0*P(23,23) - IV1*P(22,23) + IV1*P(4,23) + IV4 + P(6,23)*vd) + IV5*(IV0*P(5,5) + IV1*P(4,5) - IV1*P(5,22) - IV4 + P(5,6)*vd) - IV7*(-IV0*P(22,23) + IV0*P(5,22) - IV1*P(22,22) + IV6 + P(6,22)*vd) + IV7*(-IV0*P(4,23) + IV0*P(4,5) + IV1*P(4,4) - IV6 + P(4,6)*vd) + R_TAS;
-
-
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	f82eae800d	boardctl hacks	2022-10-09 11:31:33 -04:00
Daniel Agar	bb96343e8d	move I2C bus clock defaults to board px4_i2c_buses	2022-10-08 13:25:15 -04:00
fkaiser	30150f723a	Tools/px_uploader.py: use monotonic clock if available (#20352 ) Signed-off-by: fkaiser <fabian.kaiser@wingtra.com>	2022-10-06 09:49:05 -04:00
alexklimaj	064f3f86bc	Increase mag calibration max_var_allowed	2022-10-06 08:44:36 -04:00
Hamish Willee	022aa13aa1	mpu9250_i2c - also needs docs update	2022-10-05 20:26:25 -04:00
Hamish Willee	6d612b1ba4	MPU9250 driver doc incorrect	2022-10-05 20:26:25 -04:00
Daniel Agar	8cc39096cb	load_mon: NuttX cpuload use system times for calculation - this is to minimize the impact of any load_mon scheduling jitter in the sampled load percentage	2022-10-05 14:21:38 -04:00
benjinne	29b031c862	Tools/setup/ubuntu.sh update comment to include 22.04	2022-10-05 13:41:47 -04:00
Daniel Agar	5f0a539633	gps_blending: fix blending init to best instance - output GPS publication defaults to best input instance - blended states are explicitly cleared and then populated with weighted blend	2022-10-05 10:26:48 -04:00
Beat Küng	f79dad1e63	fix ROMFS: run camera trigger, capture and PPS before pwm_out As they might need to reserve the pwm pins.	2022-10-05 09:13:35 -04:00
Matthias Grob	46e6e83e14	Commander: allow disarmin not-landed boat like rover This is a hotfix and a boat should never even detect a takeoff in normal operation.	2022-10-05 09:07:18 -04:00
Beat Küng	dafead6f20	logger: add compressed events metadata file if it exists in the ROMFS Allows to decode events not (yet) on main. The file is currently ~15KB.	2022-10-05 07:44:55 +02:00
Beat Küng	02035d94aa	metadata: sort json output ensures the files don't change arbitrarily	2022-10-05 07:44:55 +02:00
modaltb	06a0aedbdb	modalai_esc: directly using deadband param value instead of converting it to RPM	2022-10-04 20:14:59 -04:00
Thomas Debrunner	0af87ec745	mavlink: initial OPEN_DRONE_ID_BASIC_ID/OPEN_DRONE_ID_LOCATION support	2022-10-04 14:40:59 -04:00
Matthias Grob	b179427b4c	Remove opinionated non-default MPC_TKO_SPEED from airframes	2022-10-04 11:36:57 -04:00
pandafeng1999	e4b4df4e5d	drivers/distance_sensor/gy_us42: add the specified i2c address	2022-10-04 11:35:45 -04:00
Matthias Grob	f96507bb22	vtol_takeoff: fix comment typo	2022-10-04 10:01:04 -04:00
Peter van der Perk	c807d6079d	FMUK66-V3 Enable DMA on SPI	2022-10-04 09:46:04 -04:00
Peter van der Perk	0cf2ecedb9	FMUK66 Disable Telnet to save RAM	2022-10-04 09:46:04 -04:00
Julian Oes	5c77bbcb4c	ms5611: ignore reading 0 This prevents publishing a negative pressure which leads to a NAN altitude estimate further down the line.	2022-10-04 09:37:53 -04:00
Beat Küng	04b1cbb423	vtol_att_control: standard: also do blending for FW controls So that the sum of the control is always 1.	2022-10-04 09:54:11 +02:00
Silvan Fuhrer	6db92b4011	ControlAllocation: remove actuator trim value from actuator_sp to calculate allocated control Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-04 07:51:25 +02:00
Zachary Lowell	4520186878	qurt: replacing qurt threads with pthread	2022-10-03 18:01:04 -04:00
bresch	4b687beb3b	ekf2: remove old airspeed fusion code	2022-10-03 10:59:42 -04:00
bresch	299e6058e3	ekf2_test: test airspeed symforce vs sympy generated code	2022-10-03 10:59:42 -04:00
bresch	29ebef1f74	ekf2: migrate fuse_airspeed to SymForce	2022-10-03 10:59:42 -04:00
Daniel Agar	41cda14126	ekf2: add symforce code generation helper target	2022-10-03 10:59:42 -04:00
Daniel Agar	c9441bb48a	boards: px4_fmu-v2/v3 allow optional mpu9250 probe to fail without error	2022-10-02 21:40:18 -04:00
Benjamin Perseghetti	aa2b47845a	new Gazebo simulation handle older gz versions (#20342 )	2022-10-02 21:30:30 -04:00
PX4 BuildBot	f4d2e176ae	Update submodule sitl_gazebo to latest Sun Oct 2 12:38:54 UTC 2022 - sitl_gazebo in PX4/Firmware (`2bd1ac005f`): https://github.com/PX4/PX4-SITL_gazebo/commit/b968405a617005ba0a793a8b4703913d4c6eff5f - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/e80432759540c91f85a012f47aa6ebb0ee9de7e4 - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/b968405a617005ba0a793a8b4703913d4c6eff5f...e80432759540c91f85a012f47aa6ebb0ee9de7e4 e804327 2022-10-01 Thomas Stauber - set lidar measurements to 0.0 if out of range (#914) 7ed1da6 2022-09-27 Karthik S - Advanced Lift-Drag Plugin (#901) 669d7f5 2022-09-08 Oleg Kalachev - Fix local addr assignment for qgc and sdk sockets	2022-10-02 10:54:31 -04:00
PX4 BuildBot	f7a5c91fb3	Update submodule libcanard to latest Sun Oct 2 12:38:56 UTC 2022 - libcanard in PX4/Firmware (8e144eece9e9cd5a23dbeb6723591f214cad631a): https://github.com/opencyphal/libcanard/commit/db87ea32aa092c48ea103963138b6346dd3e9008 - libcanard current upstream: https://github.com/opencyphal/libcanard/commit/2e3b11f6b8325080c160d38521b169b0bbb6b1c7 - Changes: https://github.com/opencyphal/libcanard/compare/db87ea32aa092c48ea103963138b6346dd3e9008...2e3b11f6b8325080c160d38521b169b0bbb6b1c7 2e3b11f 2022-07-15 Pavel Kirienko - Follow-up for #197 -- fix minor issues discovered by Sonar (#198) 106ceaf 2022-07-11 Dmitry Ponomarev - Fix cast-align warnings on ARM 8953fe6 2022-07-06 Dmitry Ponomarev - Add ARM build to github workflow	2022-10-02 10:54:05 -04:00
PX4 BuildBot	644a836d0a	Update submodule libevents to latest Sun Oct 2 12:39:02 UTC 2022 - libevents in PX4/Firmware (ddc1e6d6c5c2707be5522d4c4d7434403202cf45): https://github.com/mavlink/libevents/commit/82dabdb914d7bd640c281900e2852d0afc074b68 - libevents current upstream: https://github.com/mavlink/libevents/commit/a89808bffd1efb0543e66f17a7fa12dfce5e6bf0 - Changes: https://github.com/mavlink/libevents/compare/82dabdb914d7bd640c281900e2852d0afc074b68...a89808bffd1efb0543e66f17a7fa12dfce5e6bf0 a89808b 2022-09-26 Beat Küng - combine.py: sort json keys	2022-10-02 10:53:28 -04:00
PX4 BuildBot	cf0cd4ebf2	Update submodule mavlink to latest Sun Oct 2 12:39:04 UTC 2022 - mavlink in PX4/Firmware (65bd1fd4c000a7fa0de2a0e8dce1ba9b2a62fa86): https://github.com/mavlink/mavlink/commit/c46af523263ff0dad59afe018fe387c1df030442 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/dda5a18ddb002a871ba301bb584893ee6378e2f3 - Changes: https://github.com/mavlink/mavlink/compare/c46af523263ff0dad59afe018fe387c1df030442...dda5a18ddb002a871ba301bb584893ee6378e2f3 dda5a18d 2022-09-28 WickedShell - Support a return to default speed for DO_CHANGE_SPEED (#1890) 4e04dbbf 2022-09-24 Hamish Willee - MAV_FRAME_BODY_FRD - match to implementations and sanity (#1894) 89a61214 2022-09-23 Hamish Willee - DO_CHANGE_SPEED - clarify lifetime of speed setting (#1892) 11414f88 2022-09-21 Søren Friis - Clarify swarm fields (#1891) 5cdf8b60 2022-09-08 Hamish Willee - HOME_POSITION - clarify local frame means NED (#1886) 551e3b49 2022-09-07 Julian Oes - Fix CI (#1888) d99ee9b6 2022-09-07 Julian Oes - pymavlink: update submodule to latest master (#1887) 92d2a58d 2022-09-01 Hamish Willee - common.xml:TIMESYNC docs and extensions (#1878) c424dc65 2022-09-01 Julian Oes - minimal: try to make component ID less confusing (#1876) 153939bd 2022-09-01 Hamish Willee - Fix typo MAV_CMD_DO_SET_MISSION_CURRENT 5043a1e0 2022-09-01 Hamish Willee - MAV_CMD_DO_SET_MISSION_CURRENT - allow mission resetting (#1870) 90aa7bfc 2022-08-31 Chikirev Sirguy - Update common.xml (#1815) 554ccadd 2022-08-31 Hamish Willee - common: OpenDroneID messages no longer WIP (#1882) 06448cf6 2022-08-24 Hamish Willee - MAV_ODID_ARM_STATUS entries update to naming convention (#1883) ff649224 2022-08-18 Patrick José Pereira - Fix gibberish descriptions ualberta.xml (#1881) 11f4b236 2022-08-18 Hamish Willee - Mission item - document values for autocontinue (#1868) fb8ac31b 2022-08-18 Julian Oes - development: remove MISSION_CHANGED (#1879) 76b794c2 2022-08-18 Julian Oes - common: extend MISSION_CURRENT (#1869) 1ae3edc4 2022-08-15 Andrew Tridgell - common: added OPEN_DRONE_ID_SYSTEM_UPDATE message (#1880) aa25f4df 2022-08-11 Andrew Tridgell - common: added OPEN_DRONE_ID_ARM_STATUS (#1873)	2022-10-02 10:53:00 -04:00
Daniel Agar	2bd1ac005f	.gitmodules update branches master -> main	2022-10-01 12:22:22 -04:00