LTE: advertise published topic early to be logged

LTE: refactor to use multiple target estimator types
LTE: use new param sub style
2026-06-20 01:30:34 +08:00 · 2022-10-07 11:45:55 +02:00 · 2022-10-06 18:57:14 +02:00 · 2022-10-06 16:56:52 +02:00 · 2022-10-06 09:49:05 -04:00 · 2022-10-06 08:44:36 -04:00
99 changed files with 3712 additions and 763 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>
@@ -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

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

@@ -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
-
-
@@ -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")
@@ -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;
-
-
@@ -37,6 +37,7 @@ add_subdirectory(sensor_simulator)
 add_subdirectory(test_helper)

 px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_airspeed_fusion_generated.cpp LINKLIBS ecl_EKF)
 px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
@@ -0,0 +1,203 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+
+#include "../EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h"
+#include "../EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h"
+
+using namespace matrix;
+
+typedef matrix::Vector<float, 24> Vector24f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+template<int ... Idxs>
+using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+
+float randf()
+{
+	return (float)rand() / (float)RAND_MAX;
+}
+
+struct DiffRatioReport {
+	float max_diff_fraction;
+	float max_row;
+	float max_v1;
+	float max_v2;
+};
+
+DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2)
+{
+	// Find largest observation variance difference as a fraction of v1 or v2
+	DiffRatioReport report = {};
+
+	for (int row = 0; row < 24; row++) {
+		float diff_fraction;
+
+		if (fabsf(v1(row)) > FLT_EPSILON) {
+			diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v1(row));
+
+		} else if (fabsf(v2(row)) > FLT_EPSILON) {
+			diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v2(row));
+
+		} else {
+			diff_fraction = 0.0f;
+		}
+
+		if (diff_fraction > report.max_diff_fraction) {
+			report.max_diff_fraction = diff_fraction;
+			report.max_row = row;
+			report.max_v1 = v1(row);
+			report.max_v2 = v2(row);
+		}
+	}
+
+	return report;
+}
+
+TEST(AirspeedFusionGenerated, SympyVsSymforce)
+{
+	// Compare calculation of observation Jacobians and Kalman gains for sympy and symforce generated equations
+	const float R_TAS = sq(1.5f);
+
+	const float vn = 9.0f;
+	const float ve = 12.0f;
+	const float vd = -1.5f;
+
+	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) = randf();
+
+			} else {
+				P(col, row) = P(row, col) = 2.0f * (randf() - 0.5f);
+			}
+		}
+	}
+
+	// First calculate observationjacobians and Kalman gains using sympy generated equations
+	Vector24f Hfusion_sympy;
+	Vector24f Kfusion_sympy;
+
+	{
+		// 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);
+		// calculation can be badly conditioned for very low airspeed values so don't fuse this time
+		EXPECT_GT(v_tas_pred, 1.f);
+		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 inn_var = (-HK10 * HK14 + HK10 * HK9 + HK12 * HK13 - HK13 * HK15 + HK6 * HK7 * vd + R_TAS);
+		const float HK16 = HK3 / inn_var;
+
+		// 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;
+
+		Vector24f Kfusion;
+
+		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;
+	}
+
+	// Then calculate observationjacobians and Kalman gains using symforce generated equations
+	Vector24f Hfusion_symforce;
+	Vector24f Kfusion_symforce;
+
+	{
+		Vector24f state_vector{};
+		state_vector(4) = vn;
+		state_vector(5) = ve;
+		state_vector(6) = vd;
+		state_vector(22) = vwn;
+		state_vector(23) = vwe;
+
+		float innov;
+		float innov_var;
+
+		sym::ComputeAirspeedInnovAndInnovVar(state_vector, P, 0.f, R_TAS, FLT_EPSILON, &innov, &innov_var);
+		sym::ComputeAirspeedHAndK(state_vector, P, innov_var, FLT_EPSILON, &Hfusion_symforce, &Kfusion_symforce);
+	}
+
+	DiffRatioReport report = computeDiffRatioVector24f(Hfusion_sympy, Hfusion_symforce);
+	EXPECT_LT(report.max_diff_fraction, 1e-5f) << "Airspeed Hfusion max diff fraction = " << report.max_diff_fraction <<
+			" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
+
+	report = computeDiffRatioVector24f(Kfusion_sympy, Kfusion_symforce);
+	EXPECT_LT(report.max_diff_fraction, 1e-5f) << "Airspeed Kfusion max diff fraction = " << report.max_diff_fraction <<
+			" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
+}
@@ -43,30 +43,8 @@

 namespace landing_target_estimator
 {
-KalmanFilter::KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
-{
-	init(initial, covInit);
-}

-void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
-{
-	_x = initial;
-	_covariance = covInit;
-}
-
-void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11)
-{
-	matrix::Vector<float, 2> initial;
-	initial(0) = initial0;
-	initial(1) = initial1;
-	matrix::Matrix<float, 2, 2> covInit;
-	covInit(0, 0) = covInit00;
-	covInit(1, 1) = covInit11;
-
-	init(initial, covInit);
-}
-
-void KalmanFilter::predict(float dt, float acc, float acc_unc)
+void KalmanFilter::predict(float dt, float acc)
 {
 	_x(0) += _x(1) * dt + dt * dt / 2 * acc;
 	_x(1) += acc * dt;
@@ -80,12 +58,12 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc)
 	G(0, 0) = dt * dt / 2;
 	G(1, 0) = dt;

-	matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * acc_unc;
+	matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * _acc_var;

 	_covariance = A * _covariance * A.transpose() + process_noise;
 }

-bool KalmanFilter::update(float meas, float measUnc)
+bool KalmanFilter::fusePosition(float meas, float measUnc)
 {

 	// H = [1, 0]
@@ -122,28 +100,6 @@ bool KalmanFilter::update(float meas, float measUnc)

 }

