Basic gps base station app

Start gps base board config
2026-06-04 13:10:04 +08:00 · 2022-09-30 21:41:17 -06:00 · 2022-09-30 20:51:58 -06:00
110 changed files with 854 additions and 3712 deletions
@@ -23,6 +23,7 @@ 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 = main
+	branch = master
 [submodule "Tools/simulation/gazebo/sitl_gazebo"]
 	path = Tools/simulation/gazebo/sitl_gazebo
 	url = https://github.com/PX4/PX4-SITL_gazebo.git
-	branch = main
+	branch = master
 [submodule "src/drivers/gps/devices"]
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/PX4-GPSDrivers.git
-	branch = main
+	branch = master
 [submodule "src/modules/micrortps_bridge/micro-CDR"]
 	path = src/modules/micrortps_bridge/micro-CDR
 	url = https://github.com/PX4/Micro-CDR.git
@@ -53,7 +53,6 @@
 [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,6 +6,11 @@ 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,20 +41,6 @@ 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,6 +38,7 @@ 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,6 +65,7 @@ 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,6 +63,7 @@ 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,6 +64,7 @@ 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,6 +53,7 @@ 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,6 +62,7 @@ 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,27 +324,6 @@ 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.
@@ -411,6 +390,12 @@ 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
 	#
@@ -431,6 +416,21 @@ 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, sort_keys=True)
+    json.dump(actuators, outfile, indent=indent)

 if compress:
    save_compressed(output_file)
@@ -77,23 +77,11 @@ 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):
@@ -242,7 +230,7 @@ class uploader(object):

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

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

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

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

            usualEraseDuration = 15.0
-            estimatedTimeRemaining = deadline-_time()
+            estimatedTimeRemaining = deadline-time.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()))
+                sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()))
                sys.stdout.flush()

            if self.__trySync():
@@ -584,7 +572,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()
+        start = time.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'))
@@ -680,7 +668,7 @@ class uploader(object):
        print("\nRebooting.", end='')
        self.__reboot()
        self.port.close()
-        print(" Elapsed Time %3.3f\n" % (_time() - start))
+        print(" Elapsed Time %3.3f\n" % (time.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 (22.04, 20.04, 18.04).
+## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
 ## Can also be used in docker.
 ##
 ## Installs:
@@ -1,11 +0,0 @@
-<?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>
@@ -1,76 +0,0 @@
-<?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>
@@ -1,11 +0,0 @@
-<?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>
@@ -1,516 +0,0 @@
-<?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,71 +2,7 @@
 <sdf version='1.9'>
  <model name='x500'>
    <include merge='true'>
-      <uri>model://x500-NoPlugin</uri>
+      <uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</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,6 +127,7 @@ 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
@@ -156,6 +157,7 @@ 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,6 +165,8 @@ 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
@@ -200,6 +204,10 @@ 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,9 +95,7 @@ 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
@@ -127,6 +125,7 @@ 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
@@ -156,6 +155,7 @@ 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,6 +163,8 @@ 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
@@ -200,6 +202,10 @@ 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 -q start
+		mpu9250 -s -b 1 -R 14 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 -q start
+		mpu9250 -s -b 1 -R 14 start
 	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
 	fi

@@ -0,0 +1,27 @@
+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,7 +56,6 @@ set(msg_files
 	camera_capture.msg
 	camera_status.msg
 	camera_trigger.msg
-	can_frame.msg
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg
@@ -1,15 +0,0 @@
-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,7 +45,6 @@ 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
@@ -59,5 +58,4 @@ 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,7 +96,6 @@ 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,11 +112,10 @@ 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,6 +100,7 @@ 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
-		motor_output = (motor_output < _rpm_turtle_min + _parameters.turtle_motor_deadband) ? 0.0f :
-			       (motor_output - _parameters.turtle_motor_deadband);
+		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);

 		// using the output map here for clarity as PX4 motors are 1-4
 		switch (_output_map[i].number) {
@@ -253,18 +253,6 @@ 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,7 +54,6 @@ 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("mpu9250_i2c", "driver");
+	PRINT_MODULE_USAGE_NAME("mpu9520_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("mpu9250", "driver");
+	PRINT_MODULE_USAGE_NAME("mpu9520", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
@@ -130,10 +130,6 @@ UavcanNode::~UavcanNode()
 		} while (_instance);
 	}

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

@@ -502,17 +498,6 @@ 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) {
@@ -704,22 +689,6 @@ 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{};
@@ -908,13 +877,6 @@ 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,7 +74,6 @@
 #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>
@@ -316,26 +315,4 @@ 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,18 +77,6 @@ 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,7 +90,6 @@ 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, sort_keys=True)
+    json.dump(component_general, outfile)

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

 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')
@@ -31,16 +30,6 @@ 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")
@@ -49,12 +38,10 @@ 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, sort_keys=True)
+        self.output = json.dumps(all_json,indent=2)


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

@@ -50,7 +50,9 @@ 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_vehicle(_vehicle_status));
+				     || is_ground_rover(_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,23 +3022,6 @@ 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;
@@ -3089,17 +3072,6 @@ 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,11 +296,9 @@ 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,16 +123,6 @@ 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,8 +56,6 @@ 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, 60);
+PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);

 /**
 * Enable mag strength preflight check
@@ -143,7 +143,7 @@ public:
 	 * @return Control vector
 	 */
 	matrix::Vector<float, NUM_AXES> getAllocatedControl() const
