Only command Gripper grab when we are actually initializing gripper

- Previously, on every parameter update, gripper grab was being
commanded
- This commit narrows that scope to only when we are actually
initializing the gripper

.gitmodules

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

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

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

ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil

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

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

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

ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision

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

ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_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

ROMFS/px4fmu_common/init.d/airframes/4071_ifo

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

ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s

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

ROMFS/px4fmu_common/init.d/rcS

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

Tools/module_config/generate_actuators_metadata.py

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

Tools/px_uploader.py

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

Tools/setup/ubuntu.sh

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

Tools/simulation/gazebo/sitl_gazebo

@@ -1 +1 @@
-Subproject commit e80432759540c91f85a012f47aa6ebb0ee9de7e4
+Subproject commit b968405a617005ba0a793a8b4703913d4c6eff5f

Tools/simulation/gz/models/x500-Legacy/model.config

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

Tools/simulation/gz/models/x500-Legacy/model.sdf

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

Tools/simulation/gz/models/x500-NoPlugin/model.config

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

Tools/simulation/gz/models/x500-NoPlugin/model.sdf

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

Tools/simulation/gz/models/x500/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>

boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig

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

boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig

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

boards/px4/fmu-v2/init/rc.board_sensors

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

boards/px4/fmu-v3/init/rc.board_sensors

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

msg/CMakeLists.txt

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

msg/can_frame.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

msg/telemetry_status.msg

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

msg/vehicle_command.msg

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

msg/vehicle_status.msg

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

platforms/qurt/cmake/qurt_reqs.cmake

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

src/drivers/actuators/modalai_esc/modalai_esc.cpp

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

src/drivers/barometer/ms5611/ms5611.cpp

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

src/drivers/cyphal/libcanard

@@ -1 +1 @@
-Subproject commit 2e3b11f6b8325080c160d38521b169b0bbb6b1c7
+Subproject commit db87ea32aa092c48ea103963138b6346dd3e9008

src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp

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

src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp

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

src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp

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

src/drivers/uavcan/uavcan_main.cpp

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

src/drivers/uavcan/uavcan_main.hpp

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

src/drivers/uavcan/uavcan_params.c

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

src/lib/component_information/CMakeLists.txt

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

src/lib/component_information/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)

src/lib/component_information/generate_crc.py

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

src/lib/events/libevents

@@ -1 +1 @@
-Subproject commit a89808bffd1efb0543e66f17a7fa12dfce5e6bf0
+Subproject commit 82dabdb914d7bd640c281900e2852d0afc074b68

src/lib/parameters/px4params/jsonout.py

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

src/lib/parameters/px_generate_params.py

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

src/lib/parameters/px_process_params.py

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

src/modules/commander/Commander.cpp

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

src/modules/commander/Commander.hpp

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

src/modules/commander/commander_helper.cpp

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

src/modules/commander/commander_helper.h

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

src/modules/commander/commander_params.c

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

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

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

src/modules/ekf2/CMakeLists.txt

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

src/modules/ekf2/EKF/airspeed_fusion.cpp

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

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -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"])

src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py

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

src/modules/ekf2/EKF/python/ekf_derivation/generated/.gitignore

@@ -1 +0,0 @@
-*.bak

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h

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

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h

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

src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp

@@ -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;
+}

src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp

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

src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_hk_generated.cpp

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

src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_var_generated.cpp

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

src/modules/ekf2/test/CMakeLists.txt

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

src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp

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

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -353,14 +353,13 @@ void FixedwingAttitudeControl::Run()
 
 		const matrix::Eulerf euler_angles(_R);
 
-		vehicle_manual_poll(euler_angles.psi());
-
 		vehicle_attitude_setpoint_poll();
 
 		// vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition
 		_vehicle_status_sub.update(&_vehicle_status);
 
 		vehicle_control_mode_poll();
+		vehicle_manual_poll(euler_angles.psi());
 		vehicle_land_detected_poll();
 
 		// the position controller will not emit attitude setpoints in some modes
