UAVCAN: publish CAN frames to uORB (ORB_ID(can_frame_out))

- can frames published to ORB_ID(can_frame_in) are injected
sensors: fix accel/gyro/mag calibration offset units
2026-05-29 00:50:05 +08:00 · 2022-10-09 13:18:38 -04:00 · 2022-10-09 13:02:38 -04:00 · 2022-10-09 12:41:06 -04:00 · 2022-10-06 09:49:05 -04:00 · 2022-10-06 08:44:36 -04:00
110 changed files with 3711 additions and 853 deletions
@@ -23,7 +23,6 @@ jobs:
          "shellcheck_all",
          "NO_NINJA_BUILD=1 px4_fmu-v5_default",
          "NO_NINJA_BUILD=1 px4_sitl_default",
-          "NO_NINJA_BUILD=1 px4_sitl_gps_base",
          "BUILD_MICRORTPS_AGENT=1 px4_sitl_rtps",
          "airframe_metadata",
          "module_documentation",
@@ -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
@@ -6,11 +6,6 @@ CONFIG:
      buildType: RelWithDebInfo
      settings:
        CONFIG: px4_sitl_default
-    px4_sitl_gps_base:
-      short: px4_sitl_gps_base
-      buildType: RelWithDebInfo
-      settings:
-        CONFIG: px4_sitl_gps_base
    px4_sitl_rtps:
      short: px4_sitl_rtps
      buildType: RelWithDebInfo
@@ -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

@@ -1,27 +0,0 @@
-CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_TESTING=y
-CONFIG_BOARD_ETHERNET=y
-CONFIG_DRIVERS_GPS=y
-CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_EVENTS=y
-CONFIG_MODULES_GPS_BASE_STATION=y
-CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAVLINK=y
-CONFIG_MODULES_REPLAY=y
-CONFIG_COMMON_SIMULATION=y
-CONFIG_SYSTEMCMDS_DYN=y
-CONFIG_SYSTEMCMDS_FAILURE=y
-CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SHUTDOWN=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_HELLO=y
-CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
-CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
-CONFIG_EXAMPLES_WORK_ITEM=y
@@ -56,6 +56,7 @@ set(msg_files
 	camera_capture.msg
 	camera_status.msg
 	camera_trigger.msg
+	can_frame.msg
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg
@@ -0,0 +1,15 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint32 id
+
+bool is_rtr
+bool is_extended
+bool is_error
+
+uint8 dlc
+
+uint8[8] data
+
+uint8 ORB_QUEUE_LENGTH = 8
+
+# TOPICS can_frame can_frame_out can_frame_in
@@ -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
@@ -96,6 +96,7 @@ uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    	# Command VTOL transition
 uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001	# Request arm authorization
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
+uint16 VEHICLE_CMD_CAN_FORWARD = 32000
 uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
 uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.

@@ -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);
@@ -130,6 +130,10 @@ UavcanNode::~UavcanNode()
 		} while (_instance);
 	}

+	_node.removeRxFrameListener();
+	delete _rx_frame_listener_uorb;
+	_rx_frame_listener_uorb = nullptr;
+
 	// Removing the sensor bridges
 	_sensor_bridges.clear();

@@ -498,6 +502,17 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)

 	fill_node_info();

+	// install frame listener
+	int32_t uavcan_frame_dbg = 0;
+	param_get(param_find("UAVCAN_FRAME_DBG"), &uavcan_frame_dbg);
+
+	if (uavcan_frame_dbg == 1) {
+		PX4_INFO("UAVCAN FRAME DBG enabled");
+		_rx_frame_listener_uorb = new RxFrameUorbPublisher();
+		_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
+		_mirror_to_uorb = true;
+	}
+
 	int ret = _beep_controller.init();

 	if (ret < 0) {
@@ -689,6 +704,22 @@ UavcanNode::Run()
 		update_params();
 	}