-	{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }
+	{ return (_effectiveness * _actuator_sp).emult(_control_allocation_scale); }

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

 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,10 +43,6 @@
 */

 #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
@@ -54,17 +50,38 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
 	// reset flags
 	resetEstimatorAidStatusFlags(airspeed);

-	// Variance for true airspeed measurement - (m/sec)^2
-	const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
-			   math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
+	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

-	float innov = 0.f;
-	float innov_var = 0.f;
-	sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
+	// 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));
+
+	// 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;

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

 	airspeed.fusion_enabled = _control_status.flags.fuse_aspd;
@@ -81,9 +98,34 @@ 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) {
@@ -108,22 +150,31 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed)
 		return;
 	}

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

-	Vector24f H; // Observation jacobian
-	Vector24f K; // Kalman gain vector
+	// 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;

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

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

-	const bool is_fused = measurementUpdate(K, H_sparse, airspeed.innovation);
+	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);

 	airspeed.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;
@@ -1,106 +0,0 @@
-#!/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"])
@@ -1,101 +0,0 @@
-#!/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)
@@ -1 +0,0 @@
-*.bak
@@ -1,116 +0,0 @@
-// -----------------------------------------------------------------------------
-// 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
@@ -1,74 +0,0 @@
-// -----------------------------------------------------------------------------
-// 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
@@ -0,0 +1,227 @@
+#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;
+}
@@ -0,0 +1,61 @@
+// 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
+
+
@@ -0,0 +1,55 @@
+// 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
+
+
@@ -0,0 +1,25 @@
+// 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,7 +37,6 @@ 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)
@@ -1,203 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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
+	)
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   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,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,53 +31,46 @@
 *
 ****************************************************************************/

-#ifndef CAN_FRAME_HPP
-#define CAN_FRAME_HPP
+#pragma once

-#include <uORB/topics/can_frame.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/perf/perf_counter.h>
+#include <drivers/gps/devices/src/base_station.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/gps_inject_data.h>

-class MavlinkStreamCanFrame : public MavlinkStream
+using namespace time_literals;
+
+class GPSBaseStation : public ModuleBase<GPSBaseStation>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCanFrame(mavlink); }
+	GPSBaseStation();
+	~GPSBaseStation() override;

-	static constexpr const char *get_name_static() { return "CAN_FRAME"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CAN_FRAME; }
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);

-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);