@@ -609,7 +608,7 @@ void FixedwingAttitudeControl::Run()
 
 				/* throttle passed through if it is finite */
 				_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
-					(PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f;
+					(PX4_ISFINITE(_att_sp.thrust_body[0])) ? _att_sp.thrust_body[0] : 0.0f;
 
 				/* scale effort by battery status */
 				if (_param_fw_bat_scale_en.get() &&

src/modules/load_mon/LoadMon.cpp

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

src/modules/load_mon/LoadMon.hpp

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

src/modules/logger/CMakeLists.txt

@@ -48,5 +48,4 @@ px4_add_module(
 		watchdog.cpp
 	DEPENDS
 		version
-		component_general_json # for checksums.h
 	)

src/modules/logger/logger.cpp

@@ -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 = {};

src/modules/logger/logger.h

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

src/modules/logger/messages.h

@@ -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
 };
 
 /**

src/modules/mavlink/mavlink

@@ -1 +1 @@
-Subproject commit dda5a18ddb002a871ba301bb584893ee6378e2f3
+Subproject commit c46af523263ff0dad59afe018fe387c1df030442

src/modules/mavlink/mavlink_main.cpp

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

src/modules/mavlink/mavlink_messages.cpp

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

src/modules/mavlink/mavlink_receiver.cpp

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

src/modules/mavlink/mavlink_receiver.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -67,7 +67,6 @@
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/camera_status.h>
-#include <uORB/topics/can_frame.h>
 #include <uORB/topics/cellular_status.h>
 #include <uORB/topics/collision_report.h>
 #include <uORB/topics/differential_pressure.h>
@@ -161,7 +160,6 @@ private:
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_att_pos_mocap(mavlink_message_t *msg);
 	void handle_message_battery_status(mavlink_message_t *msg);
-	void handle_message_can_frame(mavlink_message_t *msg);
 	void handle_message_cellular_status(mavlink_message_t *msg);
 	void handle_message_collision(mavlink_message_t *msg);
 	void handle_message_command_ack(mavlink_message_t *msg);
@@ -230,7 +228,7 @@ private:
 	int set_message_interval(int msgId, float interval, int data_rate = -1);
 	void get_message_interval(int msgId);
 
-	bool evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component);
+	bool evaluate_target_ok(int command, int target_system, int target_component);
 
 	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
 
@@ -296,7 +294,6 @@ private:
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
-	uORB::Publication<can_frame_s>                          _can_frame_pub{ORB_ID(can_frame_in)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
@@ -389,7 +386,6 @@ private:
 	hrt_abstime _heartbeat_type_adsb{0};
 	hrt_abstime _heartbeat_type_camera{0};
 	hrt_abstime _heartbeat_type_parachute{0};
-	hrt_abstime _heartbeat_type_open_drone_id{0};
 
 	hrt_abstime _heartbeat_component_telemetry_radio{0};
 	hrt_abstime _heartbeat_component_log{0};

src/modules/mavlink/streams/CAN_FRAME.hpp

@@ -1,83 +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.
- *
- ****************************************************************************/
-
-#ifndef CAN_FRAME_HPP
-#define CAN_FRAME_HPP
-
-#include <uORB/topics/can_frame.h>
-
-class MavlinkStreamCanFrame : public MavlinkStream
-{
-public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCanFrame(mavlink); }
-
-	static constexpr const char *get_name_static() { return "CAN_FRAME"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CAN_FRAME; }
-
-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
-
-	unsigned get_size() override
-	{
-		return _can_frame_out_sub.advertised() ? MAVLINK_MSG_ID_CAN_FRAME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-	}
-
-private:
-	explicit MavlinkStreamCanFrame(Mavlink *mavlink) : MavlinkStream(mavlink) {}
-
-	uORB::Subscription _can_frame_out_sub{ORB_ID(can_frame_out)};
-
-	bool send() override
-	{
-		can_frame_s can_frame;
-
-		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;
-	}
-};
-
-#endif // CAN_FRAME_HPP