+	if (_mirror_to_uorb && _can_frame_in_sub.updated()) {
+		can_frame_s can_frame;
+
+		if (_can_frame_in_sub.copy(&can_frame)) {
+			uavcan::CanFrame frame{};
+			frame.id = can_frame.id;
+			memcpy(frame.data, can_frame.data, sizeof(can_frame.data));
+			frame.dlc = can_frame.dlc;
+
+			uavcan::MonotonicTime tx_deadline = _node.getMonotonicTime() + uavcan::MonotonicDuration::fromUSec(1000);
+			uint8_t iface_mask = 3;
+
+			_node.injectTxFrame(frame, tx_deadline, iface_mask, uavcan::CanTxQueue::Volatile);
+		}
+	}
+
 	// Check for parameter requests (get/set/list)
 	if (_param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
 		uavcan_parameter_request_s request{};
@@ -877,6 +908,13 @@ UavcanNode::Run()
 					break;
 				}
 			}
+
+		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_CAN_FORWARD) {
+			if (_rx_frame_listener_uorb == nullptr) {
+				_rx_frame_listener_uorb = new RxFrameUorbPublisher();
+				_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
+				_mirror_to_uorb = true;
+			}
 		}

 		if (acknowledge) {
@@ -74,6 +74,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/can_frame.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -315,4 +316,26 @@ private:
 	bool _check_fw{false};

 	UavcanServers   *_servers{nullptr};
+
+
+	struct RxFrameUorbPublisher : public uavcan::IRxFrameListener {
+		void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
+		{
+			can_frame_s can_frame{};
+			can_frame.id = frame.id;
+			//can_frame.iface_index = frame.iface_index;
+			can_frame.dlc = frame.dlc;
+			memcpy(&can_frame.data, &frame.data, sizeof(frame.data));
+			can_frame.timestamp = hrt_absolute_time();
+			_can_frame_pub.publish(can_frame);
+		}
+	private:
+		uORB::Publication<can_frame_s> _can_frame_pub{ORB_ID(can_frame_out)};
+	};
+
+	RxFrameUorbPublisher *_rx_frame_listener_uorb{nullptr};
+
+	uORB::Subscription _can_frame_in_sub{ORB_ID(can_frame_in)};
+
+	bool _mirror_to_uorb{false};
 };
@@ -77,6 +77,18 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
 */
 PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);

+/**
+ * UAVCAN CAN frame debug
+ *
+ * Publish UAVCAN CAN frames to ORB_ID(can_frame_out)
+ * CAN frames published to ORB_ID(can_frame_in) are injected into UAVCAN
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_FRAME_DBG, 0);
+
 /**
 * UAVCAN rangefinder minimum range
 *
@@ -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);
@@ -609,7 +609,7 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f);
 * @min 3
 * @max 180
 */
-PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
+PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 60);

 /**
 * Enable mag strength preflight check
@@ -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;
+}
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2021-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.
-#
-############################################################################
-
-px4_add_module(
-	MODULE modules__gps_base_station
-	MAIN gps_base_station
-	COMPILE_FLAGS
-		#-DDEBUG_BUILD
-	SRCS
-		GPSBaseStation.cpp
-		GPSBaseStation.hpp
-	DEPENDS
-		px4_work_queue
-	)
@@ -1,121 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021-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 "GPSBaseStation.hpp"
-
-GPSBaseStation::GPSBaseStation() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-}
-
-GPSBaseStation::~GPSBaseStation()
-{
-	perf_free(_loop_interval_perf);
-}
-
-bool GPSBaseStation::init()
-{
-	ScheduleOnInterval(INTERVAL_US);
-	return true;
-}
-
-void GPSBaseStation::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	perf_count(_loop_interval_perf);
-}
-
-int GPSBaseStation::task_spawn(int argc, char *argv[])
-{
-	GPSBaseStation *instance = new GPSBaseStation();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int GPSBaseStation::print_status()
-{
-	perf_print_counter(_loop_interval_perf);
-	return 0;
-}
-
-int GPSBaseStation::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int GPSBaseStation::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Simple online gyroscope calibration.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("gyro_calibration", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int gps_base_station_main(int argc, char *argv[])
-{
-	return GPSBaseStation::main(argc, argv);
-}
@@ -1,5 +0,0 @@
-menuconfig MODULES_GPS_BASE_STATION
-	bool "gps_base_station"
-	default n
-	---help---
-		Enable support for gps_base_station
@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
-
-/**
-* IMU gyro auto calibration enable.
-*
-* @unit m
-* @reboot_required true
-* @group GPS_Base_Station
-*/
-PARAM_DEFINE_INT32(GPS_BASE_ACC_LIM, 2);
@@ -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);
@@ -1709,6 +1711,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE_TARGET", 8.0f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
+		configure_stream_local("CAN_FRAME", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
 		configure_stream_local("EFI_STATUS", 10.0f);
 		configure_stream_local("ESC_INFO", 10.0f);
@@ -1723,10 +1726,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 +1739,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 +1810,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
@@ -66,6 +66,7 @@
 #include "streams/BATTERY_STATUS.hpp"
 #include "streams/CAMERA_IMAGE_CAPTURED.hpp"
 #include "streams/CAMERA_TRIGGER.hpp"
+#include "streams/CAN_FRAME.hpp"
 #include "streams/COLLISION.hpp"
 #include "streams/COMMAND_LONG.hpp"
 #include "streams/COMPONENT_INFORMATION.hpp"
@@ -95,6 +96,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"
@@ -472,6 +475,9 @@ static const StreamListItem streams_list[] = {
 #if defined(NAV_CONTROLLER_OUTPUT_HPP)
 	create_stream_list_item<MavlinkStreamNavControllerOutput>(),
 #endif // NAV_CONTROLLER_OUTPUT_HPP
+#if defined(CAN_FRAME_HPP)
+	create_stream_list_item<MavlinkStreamCanFrame>(),
+#endif // CAN_FRAME_HPP
 #if defined(CAMERA_TRIGGER_HPP)
 	create_stream_list_item<MavlinkStreamCameraTrigger>(),
 #endif // CAMERA_TRIGGER_HPP
@@ -517,6 +523,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
@@ -234,6 +234,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_battery_status(msg);
 		break;

+	case MAVLINK_MSG_ID_CAN_FRAME:
+		handle_message_can_frame(msg);
+		break;
+
 	case MAVLINK_MSG_ID_SERIAL_CONTROL:
 		handle_message_serial_control(msg);
 		break;
@@ -369,7 +373,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 }

 bool
-MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component)
+MavlinkReceiver::evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component)
 {
 	/* evaluate if this system should accept this command */
 	bool target_ok = false;
@@ -382,6 +386,11 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c
 		target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
 		break;

+	case MAV_CMD_CAN_FORWARD:
+		// MAV_COMP_ID_MAVCAN
+		target_ok = ((target_system == 0) || (target_system == mavlink_system.sysid)) && (msg->compid == MAV_COMP_ID_MAVCAN);
+		break;
+
 	default:
 		target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid)
 				|| (target_component == MAV_COMP_ID_ALL));
@@ -482,12 +491,18 @@ template <class T>
 void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
 		const vehicle_command_s &vehicle_command)
 {
-	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
+	bool target_ok = evaluate_target_ok(msg, cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
 	bool send_ack = true;
 	uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 	uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility

 	if (!target_ok) {
+
+		if (cmd_mavlink.command == MAV_CMD_CAN_FORWARD) {
+			PX4_WARN("cmd:%d from SYSID:%d, COMPID:%d, target_ok:%d, target_system:%d, target_component:%d", cmd_mavlink.command,
+				 msg->sysid, msg->compid, target_ok, cmd_mavlink.target_system, cmd_mavlink.target_component);
+		}
+
 		// Reject alien commands only if there is no forwarding or we've never seen target component before
 		if (!_mavlink->get_forwarding_on()
 		    || !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
@@ -561,6 +576,18 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			send_ack = true;
 		}

+	} else if (cmd_mavlink.command == MAV_CMD_CAN_FORWARD) {
+
+		//PX4_INFO("cmd MAV_CMD_CAN_FORWARD");
+
+		// 1: bus	Bus number (0 to disable forwarding, 1 for first bus, 2 for 2nd bus, 3 for 3rd bus).
+
+		if (!_can_frame_pub.advertised()) {
+			PX4_INFO("MAV_CMD_CAN_FORWARD: enabling CAN_FRAME stream");
+			const char stream_name[] {"CAN_FRAME"};
+			_mavlink->configure_stream_threadsafe(stream_name, -1.f);
+		}
+
 	} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {

 		bool has_module = true;
@@ -1834,6 +1861,30 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
 	_battery_pub.publish(battery_status);
 }

+void
+MavlinkReceiver::handle_message_can_frame(mavlink_message_t *msg)
+{
+	if (!_can_frame_pub.advertised()) {
+		PX4_INFO("enabling CAN_FRAME stream");
+		const char stream_name[] {"CAN_FRAME"};
+		_mavlink->configure_stream_threadsafe(stream_name, -1.f);
+	}
+
+	mavlink_can_frame_t mavlink_can_frame;
+	mavlink_msg_can_frame_decode(msg, &mavlink_can_frame);
+
+	can_frame_s can_frame{};
+	//msg.target_system = 0;
+	//msg.target_component = 0;
+	//can_frame.bus = mavlink_can_frame.bus;
+	can_frame.id = mavlink_can_frame.id;
+	can_frame.dlc = mavlink_can_frame.len;
+	memcpy(can_frame.data, mavlink_can_frame.data, sizeof(mavlink_can_frame.data));
+
+	can_frame.timestamp = hrt_absolute_time();
+	_can_frame_pub.publish(can_frame);
+}
+
 void
 MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
 {
@@ -2206,6 +2257,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 +3070,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);
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2021 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
@@ -67,6 +67,7 @@
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/camera_status.h>
+#include <uORB/topics/can_frame.h>
 #include <uORB/topics/cellular_status.h>
 #include <uORB/topics/collision_report.h>
 #include <uORB/topics/differential_pressure.h>
@@ -160,6 +161,7 @@ private:
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_att_pos_mocap(mavlink_message_t *msg);
 	void handle_message_battery_status(mavlink_message_t *msg);
+	void handle_message_can_frame(mavlink_message_t *msg);
 	void handle_message_cellular_status(mavlink_message_t *msg);
 	void handle_message_collision(mavlink_message_t *msg);
 	void handle_message_command_ack(mavlink_message_t *msg);
@@ -228,7 +230,7 @@ private:
 	int set_message_interval(int msgId, float interval, int data_rate = -1);
 	void get_message_interval(int msgId);

-	bool evaluate_target_ok(int command, int target_system, int target_component);
+	bool evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component);

 	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);