-	unsigned get_size() override
-	{
-		return _can_frame_out_sub.advertised() ? MAVLINK_MSG_ID_CAN_FRAME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-	}
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	int print_status() override;

 private:
-	explicit MavlinkStreamCanFrame(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+	static constexpr hrt_abstime INTERVAL_US = 20000_us;

-	uORB::Subscription _can_frame_out_sub{ORB_ID(can_frame_out)};
+	void Run() override;

-	bool send() override
-	{
-		can_frame_s can_frame;
+	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};

-		if (_can_frame_out_sub.update(&can_frame)) {
-			mavlink_can_frame_t msg{};
-
-			msg.target_system = 0;
-			msg.target_component = 0;
-			msg.bus = 0;
-			msg.len = can_frame.dlc;
-			msg.id = can_frame.id;
-			memcpy(msg.data, can_frame.data, sizeof(msg.data));
-
-			mavlink_msg_can_frame_send_struct(_mavlink->get_channel(), &msg);
-
-			return true;
-		}
-
-		return false;
-	}
+	perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
 };
-
-#endif // CAN_FRAME_HPP
@@ -0,0 +1,5 @@
+menuconfig MODULES_GPS_BASE_STATION
+	bool "gps_base_station"
+	default n
+	---help---
+		Enable support for gps_base_station
@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   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-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 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,23 +140,17 @@ 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 = system_load.tasks[0].curr_start_time;
-		leave_critical_section(irqstate);
+		_last_idle_time_sample = hrt_absolute_time();
 		return;
 	}

 	irqstate_t irqstate = enter_critical_section();
-	const hrt_abstime now = system_load.tasks[0].curr_start_time;
+	const hrt_abstime now = hrt_absolute_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-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 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,5 +48,4 @@ px4_add_module(
 		watchdog.cpp
 	DEPENDS
 		version
-		component_general_json # for checksums.h
 	)
@@ -64,7 +64,6 @@
 #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

@@ -1417,7 +1416,6 @@ 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);
 	}

@@ -1475,7 +1473,6 @@ 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);
@@ -1888,56 +1885,6 @@ 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");
@@ -2328,25 +2275,6 @@ 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,7 +253,6 @@ 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);

@@ -265,7 +264,6 @@ 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[1200]; ///< String with the key and value information
+	char key_value_str[255]; ///< 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_QUATERNION", 10.0f);
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
+		configure_stream_local("ATTITUDE_QUATERNION", 10.0f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
@@ -1511,17 +1511,16 @@ 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);
@@ -1575,8 +1574,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);
@@ -1584,7 +1583,6 @@ 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);
@@ -1711,7 +1709,6 @@ 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);
@@ -1726,10 +1723,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);
@@ -1739,7 +1736,6 @@ 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);
@@ -1810,7 +1806,6 @@ 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-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 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,7 +66,6 @@
 #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"
@@ -96,8 +95,6 @@
 #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"
@@ -475,9 +472,6 @@ 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
@@ -523,12 +517,6 @@ 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,10 +234,6 @@ 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;
@@ -373,7 +369,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 }

 bool
-MavlinkReceiver::evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component)
+MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component)
 {
 	/* evaluate if this system should accept this command */
 	bool target_ok = false;
@@ -386,11 +382,6 @@ MavlinkReceiver::evaluate_target_ok(mavlink_message_t *msg, int command, int tar
 		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));
@@ -491,18 +482,12 @@ 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(msg, cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
+	bool target_ok = evaluate_target_ok(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)) {
@@ -576,18 +561,6 @@ 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;
@@ -1861,30 +1834,6 @@ 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)
 {
@@ -2257,12 +2206,6 @@ 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);
@@ -3070,7 +3013,6 @@ 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);
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
alexklimaj	e339297388	Basic gps base station app	2022-09-30 21:41:17 -06:00
alexklimaj	59360b6cab	Start gps base board config	2022-09-30 20:51:58 -06:00