-void KalmanFilter::getState(matrix::Vector<float, 2> &state)
-{
-	state = _x;
-}
-
-void KalmanFilter::getState(float &state0, float &state1)
-{
-	state0 = _x(0);
-	state1 = _x(1);
-}
-
-void KalmanFilter::getCovariance(matrix::Matrix<float, 2, 2> &covariance)
-{
-	covariance = _covariance;
-}
-
-void KalmanFilter::getCovariance(float &cov00, float &cov11)
-{
-	cov00 = _covariance(0, 0);
-	cov11 = _covariance(1, 1);
-}
-
 void KalmanFilter::getInnovations(float &innov, float &innovCov)
 {
 	innov = _residual;
@@ -51,11 +51,13 @@
 #include <matrix/Matrix.hpp>
 #include <matrix/Vector.hpp>

+#include "target_estimator.h"
+
 #pragma once

 namespace landing_target_estimator
 {
-class KalmanFilter
+class KalmanFilter : public TargetEstimator
 {
 public:
 	/**
@@ -63,31 +65,13 @@ public:
 	 */
 	KalmanFilter() {};

-	/**
-	 * Constructor, initialize state
-	 */
-	KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
-
 	/**
 	 * Default desctructor
 	 */
 	virtual ~KalmanFilter() {};

-	/**
-	 * Initialize filter state
-	 * @param initial initial state
-	 * @param covInit initial covariance
-	 */
-	void init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
-
-	/**
-	 * Initialize filter state, only specifying diagonal covariance elements
-	 * @param initial0  first initial state
-	 * @param initial1  second initial state
-	 * @param covInit00 initial variance of first state
-	 * @param covinit11 initial variance of second state
-	 */
-	void init(float initial0, float initial1, float covInit00, float covInit11);
+	void setPosition(float pos) override { _x(0) = pos; }
+	void setVelocity(float vel) override { _x(1) = vel; }

 	/**
 	 * Predict the state with an external acceleration estimate
@@ -95,7 +79,7 @@ public:
 	 * @param acc           Acceleration estimate
 	 * @param acc_unc       Variance of acceleration estimate
 	 */
-	void predict(float dt, float acc, float acc_unc);
+	void predict(float dt, float acc) override;

 	/**
 	 * Update the state estimate with a measurement
@@ -103,33 +87,13 @@ public:
 	 * @param measUnc measurement uncertainty
 	 * @return update success (measurement not rejected)
 	 */
-	bool update(float meas, float measUnc);
+	bool fusePosition(float meas, float measUnc) override;

-	/**
-	 * Get the current filter state
-	 * @param x1 State
-	 */
-	void getState(matrix::Vector<float, 2> &state);
+	float getPosition() override { return _x(0); }
+	float getVelocity() override { return _x(1); }

-	/**
-	 * Get the current filter state
-	 * @param state0 First state
-	 * @param state1 Second state
-	 */
-	void getState(float &state0, float &state1);
-
-	/**
-	 * Get state covariance
-	 * @param covariance Covariance of the state
-	 */
-	void getCovariance(matrix::Matrix<float, 2, 2> &covariance);
-
-	/**
-	 * Get state variances (diagonal elements)
-	 * @param cov00 Variance of first state
-	 * @param cov11 Variance of second state
-	 */
-	void getCovariance(float &cov00, float &cov11);
+	float getPosVar() override { return _covariance(0, 0); }
+	float getVelVar() override { return _covariance(0, 0); }

 	/**
 	 * Get measurement innovation and covariance of last update call
@@ -50,26 +50,29 @@
 namespace landing_target_estimator
 {

-LandingTargetEstimator::LandingTargetEstimator()
+using namespace matrix;
+
+LandingTargetEstimator::LandingTargetEstimator() :
+	ModuleParams(nullptr)
 {
-	_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
-	_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
-	_paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN");
-	_paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN");
-	_paramHandle.mode = param_find("LTEST_MODE");
-	_paramHandle.scale_x = param_find("LTEST_SCALE_X");
-	_paramHandle.scale_y = param_find("LTEST_SCALE_Y");
-	_paramHandle.sensor_yaw = param_find("LTEST_SENS_ROT");
-	_paramHandle.offset_x = param_find("LTEST_SENS_POS_X");
-	_paramHandle.offset_y = param_find("LTEST_SENS_POS_Y");
-	_paramHandle.offset_z = param_find("LTEST_SENS_POS_Z");
+	_targetPosePub.advertise();
 	_check_params(true);
 }

+LandingTargetEstimator::~LandingTargetEstimator()
+{
+	delete _target_estimator[0];
+	delete _target_estimator[1];
+}
+
 void LandingTargetEstimator::update()
 {
 	_check_params(false);

+	if ((_target_estimator[0] == nullptr) || (_target_estimator[1] == nullptr)) {
+		return;
+	}
+
 	_update_topics();

 	/* predict */
@@ -93,8 +96,8 @@ void LandingTargetEstimator::update()
 				a.zero();
 			}

-			_kalman_filter_x.predict(dt, -a(0), _params.acc_unc);
-			_kalman_filter_y.predict(dt, -a(1), _params.acc_unc);
+			_target_estimator[0]->predict(dt, -a(0));
+			_target_estimator[1]->predict(dt, -a(1));

 			_last_predict = hrt_absolute_time();
 		}
@@ -110,11 +113,20 @@ void LandingTargetEstimator::update()


 	if (!_estimator_initialized) {
-		float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
-		float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
-		PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init);
-		_kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init);
-		_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init);
+		Vector2f v_init;
+		v_init(0) = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
+		v_init(1) = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
+
+		Vector2f p_init(_target_position_report.rel_pos_x, _target_position_report.rel_pos_y);
+
+		PX4_INFO("Init %.2f %.2f", (double)v_init(0), (double)v_init(1));
+
+		for (int i = 0; i < 2; i++) {
+			_target_estimator[i]->setPosition(p_init(i));
+			_target_estimator[i]->setVelocity(v_init(i));
+			_target_estimator[i]->setStatePosVar(_param_ltest_pos_unc_in.get());
+			_target_estimator[i]->setStateVelVar(_param_ltest_vel_unc_in.get());
+		}

 		_estimator_initialized = true;
 		_last_update = hrt_absolute_time();