@@ -294,6 +296,7 @@ private:
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
+	uORB::Publication<can_frame_s>                          _can_frame_pub{ORB_ID(can_frame_in)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
@@ -386,6 +389,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};
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	b491356d3f	UAVCAN: publish CAN frames to uORB (ORB_ID(can_frame_out)) - can frames published to ORB_ID(can_frame_in) are injected	2022-10-09 13:18:38 -04:00
Daniel Agar	f3964513c7	sensors: fix accel/gyro/mag calibration offset units	2022-10-09 13:02:38 -04:00
Daniel Agar	ee4821ed0a	commander: relax COM_ARM_MAG_ANG default to minimize false positives - this check is mainly intended for catching blatant setup problems like a magnetometer that's mounted 90 or 180 degrees off	2022-10-09 12:41:06 -04:00
fkaiser	30150f723a	Tools/px_uploader.py: use monotonic clock if available (#20352 ) Signed-off-by: fkaiser <fabian.kaiser@wingtra.com>	2022-10-06 09:49:05 -04:00
alexklimaj	064f3f86bc	Increase mag calibration max_var_allowed	2022-10-06 08:44:36 -04:00
Hamish Willee	022aa13aa1	mpu9250_i2c - also needs docs update	2022-10-05 20:26:25 -04:00
Hamish Willee	6d612b1ba4	MPU9250 driver doc incorrect	2022-10-05 20:26:25 -04:00
Daniel Agar	8cc39096cb	load_mon: NuttX cpuload use system times for calculation - this is to minimize the impact of any load_mon scheduling jitter in the sampled load percentage	2022-10-05 14:21:38 -04:00
benjinne	29b031c862	Tools/setup/ubuntu.sh update comment to include 22.04	2022-10-05 13:41:47 -04:00
Daniel Agar	5f0a539633	gps_blending: fix blending init to best instance - output GPS publication defaults to best input instance - blended states are explicitly cleared and then populated with weighted blend	2022-10-05 10:26:48 -04:00
Beat Küng	f79dad1e63	fix ROMFS: run camera trigger, capture and PPS before pwm_out As they might need to reserve the pwm pins.	2022-10-05 09:13:35 -04:00
Matthias Grob	46e6e83e14	Commander: allow disarmin not-landed boat like rover This is a hotfix and a boat should never even detect a takeoff in normal operation.	2022-10-05 09:07:18 -04:00
Beat Küng	dafead6f20	logger: add compressed events metadata file if it exists in the ROMFS Allows to decode events not (yet) on main. The file is currently ~15KB.	2022-10-05 07:44:55 +02:00
Beat Küng	02035d94aa	metadata: sort json output ensures the files don't change arbitrarily	2022-10-05 07:44:55 +02:00
modaltb	06a0aedbdb	modalai_esc: directly using deadband param value instead of converting it to RPM	2022-10-04 20:14:59 -04:00
Thomas Debrunner	0af87ec745	mavlink: initial OPEN_DRONE_ID_BASIC_ID/OPEN_DRONE_ID_LOCATION support	2022-10-04 14:40:59 -04:00
Matthias Grob	b179427b4c	Remove opinionated non-default MPC_TKO_SPEED from airframes	2022-10-04 11:36:57 -04:00
pandafeng1999	e4b4df4e5d	drivers/distance_sensor/gy_us42: add the specified i2c address	2022-10-04 11:35:45 -04:00
Matthias Grob	f96507bb22	vtol_takeoff: fix comment typo	2022-10-04 10:01:04 -04:00
Peter van der Perk	c807d6079d	FMUK66-V3 Enable DMA on SPI	2022-10-04 09:46:04 -04:00
Peter van der Perk	0cf2ecedb9	FMUK66 Disable Telnet to save RAM	2022-10-04 09:46:04 -04:00
Julian Oes	5c77bbcb4c	ms5611: ignore reading 0 This prevents publishing a negative pressure which leads to a NAN altitude estimate further down the line.	2022-10-04 09:37:53 -04:00
Beat Küng	04b1cbb423	vtol_att_control: standard: also do blending for FW controls So that the sum of the control is always 1.	2022-10-04 09:54:11 +02:00
Silvan Fuhrer	6db92b4011	ControlAllocation: remove actuator trim value from actuator_sp to calculate allocated control Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-04 07:51:25 +02:00
Zachary Lowell	4520186878	qurt: replacing qurt threads with pthread	2022-10-03 18:01:04 -04:00
bresch	4b687beb3b	ekf2: remove old airspeed fusion code	2022-10-03 10:59:42 -04:00
bresch	299e6058e3	ekf2_test: test airspeed symforce vs sympy generated code	2022-10-03 10:59:42 -04:00
bresch	29ebef1f74	ekf2: migrate fuse_airspeed to SymForce	2022-10-03 10:59:42 -04:00
Daniel Agar	41cda14126	ekf2: add symforce code generation helper target	2022-10-03 10:59:42 -04:00
Daniel Agar	c9441bb48a	boards: px4_fmu-v2/v3 allow optional mpu9250 probe to fail without error	2022-10-02 21:40:18 -04:00
Benjamin Perseghetti	aa2b47845a	new Gazebo simulation handle older gz versions (#20342 )	2022-10-02 21:30:30 -04:00
PX4 BuildBot	f4d2e176ae	Update submodule sitl_gazebo to latest Sun Oct 2 12:38:54 UTC 2022 - sitl_gazebo in PX4/Firmware (`2bd1ac005f`): https://github.com/PX4/PX4-SITL_gazebo/commit/b968405a617005ba0a793a8b4703913d4c6eff5f - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/e80432759540c91f85a012f47aa6ebb0ee9de7e4 - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/b968405a617005ba0a793a8b4703913d4c6eff5f...e80432759540c91f85a012f47aa6ebb0ee9de7e4 e804327 2022-10-01 Thomas Stauber - set lidar measurements to 0.0 if out of range (#914) 7ed1da6 2022-09-27 Karthik S - Advanced Lift-Drag Plugin (#901) 669d7f5 2022-09-08 Oleg Kalachev - Fix local addr assignment for qgc and sdk sockets	2022-10-02 10:54:31 -04:00
PX4 BuildBot	f7a5c91fb3	Update submodule libcanard to latest Sun Oct 2 12:38:56 UTC 2022 - libcanard in PX4/Firmware (8e144eece9e9cd5a23dbeb6723591f214cad631a): https://github.com/opencyphal/libcanard/commit/db87ea32aa092c48ea103963138b6346dd3e9008 - libcanard current upstream: https://github.com/opencyphal/libcanard/commit/2e3b11f6b8325080c160d38521b169b0bbb6b1c7 - Changes: https://github.com/opencyphal/libcanard/compare/db87ea32aa092c48ea103963138b6346dd3e9008...2e3b11f6b8325080c160d38521b169b0bbb6b1c7 2e3b11f 2022-07-15 Pavel Kirienko - Follow-up for #197 -- fix minor issues discovered by Sonar (#198) 106ceaf 2022-07-11 Dmitry Ponomarev - Fix cast-align warnings on ARM 8953fe6 2022-07-06 Dmitry Ponomarev - Add ARM build to github workflow	2022-10-02 10:54:05 -04:00
PX4 BuildBot	644a836d0a	Update submodule libevents to latest Sun Oct 2 12:39:02 UTC 2022 - libevents in PX4/Firmware (ddc1e6d6c5c2707be5522d4c4d7434403202cf45): https://github.com/mavlink/libevents/commit/82dabdb914d7bd640c281900e2852d0afc074b68 - libevents current upstream: https://github.com/mavlink/libevents/commit/a89808bffd1efb0543e66f17a7fa12dfce5e6bf0 - Changes: https://github.com/mavlink/libevents/compare/82dabdb914d7bd640c281900e2852d0afc074b68...a89808bffd1efb0543e66f17a7fa12dfce5e6bf0 a89808b 2022-09-26 Beat Küng - combine.py: sort json keys	2022-10-02 10:53:28 -04:00
PX4 BuildBot	cf0cd4ebf2	Update submodule mavlink to latest Sun Oct 2 12:39:04 UTC 2022 - mavlink in PX4/Firmware (65bd1fd4c000a7fa0de2a0e8dce1ba9b2a62fa86): https://github.com/mavlink/mavlink/commit/c46af523263ff0dad59afe018fe387c1df030442 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/dda5a18ddb002a871ba301bb584893ee6378e2f3 - Changes: https://github.com/mavlink/mavlink/compare/c46af523263ff0dad59afe018fe387c1df030442...dda5a18ddb002a871ba301bb584893ee6378e2f3 dda5a18d 2022-09-28 WickedShell - Support a return to default speed for DO_CHANGE_SPEED (#1890) 4e04dbbf 2022-09-24 Hamish Willee - MAV_FRAME_BODY_FRD - match to implementations and sanity (#1894) 89a61214 2022-09-23 Hamish Willee - DO_CHANGE_SPEED - clarify lifetime of speed setting (#1892) 11414f88 2022-09-21 Søren Friis - Clarify swarm fields (#1891) 5cdf8b60 2022-09-08 Hamish Willee - HOME_POSITION - clarify local frame means NED (#1886) 551e3b49 2022-09-07 Julian Oes - Fix CI (#1888) d99ee9b6 2022-09-07 Julian Oes - pymavlink: update submodule to latest master (#1887) 92d2a58d 2022-09-01 Hamish Willee - common.xml:TIMESYNC docs and extensions (#1878) c424dc65 2022-09-01 Julian Oes - minimal: try to make component ID less confusing (#1876) 153939bd 2022-09-01 Hamish Willee - Fix typo MAV_CMD_DO_SET_MISSION_CURRENT 5043a1e0 2022-09-01 Hamish Willee - MAV_CMD_DO_SET_MISSION_CURRENT - allow mission resetting (#1870) 90aa7bfc 2022-08-31 Chikirev Sirguy - Update common.xml (#1815) 554ccadd 2022-08-31 Hamish Willee - common: OpenDroneID messages no longer WIP (#1882) 06448cf6 2022-08-24 Hamish Willee - MAV_ODID_ARM_STATUS entries update to naming convention (#1883) ff649224 2022-08-18 Patrick José Pereira - Fix gibberish descriptions ualberta.xml (#1881) 11f4b236 2022-08-18 Hamish Willee - Mission item - document values for autocontinue (#1868) fb8ac31b 2022-08-18 Julian Oes - development: remove MISSION_CHANGED (#1879) 76b794c2 2022-08-18 Julian Oes - common: extend MISSION_CURRENT (#1869) 1ae3edc4 2022-08-15 Andrew Tridgell - common: added OPEN_DRONE_ID_SYSTEM_UPDATE message (#1880) aa25f4df 2022-08-11 Andrew Tridgell - common: added OPEN_DRONE_ID_ARM_STATUS (#1873)	2022-10-02 10:53:00 -04:00
Daniel Agar	2bd1ac005f	.gitmodules update branches master -> main	2022-10-01 12:22:22 -04:00