src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp

@@ -1,185 +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.
- *
- ****************************************************************************/
-
-#ifndef OPEN_DRONE_ID_BASIC_ID_HPP
-#define OPEN_DRONE_ID_BASIC_ID_HPP
-
-#include <uORB/topics/vehicle_status.h>
-
-class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream
-{
-public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdBasicId(mavlink); }
-
-	static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_BASIC_ID"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; }
-
-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
-
-	unsigned get_size() override
-	{
-		return _vehicle_status_sub.advertised() ? MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-	}
-
-private:
-	explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) {}
-
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
-	MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type)
-	{
-		switch (system_type) {
-		case MAV_TYPE_GENERIC: return MAV_ODID_UA_TYPE_OTHER;
-
-		case MAV_TYPE_FIXED_WING: return MAV_ODID_UA_TYPE_AEROPLANE;
-
-		case MAV_TYPE_QUADROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
-
-		case MAV_TYPE_COAXIAL: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
-
-		case MAV_TYPE_HELICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
-
-		case MAV_TYPE_ANTENNA_TRACKER: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_GCS: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_AIRSHIP: return MAV_ODID_UA_TYPE_AIRSHIP;
-
-		case MAV_TYPE_FREE_BALLOON: return MAV_ODID_UA_TYPE_FREE_BALLOON;
-
-		case MAV_TYPE_ROCKET: return MAV_ODID_UA_TYPE_ROCKET;
-
-		case MAV_TYPE_GROUND_ROVER: return MAV_ODID_UA_TYPE_GROUND_OBSTACLE;
-
-		case MAV_TYPE_SURFACE_BOAT: return MAV_ODID_UA_TYPE_OTHER;
-
-		case MAV_TYPE_SUBMARINE: return MAV_ODID_UA_TYPE_OTHER;
-
-		case MAV_TYPE_HEXAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
-
-		case MAV_TYPE_OCTOROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
-
-		case MAV_TYPE_TRICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
-
-		case MAV_TYPE_FLAPPING_WING: return MAV_ODID_UA_TYPE_ORNITHOPTER;
-
-		case MAV_TYPE_KITE: return MAV_ODID_UA_TYPE_KITE;
-
-		case MAV_TYPE_ONBOARD_CONTROLLER: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
-
-		case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
-
-		case MAV_TYPE_VTOL_TILTROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
-
-		case MAV_TYPE_VTOL_FIXEDROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
-
-		case MAV_TYPE_VTOL_TAILSITTER: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
-
-		case MAV_TYPE_VTOL_RESERVED4: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
-
-		case MAV_TYPE_VTOL_RESERVED5: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
-
-		case MAV_TYPE_GIMBAL: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_ADSB: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_PARAFOIL: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
-
-		case MAV_TYPE_DODECAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
-
-		case MAV_TYPE_CAMERA: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_CHARGING_STATION: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_FLARM: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_SERVO: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_ODID: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_DECAROTOR: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_BATTERY: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_PARACHUTE: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
-
-		case MAV_TYPE_LOG: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_OSD: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_IMU: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_GPS: return MAV_ODID_UA_TYPE_NONE;
-
-		case MAV_TYPE_WINCH: return MAV_ODID_UA_TYPE_NONE;
-
-		default: return MAV_ODID_UA_TYPE_OTHER;
-		}
-	}
-
-
-	bool send() override
-	{
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.update(&vehicle_status)) {
-
-			mavlink_open_drone_id_basic_id_t msg{};
-
-			msg.target_system = 0; // 0 for broadcast
-			msg.target_component = 0; // 0 for broadcast
-			// msg.id_or_mac // Only used for drone ID data received from other UAs.
-
-			// id_type: MAV_ODID_ID_TYPE
-			msg.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER;
-
-			// ua_type: MAV_ODID_UA_TYPE
-			msg.ua_type = odidTypeForMavType(vehicle_status.system_type);
-
-			// uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type
-			// TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format
-			board_get_px4_guid_formated((char *)(msg.uas_id), sizeof(msg.uas_id));
-
-			mavlink_msg_open_drone_id_basic_id_send_struct(_mavlink->get_channel(), &msg);
-
-			return true;
-		}
-
-		return false;
-	}
-};
-
-#endif // OPEN_DRONE_ID_BASIC_ID_HPP