@@ -122,9 +134,9 @@ void LandingTargetEstimator::update()

 	} else {
 		// update
-		const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z;
-		bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty);
-		bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty);
+		const float measurement_uncertainty = _param_ltest_meas_unc.get() * _dist_z * _dist_z;
+		bool update_x = _target_estimator[0]->fusePosition(_target_position_report.rel_pos_x, measurement_uncertainty);
+		bool update_y = _target_estimator[1]->fusePosition(_target_position_report.rel_pos_y, measurement_uncertainty);

 		if (!update_x || !update_y) {
 			if (!_faulty) {
@@ -139,30 +151,26 @@ void LandingTargetEstimator::update()
 		if (!_faulty) {
 			// only publish if both measurements were good

+			const float x = _target_estimator[0]->getPosition();
+			const float y = _target_estimator[1]->getPosition();
+
 			_target_pose.timestamp = _target_position_report.timestamp;

-			float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
-			_kalman_filter_x.getState(x, xvel);
-			_kalman_filter_x.getCovariance(covx, covx_v);
-
-			_kalman_filter_y.getState(y, yvel);
-			_kalman_filter_y.getCovariance(covy, covy_v);
-
-			_target_pose.is_static = (_params.mode == TargetMode::Stationary);
+			_target_pose.is_static = ((TargetMode)_param_ltest_mode.get() == TargetMode::Stationary);

 			_target_pose.rel_pos_valid = true;
 			_target_pose.rel_vel_valid = true;
 			_target_pose.x_rel = x;
 			_target_pose.y_rel = y;
 			_target_pose.z_rel = _target_position_report.rel_pos_z ;
-			_target_pose.vx_rel = xvel;
-			_target_pose.vy_rel = yvel;
+			_target_pose.vx_rel = _target_estimator[0]->getVelocity();
+			_target_pose.vy_rel = _target_estimator[1]->getVelocity();

-			_target_pose.cov_x_rel = covx;
-			_target_pose.cov_y_rel = covy;
+			_target_pose.cov_x_rel = _target_estimator[0]->getPosVar();
+			_target_pose.cov_y_rel = _target_estimator[1]->getPosVar();

-			_target_pose.cov_vx_rel = covx_v;
-			_target_pose.cov_vy_rel = covy_v;
+			_target_pose.cov_vx_rel = _target_estimator[0]->getVelVar();
+			_target_pose.cov_vy_rel = _target_estimator[0]->getVelVar();

 			if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
 				_target_pose.x_abs = x + _vehicleLocalPosition.x;
@@ -180,15 +188,16 @@ void LandingTargetEstimator::update()
 			_last_predict = _last_update;
 		}

-		float innov_x, innov_cov_x, innov_y, innov_cov_y;
-		_kalman_filter_x.getInnovations(innov_x, innov_cov_x);
-		_kalman_filter_y.getInnovations(innov_y, innov_cov_y);
+		//TODO:fix
+		/* float innov_x, innov_cov_x, innov_y, innov_cov_y; */
+		/* _target_estimator[0]->getInnovations(innov_x, innov_cov_x); */
+		/* _target_estimator[1]->getInnovations(innov_y, innov_cov_y); */

-		_target_innovations.timestamp = _target_position_report.timestamp;
-		_target_innovations.innov_x = innov_x;
-		_target_innovations.innov_cov_x = innov_cov_x;
-		_target_innovations.innov_y = innov_y;
-		_target_innovations.innov_cov_y = innov_cov_y;
+		/* _target_innovations.timestamp = _target_position_report.timestamp; */
+		/* _target_innovations.innov_x = innov_x; */
+		/* _target_innovations.innov_cov_x = innov_cov_x; */
+		/* _target_innovations.innov_y = innov_y; */
+		/* _target_innovations.innov_cov_y = innov_cov_y; */

 		_targetInnovationsPub.publish(_target_innovations);
 	}
@@ -200,7 +209,7 @@ void LandingTargetEstimator::_check_params(const bool force)
 		parameter_update_s pupdate;
 		_parameter_update_sub.copy(&pupdate);

-		_update_params();
+		updateParams();
 	}
 }

@@ -224,12 +233,12 @@ void LandingTargetEstimator::_update_topics()
 		}

 		matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
-		sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward
-		sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right
+		sensor_ray(0) = _irlockReport.pos_x * _param_ltest_scale_x.get(); // forward
+		sensor_ray(1) = _irlockReport.pos_y * _param_ltest_scale_y.get(); // right
 		sensor_ray(2) = 1.0f;

 		// rotate unit ray according to sensor orientation
-		_S_att = get_rot_matrix(_params.sensor_yaw);
+		_S_att = get_rot_matrix(static_cast<enum Rotation>(_param_ltest_sens_rot.get()));
 		sensor_ray = _S_att * sensor_ray;

 		// rotate the unit ray into the navigation frame
@@ -242,7 +251,7 @@ void LandingTargetEstimator::_update_topics()
 			return;
 		}

-		_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z;
+		_dist_z = _vehicleLocalPosition.dist_bottom - _param_ltest_sens_pos_z.get();

 		// scale the ray s.t. the z component has length of _uncertainty_scale
 		_target_position_report.timestamp = _irlockReport.timestamp;
@@ -251,8 +260,8 @@ void LandingTargetEstimator::_update_topics()
 		_target_position_report.rel_pos_z = _dist_z;

 		// Adjust relative position according to sensor offset
-		_target_position_report.rel_pos_x += _params.offset_x;
-		_target_position_report.rel_pos_y += _params.offset_y;
+		_target_position_report.rel_pos_x += _param_ltest_sens_pos_x.get();
+		_target_position_report.rel_pos_y += _param_ltest_sens_pos_y.get();

 		_new_sensorReport = true;

@@ -282,28 +291,48 @@ void LandingTargetEstimator::_update_topics()
 	}
 }

-void LandingTargetEstimator::_update_params()
+void LandingTargetEstimator::updateParams()
 {
-	param_get(_paramHandle.acc_unc, &_params.acc_unc);
-	param_get(_paramHandle.meas_unc, &_params.meas_unc);
-	param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init);
-	param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init);
+	int32_t current_target_estimator_mode = _param_ltest_mode.get();
+	ModuleParams::updateParams();

-	int32_t mode = 0;
-	param_get(_paramHandle.mode, &mode);
-	_params.mode = (TargetMode)mode;
+	if ((current_target_estimator_mode != _param_ltest_mode.get()) || (_target_estimator[0] == nullptr)
+	    || (_target_estimator[1] == nullptr)) {
+		selectTargetEstimator();
+	}

-	param_get(_paramHandle.scale_x, &_params.scale_x);
-	param_get(_paramHandle.scale_y, &_params.scale_y);
-
-	int32_t sensor_yaw = 0;
-	param_get(_paramHandle.sensor_yaw, &sensor_yaw);
-	_params.sensor_yaw = static_cast<enum Rotation>(sensor_yaw);
-
-	param_get(_paramHandle.offset_x, &_params.offset_x);
-	param_get(_paramHandle.offset_y, &_params.offset_y);
-	param_get(_paramHandle.offset_z, &_params.offset_z);
+	_target_estimator[0]->setInputAccVar(_param_ltest_acc_unc.get());
+	_target_estimator[1]->setInputAccVar(_param_ltest_acc_unc.get());
 }

+void LandingTargetEstimator::selectTargetEstimator()
+{
+	const TargetMode target_mode = (TargetMode)_param_ltest_mode.get();
+
+	TargetEstimator *tmp_x = nullptr;
+	TargetEstimator *tmp_y = nullptr;
+
+	switch (target_mode) {
+	case TargetMode::Moving:
+		/* tmp = new xxx */
+		break;
+
+	case TargetMode::Stationary:
+		tmp_x = new KalmanFilter();
+		tmp_y = new KalmanFilter();
+		break;
+	}
+
+	if ((tmp_x == nullptr) || (tmp_y == nullptr)) {
+		PX4_ERR("LTE init failed");
+		_param_ltest_mode.set(0);
+
+	} else {
+		delete _target_estimator[0];
+		delete _target_estimator[1];
+		_target_estimator[0] = tmp_x;
+		_target_estimator[1] = tmp_y;
+	}
+}

 } // namespace landing_target_estimator
@@ -42,6 +42,7 @@

 #pragma once

+#include <px4_platform_common/module_params.h>
 #include <px4_platform_common/workqueue.h>
 #include <drivers/drv_hrt.h>
 #include <parameters/param.h>