src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp

@@ -1,273 +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.
- *
- ****************************************************************************/
-
-#ifndef OPEN_DRONE_ID_LOCATION
-#define OPEN_DRONE_ID_LOCATION
-
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/vehicle_air_data.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_status.h>
-
-class MavlinkStreamOpenDroneIdLocation : public MavlinkStream
-{
-public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdLocation(mavlink); }
-
-	static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_LOCATION"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; }
-
-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
-
-	unsigned get_size() override
-	{
-		return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-	}
-
-private:
-	explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) {}
-
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
-	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
-	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
-	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
-	bool send() override
-	{
-		mavlink_open_drone_id_location_t msg{};
-		msg.target_component = 0; // 0 for broadcast
-		msg.target_system = 0; // 0 for broadcast
-		// msg.id_or_mac // Only used for drone ID data received from other UAs.
-
-		// initialize all fields to unknown
-		msg.status = MAV_ODID_STATUS_UNDECLARED;
-		msg.direction = 36100; // If unknown: 36100 centi-degrees
-		msg.speed_horizontal = 25500; // If unknown: 25500 cm/s
-		msg.speed_vertical = 6300; // If unknown: 6300 cm/s
-		msg.latitude = 0; // If unknown: 0
-		msg.longitude = 0; // If unknown: 0
-		msg.altitude_geodetic = -1000; // If unknown: -1000 m
-		msg.altitude_geodetic = -1000; // If unknown: -1000 m
-		msg.height = -1000; // If unknown: -1000 m
-		msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
-		msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
-		msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
-		msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
-		msg.timestamp = 0xFFFF; // If unknown: 0xFFFF
-		msg.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN;
-
-		bool updated = false;
-
-		// status: MAV_ODID_STATUS_GROUND/MAV_ODID_STATUS_AIRBORNE
-		if (_vehicle_land_detected_sub.advertised()) {
-			vehicle_land_detected_s vehicle_land_detected{};
-
-			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
-			    && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 10_s)) {
-				if (vehicle_land_detected.landed) {
-					msg.status = MAV_ODID_STATUS_GROUND;
-
-				} else {
-					msg.status = MAV_ODID_STATUS_AIRBORNE;
-				}
-
-				updated = true;
-			}
-		}
-
-		// status: MAV_ODID_STATUS_EMERGENCY
-		if (_vehicle_status_sub.advertised()) {
-			vehicle_status_s vehicle_status{};
-
-			if (_vehicle_status_sub.copy(&vehicle_status) && hrt_elapsed_time(&vehicle_status.timestamp) < 10_s) {
-				if (vehicle_status.failsafe && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
-					msg.status = MAV_ODID_STATUS_EMERGENCY;
-					updated = true;
-				}
-			}
-		}
-
-		if (_vehicle_gps_position_sub.advertised()) {
-			sensor_gps_s vehicle_gps_position{};
-
-			if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)
-			    && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 10_s)) {
-
-				if (vehicle_gps_position.vel_ned_valid) {
-					const matrix::Vector3f vel_ned{vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s};
-
-					// direction: calculate GPS course over ground angle
-					const float course = atan2f(vel_ned(1), vel_ned(0));
-					const int course_deg = roundf(math::degrees(matrix::wrap_2pi(course)));
-					msg.direction = math::constrain(100 * course_deg, 0, 35999); // 0 - 35999 centi-degrees
-
-					// speed_horizontal: If speed is larger than 25425 cm/s, use 25425 cm/s.
-					const int speed_horizontal_cm_s = matrix::Vector2f(vel_ned).length() * 100.f;
-					msg.speed_horizontal = math::constrain(speed_horizontal_cm_s, 0, 25425);
-
-					// speed_vertical: Up is positive, If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.
-					const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f);
-					msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200);
-
-					// speed_accuracy
-					if (vehicle_gps_position.s_variance_m_s < 0.3f) {
-						msg.speed_accuracy = MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND;
-
-					} else if (vehicle_gps_position.s_variance_m_s < 1.f) {
-						msg.speed_accuracy = MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND;
-
-					} else if (vehicle_gps_position.s_variance_m_s < 3.f) {
-						msg.speed_accuracy = MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND;
-
-					} else if (vehicle_gps_position.s_variance_m_s < 10.f) {
-						msg.speed_accuracy = MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND;
-
-					} else {
-						msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
-					}
-
-					updated = true;
-				}
-
-				if (vehicle_gps_position.fix_type >= 2) {
-					msg.latitude = vehicle_gps_position.lat;
-					msg.longitude = vehicle_gps_position.lon;
-
-					// altitude_geodetic
-					if (vehicle_gps_position.fix_type >= 3) {
-						msg.altitude_geodetic = vehicle_gps_position.alt * 1e-3f;
-					}
-
-					// horizontal_accuracy
-					if (vehicle_gps_position.eph < 1.f) {
-						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_1_METER;
-
-					} else if (vehicle_gps_position.eph < 3.f) {
-						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_3_METER;
-
-					} else if (vehicle_gps_position.eph < 10.f) {
-						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_10_METER;
-
-					} else if (vehicle_gps_position.eph < 30.f) {
-						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_30_METER;
-
-					} else {
-						msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
-					}
-
-					// vertical_accuracy
-					if (vehicle_gps_position.epv < 1.f) {
-						msg.vertical_accuracy = MAV_ODID_VER_ACC_1_METER;
-
-					} else if (vehicle_gps_position.epv < 3.f) {
-						msg.vertical_accuracy = MAV_ODID_VER_ACC_3_METER;
-
-					} else if (vehicle_gps_position.epv < 10.f) {
-						msg.vertical_accuracy = MAV_ODID_VER_ACC_10_METER;
-
-					} else if (vehicle_gps_position.epv < 25.f) {
-						msg.vertical_accuracy = MAV_ODID_VER_ACC_25_METER;
-
-					} else if (vehicle_gps_position.epv < 45.f) {
-						msg.vertical_accuracy = MAV_ODID_VER_ACC_45_METER;
-
-					} else if (vehicle_gps_position.epv < 150.f) {
-						msg.vertical_accuracy = MAV_ODID_VER_ACC_150_METER;
-
-					} else {
-						msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
-					}
-
-					updated = true;
-				}
-
-				if (vehicle_gps_position.time_utc_usec != 0) {
-					// timestamp: UTC then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000
-					uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000;
-					msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000;
-
-					if (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s) {
-						msg.timestamp_accuracy = MAV_ODID_TIME_ACC_1_0_SECOND; // TODO
-					}
-
-					updated = true;
-				}
-			}
-		}
-
-		// altitude_barometric: The altitude calculated from the barometric pressue
-		if (_vehicle_air_data_sub.advertised()) {
-			vehicle_air_data_s vehicle_air_data{};
-
-			if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) {
-				msg.altitude_barometric = vehicle_air_data.baro_alt_meter;
-				msg.barometer_accuracy = MAV_ODID_VER_ACC_150_METER; // TODO
-				updated = true;
-			}
-		}
-
-		// height: The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference
-		if (_home_position_sub.advertised() && _vehicle_local_position_sub.updated()) {
-			home_position_s home_position{};
-			vehicle_local_position_s vehicle_local_position{};
-
-			if (_home_position_sub.copy(&home_position)
-			    && _vehicle_local_position_sub.copy(&vehicle_local_position)
-			    && (hrt_elapsed_time(&vehicle_local_position.timestamp) < 1_s)
-			   ) {
-
-				if (home_position.valid_alt && vehicle_local_position.z_valid && vehicle_local_position.z_global) {
-					float altitude = (-vehicle_local_position.z + vehicle_local_position.ref_alt);
-
-					msg.height = altitude - home_position.alt;
-					msg.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF;
-					updated = true;
-				}
-			}
-		}
-
-		if (updated) {
-			mavlink_msg_open_drone_id_location_send_struct(_mavlink->get_channel(), &msg);
-			return true;
-		}
-
-		return false;
-	}
-};
-
-#endif // OPEN_DRONE_ID_LOCATION

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -36,7 +36,6 @@
 
 #include <qurt.h>
 #include <qurt_thread.h>