@@ -69,12 +70,12 @@ using namespace time_literals;
 namespace landing_target_estimator
 {

-class LandingTargetEstimator
+class LandingTargetEstimator: public ModuleParams
 {
 public:

 	LandingTargetEstimator();
-	virtual ~LandingTargetEstimator() = default;
+	virtual ~LandingTargetEstimator();

 	/*
 	 * Get new measurements and update the state estimate
@@ -91,7 +92,7 @@ protected:
 	/*
 	 * Update parameters.
 	 */
-	void _update_params();
+	void updateParams() override;

 	/* timeout after which filter is reset if target not seen */
 	static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000;
@@ -111,37 +112,6 @@ private:
 		Stationary
 	};

-	/**
-	* Handles for parameters
-	**/
-	struct {
-		param_t acc_unc;
-		param_t meas_unc;
-		param_t pos_unc_init;
-		param_t vel_unc_init;
-		param_t mode;
-		param_t scale_x;
-		param_t scale_y;
-		param_t offset_x;
-		param_t offset_y;
-		param_t offset_z;
-		param_t sensor_yaw;
-	} _paramHandle;
-
-	struct {
-		float acc_unc;
-		float meas_unc;
-		float pos_unc_init;
-		float vel_unc_init;
-		TargetMode mode;
-		float scale_x;
-		float scale_y;
-		float offset_x;
-		float offset_y;
-		float offset_z;
-		enum Rotation sensor_yaw;
-	} _params;
-
 	struct {
 		hrt_abstime timestamp;
 		float rel_pos_x;
@@ -149,6 +119,8 @@ private:
 		float rel_pos_z;
 	} _target_position_report;

+	void selectTargetEstimator();
+
 	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
@@ -175,8 +147,7 @@ private:
 	matrix::Dcmf _R_att; //Orientation of the body frame
 	matrix::Dcmf _S_att; //Orientation of the sensor relative to body frame
 	matrix::Vector2f _rel_pos;
-	KalmanFilter _kalman_filter_x;
-	KalmanFilter _kalman_filter_y;
+	TargetEstimator *_target_estimator[2] {nullptr, nullptr};
 	hrt_abstime _last_predict{0}; // timestamp of last filter prediction
 	hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
 	float _dist_z{1.0f};
@@ -184,5 +155,19 @@ private:
 	void _check_params(const bool force);

 	void _update_state();
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::LTEST_ACC_UNC>) _param_ltest_acc_unc,
+		(ParamFloat<px4::params::LTEST_MEAS_UNC>) _param_ltest_meas_unc,
+		(ParamFloat<px4::params::LTEST_POS_UNC_IN>) _param_ltest_pos_unc_in,
+		(ParamFloat<px4::params::LTEST_VEL_UNC_IN>) _param_ltest_vel_unc_in,
+		(ParamInt<px4::params::LTEST_MODE>) _param_ltest_mode,
+		(ParamFloat<px4::params::LTEST_SCALE_X>) _param_ltest_scale_x,
+		(ParamFloat<px4::params::LTEST_SCALE_Y>) _param_ltest_scale_y,
+		(ParamInt<px4::params::LTEST_SENS_ROT>) _param_ltest_sens_rot,
+		(ParamFloat<px4::params::LTEST_SENS_POS_X>) _param_ltest_sens_pos_x,
+		(ParamFloat<px4::params::LTEST_SENS_POS_Y>) _param_ltest_sens_pos_y,
+		(ParamFloat<px4::params::LTEST_SENS_POS_Z>) _param_ltest_sens_pos_z
+	)
 };
 } // namespace landing_target_estimator
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file target_estimator.cpp
+ * @brief Interface for target estimators
+ */
+
+class TargetEstimator
+{
+public:
+	TargetEstimator() = default;
+	virtual ~TargetEstimator() = default;
+
+	virtual void predict(float dt, float acceleration) = 0;
+	virtual bool fusePosition(float pos, float var) { return true; }
+	virtual bool fuseVeloticy(float pos, float var) { return true; }
+
+	virtual void setPosition(float pos) {};
+	virtual void setVelocity(float vel) {};
+	virtual void setTargetAcc(float acc) {};
+
+	virtual void setInputAccVar(float var) { _acc_var = var; }
+
+	virtual void setStatePosVar(float var) {};
+	virtual void setStateVelVar(float var) {};
+	virtual void setStateAccVar(float var) {};
+
+	virtual float getPosition() { return 0.f; }
+	virtual float getVelocity() { return 0.f; }
+	virtual float getAcceleration() { return 0.f; }
+
+	virtual float getPosVar() = 0;
+	virtual float getVelVar() { return 0.f; }
+	virtual float getAccVar() { return 0.f; }
+
+protected:
+	float _acc_var{};
+};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -140,17 +140,23 @@ void LoadMon::cpuload()
 #elif defined(__PX4_NUTTX)

 	if (_last_idle_time == 0) {
+		irqstate_t irqstate = enter_critical_section();
 		// Just get the time in the first iteration */
 		_last_idle_time = system_load.tasks[0].total_runtime;
-		_last_idle_time_sample = hrt_absolute_time();
+		_last_idle_time_sample = system_load.tasks[0].curr_start_time;
+		leave_critical_section(irqstate);
 		return;
 	}

 	irqstate_t irqstate = enter_critical_section();
-	const hrt_abstime now = hrt_absolute_time();
+	const hrt_abstime now = system_load.tasks[0].curr_start_time;
 	const hrt_abstime total_runtime = system_load.tasks[0].total_runtime;
 	leave_critical_section(irqstate);

+	if ((now == _last_idle_time_sample) || (total_runtime == _last_idle_time)) {
+		return;
+	}
+
 	// compute system load
 	const float interval = now - _last_idle_time_sample;
 	const float interval_idletime = total_runtime - _last_idle_time;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -48,4 +48,5 @@ px4_add_module(
 		watchdog.cpp
 	DEPENDS
 		version
+		component_general_json # for checksums.h
 	)
@@ -64,6 +64,7 @@
 #include <systemlib/mavlink_log.h>
 #include <replay/definitions.hpp>
 #include <version/version.h>
+#include <component_information/checksums.h>

 //#define DBGPRINT //write status output every few seconds

@@ -1416,6 +1417,7 @@ void Logger::start_log_file(LogType type)
 		write_parameter_defaults(type);
 		write_perf_data(true);
 		write_console_output();
+		write_events_file(LogType::Full);
 		write_excluded_optional_topics(type);
 	}

@@ -1473,6 +1475,7 @@ void Logger::start_log_mavlink()
 	write_parameter_defaults(LogType::Full);
 	write_perf_data(true);
 	write_console_output();
+	write_events_file(LogType::Full);
 	write_excluded_optional_topics(LogType::Full);
 	write_all_add_logged_msg(LogType::Full);
 	_writer.set_need_reliable_transfer(false);
@@ -1885,6 +1888,56 @@ void Logger::write_info_multiple(LogType type, const char *name, const char *val
 	_writer.unlock();
 }

+void Logger::write_info_multiple(LogType type, const char *name, int fd)
+{
+	// Get the file length
+	struct stat stat_data;
+
+	if (fstat(fd, &stat_data) == -1) {
+		PX4_ERR("fstat failed (%i)", errno);
+		return;
+	}
+
+	const off_t file_size = stat_data.st_size;
+
+	ulog_message_info_multiple_s msg;
+	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
+	msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO_MULTIPLE);
+	msg.is_continued = false;
+	const int name_len = strlen(name);
+
+	int file_offset = 0;
+
+	while (file_offset < file_size) {
+		_writer.lock();
+
+		const int max_format_length = 16; // accounts for "uint8_t[x] "
+		int read_length = math::min(file_size - file_offset, (off_t)sizeof(msg.key_value_str) - name_len - max_format_length);
+
+		/* construct format key (type and name) */
+		msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "uint8_t[%i] %s", read_length, name);
+		size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
+
+		int ret = read(fd, &buffer[msg_size], read_length);
+
+		if (ret == read_length) {
+			msg_size += read_length;
+			msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;
+
+			write_message(type, buffer, msg_size);
+			file_offset += ret;
+
+		} else {
+			PX4_ERR("read failed (%i %i)", ret, errno);
+			file_offset = file_size;
+		}
+
+		msg.is_continued = true;
+		_writer.unlock();
+		_writer.notify();
+	}
+}
+
 void Logger::write_info(LogType type, const char *name, int32_t value)
 {
 	write_info_template<int32_t>(type, name, value, "int32_t");
@@ -2275,6 +2328,25 @@ void Logger::write_changed_parameters(LogType type)
 	_writer.notify();
 }

+void Logger::write_events_file(LogType type)
+{
+	int fd = open(PX4_ROOTFSDIR "/etc/extras/all_events.json.xz", O_RDONLY);
+
+	if (fd == -1) {
+		if (errno != ENOENT) {
+			PX4_ERR("open failed (%i)", errno);
+		}
+
+		return;
+	}
+
+	write_info_multiple(type, "metadata_events", fd);
+
+	close(fd);
+
+	write_info(type, "metadata_events_sha256", component_information::all_events_sha256);
+}
+
 void Logger::ack_vehicle_command(vehicle_command_s *cmd, uint32_t result)
 {
 	vehicle_command_ack_s vehicle_command_ack = {};
@@ -253,6 +253,7 @@ private:

 	void write_info(LogType type, const char *name, const char *value);
 	void write_info_multiple(LogType type, const char *name, const char *value, bool is_continued);
+	void write_info_multiple(LogType type, const char *name, int fd);
 	void write_info(LogType type, const char *name, int32_t value);
 	void write_info(LogType type, const char *name, uint32_t value);

@@ -264,6 +265,7 @@ private:
 	void write_parameter_defaults(LogType type);

 	void write_changed_parameters(LogType type);
+	void write_events_file(LogType type);

 	inline bool copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe);