-#include <pthread.h>
 
 // TODO: Move this out of here once we have px4-log functionality
 extern "C" void HAP_debug(const char *msg, int level, const char *filename, int line);
@@ -46,7 +45,7 @@ static MUORBTestType test_to_run;
 
 fc_func_ptrs muorb_func_ptrs;
 
-static void *test_runner(void *test)
+static void test_runner(void *test)
 {
 	HAP_debug("test_runner called", 1, muorb_test_topic_name, 0);
 
@@ -75,7 +74,7 @@ static void *test_runner(void *test)
 		break;
 	}
 
-	return nullptr;
+	qurt_thread_exit(0);
 }
 
 int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
@@ -94,13 +93,14 @@ char stack[TEST_STACK_SIZE];
 
 void run_test(MUORBTestType test)
 {
-	pthread_t tid;
-	pthread_attr_t attr;
-	pthread_attr_init(&attr);
-	pthread_attr_setstacksize(&attr, TEST_STACK_SIZE);
+	qurt_thread_t tid;
+	qurt_thread_attr_t attr;
+
+	qurt_thread_attr_init(&attr);
+	qurt_thread_attr_set_stack_addr(&attr, stack);
+	qurt_thread_attr_set_stack_size(&attr, TEST_STACK_SIZE);
 	test_to_run = test;
-	pthread_create(&tid, &attr, &test_runner, (void *) &test_to_run);
-	pthread_attr_destroy(&attr);
+	(void) qurt_thread_create(&tid, &attr, &test_runner, (void *) &test_to_run);
 }
 
 int px4muorb_topic_advertised(const char *topic_name)