@@ -206,7 +206,7 @@ struct ulog_message_info_multiple_s {

 	uint8_t is_continued; ///< Can be used for arrays: set to 1, if this message is part of the previous with the same key
 	uint8_t key_len;      ///< Length of the 'key'
-	char key_value_str[255]; ///< String with the key and value information
+	char key_value_str[1200]; ///< String with the key and value information
 };

 /**
@@ -1490,8 +1490,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ALTITUDE", 1.0f);
 		configure_stream_local("ATTITUDE", 15.0f);
-		configure_stream_local("ATTITUDE_TARGET", 2.0f);
 		configure_stream_local("ATTITUDE_QUATERNION", 10.0f);
+		configure_stream_local("ATTITUDE_TARGET", 2.0f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
@@ -1511,16 +1511,17 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_STATUS", 1.0f);
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 0.1f);
-		configure_stream_local("SCALED_PRESSURE", 1.0f);
 		configure_stream_local("LOCAL_POSITION_NED", 1.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f);
 		configure_stream_local("OBSTACLE_DISTANCE", 1.0f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 0.1f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f);
 		configure_stream_local("PING", 0.1f);
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f);
 		configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
 		configure_stream_local("RAW_RPM", 2.0f);
 		configure_stream_local("RC_CHANNELS", 5.0f);
+		configure_stream_local("SCALED_PRESSURE", 1.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);
 		configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
@@ -1574,8 +1575,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_STATUS", 1.0f);
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 1.0f);
-		configure_stream_local("SCALED_PRESSURE", 1.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 1.0f);
@@ -1583,6 +1584,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f);
 		configure_stream_local("RAW_RPM", 5.0f);
 		configure_stream_local("RC_CHANNELS", 20.0f);
+		configure_stream_local("SCALED_PRESSURE", 1.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f);
 		configure_stream_local("SYS_STATUS", 5.0f);
 		configure_stream_local("SYSTEM_TIME", 1.0f);
@@ -1723,10 +1725,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("HIGHRES_IMU", 50.0f);
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 1.0f);
-		configure_stream_local("SCALED_PRESSURE", 1.0f);
 		configure_stream_local("MAG_CAL_REPORT", 1.0f);
 		configure_stream_local("MANUAL_CONTROL", 5.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 1.0f);
@@ -1736,6 +1738,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SCALED_IMU", 25.0f);
 		configure_stream_local("SCALED_IMU2", 25.0f);
 		configure_stream_local("SCALED_IMU3", 25.0f);
+		configure_stream_local("SCALED_PRESSURE", 1.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);
@@ -1806,6 +1809,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
 		configure_stream_local("OPTICAL_FLOW_RAD", 1.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
 		configure_stream_local("PING", 0.1f);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -95,6 +95,8 @@
 #include "streams/MOUNT_ORIENTATION.hpp"
 #include "streams/NAV_CONTROLLER_OUTPUT.hpp"
 #include "streams/OBSTACLE_DISTANCE.hpp"
+#include "streams/OPEN_DRONE_ID_BASIC_ID.hpp"
+#include "streams/OPEN_DRONE_ID_LOCATION.hpp"
 #include "streams/OPTICAL_FLOW_RAD.hpp"
 #include "streams/ORBIT_EXECUTION_STATUS.hpp"
 #include "streams/PING.hpp"
@@ -517,6 +519,12 @@ static const StreamListItem streams_list[] = {
 #if defined(OBSTACLE_DISTANCE_HPP)
 	create_stream_list_item<MavlinkStreamObstacleDistance>(),
 #endif // OBSTACLE_DISTANCE_HPP
+#if defined(OPEN_DRONE_ID_BASIC_ID_HPP)
+	create_stream_list_item<MavlinkStreamOpenDroneIdBasicId>(),
+#endif // OPEN_DRONE_ID_BASIC_ID_HPP
+#if defined(OPEN_DRONE_ID_LOCATION)
+	create_stream_list_item<MavlinkStreamOpenDroneIdLocation>(),
+#endif // OPEN_DRONE_ID_LOCATION
 #if defined(ESC_INFO_HPP)
 	create_stream_list_item<MavlinkStreamESCInfo>(),
 #endif // ESC_INFO_HPP
@@ -2206,6 +2206,12 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 					(hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
 				break;

+			case MAV_TYPE_ODID:
+				_heartbeat_type_open_drone_id = now;
+				_mavlink->telemetry_status().open_drone_id_system_healthy =
+					(hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
+				break;
+
 			default:
 				PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %" PRIu8 " from SYSID: %" PRIu8 ", COMPID: %" PRIu8, hb.type, msg->sysid,
 					  msg->compid);
@@ -3013,6 +3019,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
 		tstatus.heartbeat_type_adsb                    = (t <= TIMEOUT + _heartbeat_type_adsb);
 		tstatus.heartbeat_type_camera                  = (t <= TIMEOUT + _heartbeat_type_camera);
 		tstatus.heartbeat_type_parachute               = (t <= TIMEOUT + _heartbeat_type_parachute);
+		tstatus.heartbeat_type_open_drone_id           = (t <= TIMEOUT + _heartbeat_type_open_drone_id);

 		tstatus.heartbeat_component_telemetry_radio    = (t <= TIMEOUT + _heartbeat_component_telemetry_radio);
 		tstatus.heartbeat_component_log                = (t <= TIMEOUT + _heartbeat_component_log);
@@ -386,6 +386,7 @@ private:
 	hrt_abstime _heartbeat_type_adsb{0};
 	hrt_abstime _heartbeat_type_camera{0};
 	hrt_abstime _heartbeat_type_parachute{0};
+	hrt_abstime _heartbeat_type_open_drone_id{0};

 	hrt_abstime _heartbeat_component_telemetry_radio{0};
 	hrt_abstime _heartbeat_component_log{0};
@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef OPEN_DRONE_ID_BASIC_ID_HPP
+#define OPEN_DRONE_ID_BASIC_ID_HPP
+
+#include <uORB/topics/vehicle_status.h>
+
+class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdBasicId(mavlink); }
+
+	static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_BASIC_ID"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _vehicle_status_sub.advertised() ? MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type)
+	{
+		switch (system_type) {
+		case MAV_TYPE_GENERIC: return MAV_ODID_UA_TYPE_OTHER;
+
+		case MAV_TYPE_FIXED_WING: return MAV_ODID_UA_TYPE_AEROPLANE;
+
+		case MAV_TYPE_QUADROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_COAXIAL: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_HELICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_ANTENNA_TRACKER: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_GCS: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_AIRSHIP: return MAV_ODID_UA_TYPE_AIRSHIP;
+
+		case MAV_TYPE_FREE_BALLOON: return MAV_ODID_UA_TYPE_FREE_BALLOON;
+
+		case MAV_TYPE_ROCKET: return MAV_ODID_UA_TYPE_ROCKET;
+
+		case MAV_TYPE_GROUND_ROVER: return MAV_ODID_UA_TYPE_GROUND_OBSTACLE;
+
+		case MAV_TYPE_SURFACE_BOAT: return MAV_ODID_UA_TYPE_OTHER;
+
+		case MAV_TYPE_SUBMARINE: return MAV_ODID_UA_TYPE_OTHER;
+
+		case MAV_TYPE_HEXAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_OCTOROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_TRICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_FLAPPING_WING: return MAV_ODID_UA_TYPE_ORNITHOPTER;
+
+		case MAV_TYPE_KITE: return MAV_ODID_UA_TYPE_KITE;
+
+		case MAV_TYPE_ONBOARD_CONTROLLER: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_TILTROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_FIXEDROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_TAILSITTER: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_RESERVED4: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_RESERVED5: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_GIMBAL: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_ADSB: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_PARAFOIL: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
+
+		case MAV_TYPE_DODECAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_CAMERA: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_CHARGING_STATION: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_FLARM: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_SERVO: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_ODID: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_DECAROTOR: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_BATTERY: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_PARACHUTE: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
+
+		case MAV_TYPE_LOG: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_OSD: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_IMU: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_GPS: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_WINCH: return MAV_ODID_UA_TYPE_NONE;
+
+		default: return MAV_ODID_UA_TYPE_OTHER;
+		}
+	}
+
+
+	bool send() override
+	{
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.update(&vehicle_status)) {
+
+			mavlink_open_drone_id_basic_id_t msg{};
+
+			msg.target_system = 0; // 0 for broadcast
+			msg.target_component = 0; // 0 for broadcast
+			// msg.id_or_mac // Only used for drone ID data received from other UAs.
+
+			// id_type: MAV_ODID_ID_TYPE
+			msg.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER;
+
+			// ua_type: MAV_ODID_UA_TYPE
+			msg.ua_type = odidTypeForMavType(vehicle_status.system_type);
+
+			// uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type
+			// TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format
+			board_get_px4_guid_formated((char *)(msg.uas_id), sizeof(msg.uas_id));
+
+			mavlink_msg_open_drone_id_basic_id_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // OPEN_DRONE_ID_BASIC_ID_HPP
@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef OPEN_DRONE_ID_LOCATION
+#define OPEN_DRONE_ID_LOCATION
+
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+class MavlinkStreamOpenDroneIdLocation : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdLocation(mavlink); }
+
+	static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_LOCATION"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	bool send() override
+	{
+		mavlink_open_drone_id_location_t msg{};
+		msg.target_component = 0; // 0 for broadcast
+		msg.target_system = 0; // 0 for broadcast
+		// msg.id_or_mac // Only used for drone ID data received from other UAs.
+
+		// initialize all fields to unknown
+		msg.status = MAV_ODID_STATUS_UNDECLARED;
+		msg.direction = 36100; // If unknown: 36100 centi-degrees
+		msg.speed_horizontal = 25500; // If unknown: 25500 cm/s
+		msg.speed_vertical = 6300; // If unknown: 6300 cm/s
+		msg.latitude = 0; // If unknown: 0
+		msg.longitude = 0; // If unknown: 0
+		msg.altitude_geodetic = -1000; // If unknown: -1000 m
+		msg.altitude_geodetic = -1000; // If unknown: -1000 m
+		msg.height = -1000; // If unknown: -1000 m
+		msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
+		msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+		msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+		msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
+		msg.timestamp = 0xFFFF; // If unknown: 0xFFFF
+		msg.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN;
+
+		bool updated = false;
+
+		// status: MAV_ODID_STATUS_GROUND/MAV_ODID_STATUS_AIRBORNE
+		if (_vehicle_land_detected_sub.advertised()) {
+			vehicle_land_detected_s vehicle_land_detected{};
+
+			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
+			    && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 10_s)) {
+				if (vehicle_land_detected.landed) {
+					msg.status = MAV_ODID_STATUS_GROUND;
+
+				} else {
+					msg.status = MAV_ODID_STATUS_AIRBORNE;
+				}
+
+				updated = true;
+			}
+		}
+
+		// status: MAV_ODID_STATUS_EMERGENCY
+		if (_vehicle_status_sub.advertised()) {
+			vehicle_status_s vehicle_status{};
+
+			if (_vehicle_status_sub.copy(&vehicle_status) && hrt_elapsed_time(&vehicle_status.timestamp) < 10_s) {
+				if (vehicle_status.failsafe && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+					msg.status = MAV_ODID_STATUS_EMERGENCY;
+					updated = true;
+				}
+			}
+		}
+
+		if (_vehicle_gps_position_sub.advertised()) {
+			sensor_gps_s vehicle_gps_position{};
+
+			if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)
+			    && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 10_s)) {
+
+				if (vehicle_gps_position.vel_ned_valid) {
+					const matrix::Vector3f vel_ned{vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s};
+
+					// direction: calculate GPS course over ground angle
+					const float course = atan2f(vel_ned(1), vel_ned(0));
+					const int course_deg = roundf(math::degrees(matrix::wrap_2pi(course)));
+					msg.direction = math::constrain(100 * course_deg, 0, 35999); // 0 - 35999 centi-degrees
+
+					// speed_horizontal: If speed is larger than 25425 cm/s, use 25425 cm/s.
+					const int speed_horizontal_cm_s = matrix::Vector2f(vel_ned).length() * 100.f;
+					msg.speed_horizontal = math::constrain(speed_horizontal_cm_s, 0, 25425);
+
+					// speed_vertical: Up is positive, If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.
+					const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f);
+					msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200);
+
+					// speed_accuracy
+					if (vehicle_gps_position.s_variance_m_s < 0.3f) {
+						msg.speed_accuracy = MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND;
+
+					} else if (vehicle_gps_position.s_variance_m_s < 1.f) {
+						msg.speed_accuracy = MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND;
+
+					} else if (vehicle_gps_position.s_variance_m_s < 3.f) {
+						msg.speed_accuracy = MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND;
+
+					} else if (vehicle_gps_position.s_variance_m_s < 10.f) {
+						msg.speed_accuracy = MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND;
+
+					} else {
+						msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
+					}
+
+					updated = true;
+				}
+
+				if (vehicle_gps_position.fix_type >= 2) {
+					msg.latitude = vehicle_gps_position.lat;
+					msg.longitude = vehicle_gps_position.lon;
+
+					// altitude_geodetic
+					if (vehicle_gps_position.fix_type >= 3) {
+						msg.altitude_geodetic = vehicle_gps_position.alt * 1e-3f;
+					}
+
+					// horizontal_accuracy
+					if (vehicle_gps_position.eph < 1.f) {
+						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_1_METER;
+
+					} else if (vehicle_gps_position.eph < 3.f) {
+						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_3_METER;
+
+					} else if (vehicle_gps_position.eph < 10.f) {
+						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_10_METER;
+
+					} else if (vehicle_gps_position.eph < 30.f) {
+						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_30_METER;
+
+					} else {
+						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
+					}
+
+					// vertical_accuracy
+					if (vehicle_gps_position.epv < 1.f) {
+						msg.vertical_accuracy = MAV_ODID_VER_ACC_1_METER;
+
+					} else if (vehicle_gps_position.epv < 3.f) {
+						msg.vertical_accuracy = MAV_ODID_VER_ACC_3_METER;
+
+					} else if (vehicle_gps_position.epv < 10.f) {
+						msg.vertical_accuracy = MAV_ODID_VER_ACC_10_METER;
+
+					} else if (vehicle_gps_position.epv < 25.f) {
+						msg.vertical_accuracy = MAV_ODID_VER_ACC_25_METER;
+
+					} else if (vehicle_gps_position.epv < 45.f) {
+						msg.vertical_accuracy = MAV_ODID_VER_ACC_45_METER;
+
+					} else if (vehicle_gps_position.epv < 150.f) {
+						msg.vertical_accuracy = MAV_ODID_VER_ACC_150_METER;
+
+					} else {
+						msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+					}
+
+					updated = true;
+				}
+
+				if (vehicle_gps_position.time_utc_usec != 0) {
+					// timestamp: UTC then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000
+					uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000;
+					msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000;
+
+					if (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s) {
+						msg.timestamp_accuracy = MAV_ODID_TIME_ACC_1_0_SECOND; // TODO
+					}
+
+					updated = true;
+				}
+			}
+		}
+
+		// altitude_barometric: The altitude calculated from the barometric pressue
+		if (_vehicle_air_data_sub.advertised()) {
+			vehicle_air_data_s vehicle_air_data{};
+
+			if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) {
+				msg.altitude_barometric = vehicle_air_data.baro_alt_meter;
+				msg.barometer_accuracy = MAV_ODID_VER_ACC_150_METER; // TODO
+				updated = true;
+			}
+		}
+
+		// height: The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference
+		if (_home_position_sub.advertised() && _vehicle_local_position_sub.updated()) {
+			home_position_s home_position{};
+			vehicle_local_position_s vehicle_local_position{};
+
+			if (_home_position_sub.copy(&home_position)
+			    && _vehicle_local_position_sub.copy(&vehicle_local_position)
+			    && (hrt_elapsed_time(&vehicle_local_position.timestamp) < 1_s)
+			   ) {
+
+				if (home_position.valid_alt && vehicle_local_position.z_valid && vehicle_local_position.z_global) {
+					float altitude = (-vehicle_local_position.z + vehicle_local_position.ref_alt);
+
+					msg.height = altitude - home_position.alt;
+					msg.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF;
+					updated = true;
+				}
+			}
+		}
+
+		if (updated) {
+			mavlink_msg_open_drone_id_location_send_struct(_mavlink->get_channel(), &msg);
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // OPEN_DRONE_ID_LOCATION
@@ -36,6 +36,7 @@

 #include <qurt.h>
 #include <qurt_thread.h>
+#include <pthread.h>

 // TODO: Move this out of here once we have px4-log functionality
 extern "C" void HAP_debug(const char *msg, int level, const char *filename, int line);
@@ -45,7 +46,7 @@ static MUORBTestType test_to_run;

 fc_func_ptrs muorb_func_ptrs;

-static void test_runner(void *test)
+static void *test_runner(void *test)
 {
 	HAP_debug("test_runner called", 1, muorb_test_topic_name, 0);

@@ -74,7 +75,7 @@ static void test_runner(void *test)
 		break;
 	}

-	qurt_thread_exit(0);
+	return nullptr;
 }

 int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
@@ -93,14 +94,13 @@ char stack[TEST_STACK_SIZE];

 void run_test(MUORBTestType test)
 {
-	qurt_thread_t tid;
-	qurt_thread_attr_t attr;
-
-	qurt_thread_attr_init(&attr);
-	qurt_thread_attr_set_stack_addr(&attr, stack);
-	qurt_thread_attr_set_stack_size(&attr, TEST_STACK_SIZE);
+	pthread_t tid;
+	pthread_attr_t attr;
+	pthread_attr_init(&attr);
+	pthread_attr_setstacksize(&attr, TEST_STACK_SIZE);
 	test_to_run = test;
-	(void) qurt_thread_create(&tid, &attr, &test_runner, (void *) &test_to_run);
+	pthread_create(&tid, &attr, &test_runner, (void *) &test_to_run);
+	pthread_attr_destroy(&attr);
 }

 int px4muorb_topic_advertised(const char *topic_name)
@@ -149,7 +149,7 @@ VtolTakeoff::on_active()
 				_navigator->reset_position_setpoint(reposition_triplet->current);
 				_navigator->reset_position_setpoint(reposition_triplet->next);

-				// the VTOL takeoff is done, proceed loitering and upate the navigation state to LOITER
+				// the VTOL takeoff is done, proceed loitering and update the navigation state to LOITER
 				_navigator->get_mission_result()->finished = true;
 				_navigator->set_mission_result_updated();

@@ -362,35 +362,34 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 	}

 	// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver.
-	sensor_gps_s gps_blended_state{_gps_state[0]}; // start with best GPS for all other misc fields
-	gps_blended_state.eph = FLT_MAX;
-	gps_blended_state.epv = FLT_MAX;
-	gps_blended_state.s_variance_m_s = FLT_MAX;
-	gps_blended_state.vel_ned_valid = true;
-	gps_blended_state.hdop = FLT_MAX;
-	gps_blended_state.vdop = FLT_MAX;
+	sensor_gps_s gps_blended_state{_gps_state[gps_best_index]}; // start with best GPS for all other misc fields
+
+	// zerp all fields that are an accumulated blend below
+	gps_blended_state.timestamp = 0;
+	gps_blended_state.timestamp_sample = 0;
+	gps_blended_state.vel_m_s = 0;
+	gps_blended_state.vel_n_m_s = 0;
+	gps_blended_state.vel_e_m_s = 0;
+	gps_blended_state.vel_d_m_s = 0;

 	// combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights()
 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
-		// blend the timing data
-		gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]);
-		gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]);
-
-		// use the highest status
-		if (_gps_state[i].fix_type > gps_blended_state.fix_type) {
-			gps_blended_state.fix_type = _gps_state[i].fix_type;
-		}
-
 		// Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers
 		// If any receiver contributing has an invalid velocity, then report blended velocity as invalid
 		if (blend_weights[i] > 0.0f) {

+			// blend the timing data
+			gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]);
+			gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]);
+
 			// calculate a blended average speed and velocity vector
 			gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * blend_weights[i];
 			gps_blended_state.vel_n_m_s += _gps_state[i].vel_n_m_s * blend_weights[i];
 			gps_blended_state.vel_e_m_s += _gps_state[i].vel_e_m_s * blend_weights[i];
 			gps_blended_state.vel_d_m_s += _gps_state[i].vel_d_m_s * blend_weights[i];

+
+			// use the lowest value
 			if (_gps_state[i].eph > 0.0f
 			    && _gps_state[i].eph < gps_blended_state.eph) {
 				gps_blended_state.eph = _gps_state[i].eph;
@@ -416,13 +415,18 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 				gps_blended_state.vdop = _gps_state[i].vdop;
 			}

-			if (_gps_state[i].satellites_used > 0
-			    && _gps_state[i].satellites_used > gps_blended_state.satellites_used) {
+
+			// use the highest status
+			if (_gps_state[i].fix_type > gps_blended_state.fix_type) {
+				gps_blended_state.fix_type = _gps_state[i].fix_type;
+			}
+
+			if (_gps_state[i].satellites_used > gps_blended_state.satellites_used) {
 				gps_blended_state.satellites_used = _gps_state[i].satellites_used;
 			}

-			if (!_gps_state[i].vel_ned_valid) {
-				gps_blended_state.vel_ned_valid = false;
+			if (_gps_state[i].vel_ned_valid) {
+				gps_blended_state.vel_ned_valid = true;
 			}
 		}

@@ -473,7 +477,7 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 	gps_blended_state.alt += (int32_t)blended_alt_offset_mm;

 	// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
-	uint8_t gps_best_yaw_index = 0;
+	int8_t gps_best_yaw_index = -1;
 	float best_yaw_weight = 0.0f;

 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
@@ -483,8 +487,11 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 		}
 	}

-	gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading;
-	gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset;
+	if (gps_best_yaw_index >= 0)  {
+		gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading;
+		gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset;
+		gps_blended_state.heading_accuracy = _gps_state[gps_best_yaw_index].heading_accuracy;
+	}

 	return gps_blended_state;
 }
@@ -497,7 +504,7 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
 	float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f);

 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
-		if (_gps_state[i].timestamp - _time_prev_us[i] > 0) {
+		if (_gps_state[i].timestamp > _time_prev_us[i]) {
 			// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
 			alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
 					     0.0f, 1.0f);
@@ -211,6 +211,11 @@ TEST_F(GpsBlendingTest, dualReceiverBlendingHPos)
 	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
 	EXPECT_LT(gps_blending.getOutputGpsData().eph, gps_data0.eph);
 	EXPECT_FLOAT_EQ(gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than
+	EXPECT_EQ(gps_blending.getOutputGpsData().timestamp, gps_data0.timestamp);
+	EXPECT_EQ(gps_blending.getOutputGpsData().timestamp_sample, gps_data0.timestamp_sample);
+	EXPECT_EQ(gps_blending.getOutputGpsData().lat, gps_data0.lat);
+	EXPECT_EQ(gps_blending.getOutputGpsData().lon, gps_data0.lon);
+	EXPECT_EQ(gps_blending.getOutputGpsData().alt, gps_data0.alt);
 }

 TEST_F(GpsBlendingTest, dualReceiverFailover)
@@ -254,7 +254,7 @@ void VehicleMagnetometer::UpdateMagCalibration()
 	// Larger values cause a larger fraction of the learned biases to be used.
 	static constexpr float magb_vref = 2.5e-7f;
 	static constexpr float min_var_allowed = magb_vref * 0.01f;
-	static constexpr float max_var_allowed = magb_vref * 100.f;
+	static constexpr float max_var_allowed = magb_vref * 500.f;

 	if (_armed) {
 		static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);
@@ -369,9 +369,9 @@ void Standard::fill_actuator_outputs()
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;

 		// FW out = FW in, with VTOL transition controlling throttle and airbrakes
-		fw_out[actuator_controls_s::INDEX_ROLL]         = fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH]        = fw_in[actuator_controls_s::INDEX_PITCH];
-		fw_out[actuator_controls_s::INDEX_YAW]          = fw_in[actuator_controls_s::INDEX_YAW];
+		fw_out[actuator_controls_s::INDEX_ROLL]         = fw_in[actuator_controls_s::INDEX_ROLL] * (1.f - _mc_roll_weight);
+		fw_out[actuator_controls_s::INDEX_PITCH]        = fw_in[actuator_controls_s::INDEX_PITCH] * (1.f - _mc_pitch_weight);
+		fw_out[actuator_controls_s::INDEX_YAW]          = fw_in[actuator_controls_s::INDEX_YAW] * (1.f - _mc_yaw_weight);
 		fw_out[actuator_controls_s::INDEX_THROTTLE]     = _pusher_throttle;
 		fw_out[actuator_controls_s::INDEX_FLAPS]        = _flaps_setpoint_with_slewrate.getState();
 		fw_out[actuator_controls_s::INDEX_SPOILERS]    	= _spoiler_setpoint_with_slewrate.getState();
Author	SHA1	Message	Date
bresch	148678d27a	LTE: advertise published topic early to be logged	2022-10-07 11:45:55 +02:00
bresch	aa09918f0d	LTE: refactor to use multiple target estimator types	2022-10-06 18:57:14 +02:00
bresch	ecdc11edf6	LTE: use new param sub style	2022-10-06 16:56:52 +02: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