src/modules/navigator/vtol_takeoff.cpp

@@ -149,7 +149,7 @@ VtolTakeoff::on_active()
 				_navigator->reset_position_setpoint(reposition_triplet->current);
 				_navigator->reset_position_setpoint(reposition_triplet->next);
 
-				// the VTOL takeoff is done, proceed loitering and update the navigation state to LOITER
+				// the VTOL takeoff is done, proceed loitering and upate the navigation state to LOITER
 				_navigator->get_mission_result()->finished = true;
 				_navigator->set_mission_result_updated();
 

src/modules/payload_deliverer/gripper.cpp

@@ -68,6 +68,10 @@ void Gripper::init(const GripperConfig &config)
 
 void Gripper::grab()
 {
+	if (_state == GripperState::GRABBING || _state == GripperState::GRABBED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_GRAB);
 	_state = GripperState::GRABBING;
 	_last_command_time = hrt_absolute_time();
@@ -75,6 +79,10 @@ void Gripper::grab()
 
 void Gripper::release()
 {
+	if (_state == GripperState::RELEASING || _state == GripperState::RELEASED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_RELEASE);
 	_state = GripperState::RELEASING;
 	_last_command_time = hrt_absolute_time();
@@ -89,11 +97,13 @@ void Gripper::update()
 	case GripperState::GRABBING:
 		if (_has_feedback_sensor) {
 			// Handle feedback sensor input, return true for now (not supported)
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 			break;
 		}
 
 		if (command_timed_out) {
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 		}